Added autopilot target values to communications link.

Pass autopilot target values to LFSGlass display so they can be properly
displayed.
Fix a gps status display bug.

diff --git a/utils/GPSsmooth/UGear.hxx b/utils/GPSsmooth/UGear.hxx
index 8e69bf0f8867c9ead084eb09a7348b4b4401a593..818d29bb81c70eaebf5fff772e41ff27b42a9983 100644 (file)
--- a/utils/GPSsmooth/UGear.hxx
+++ b/utils/GPSsmooth/UGear.hxx
@@ -46,7 +46,7 @@ struct gps {
    double lat,lon,alt;          /* gps position          */
    double ve,vn,vd;             /* gps velocity          */
    double ITOW;
-   uint64_t err_type;             /* error type            */
+   uint64_t err_type;           /* error type            */
 };
 
 struct nav {
@@ -65,7 +65,13 @@ struct servo {
 
 struct health {
     double time;
+    double target_roll_deg;     /* AP target roll angle */
+    double target_heading_deg;  /* AP target heading angle */
+    double target_pitch_deg;    /* AP target pitch angle */
+    double target_climb_fps;    /* AP target climb rate */
+    double target_altitude_ft;  /* AP target altitude */
     uint64_t command_sequence;  /* highest received command sequence num */
+    uint64_t target_waypoint;   /* index of current waypoint target */
     uint64_t loadavg;           /* system "1 minute" load average */
     uint64_t ahrs_hz;           /* actual ahrs loop hz */
     uint64_t nav_hz;            /* actual nav loop hz */

diff --git a/utils/GPSsmooth/UGear_main.cxx b/utils/GPSsmooth/UGear_main.cxx
index 9065c69ccde75950186d437f454b8b8605cc78d5..afdd94081eb8d7623066de03607f4aaaf6542951 100644 (file)
--- a/utils/GPSsmooth/UGear_main.cxx
+++ b/utils/GPSsmooth/UGear_main.cxx
@@ -473,16 +473,18 @@ static void ugear2opengc( gps *gpspacket, imu *imupacket, nav *navpacket,
         ogc->right_aileron = ((double)servopacket->chn[0] / 32768.0) - 1.0;
         ogc->rudder = 1.0 - ((double)servopacket->chn[3] / 32768.0);
     }
+    ogc->elevator *= 4.0;
+    ogc->left_aileron *= 4.0;
+    ogc->right_aileron *= 4.0;
+    ogc->rudder *= 4.0;
 
     // additional "abused" data fields
-    double fd_bank = 0.0;
-    double fd_pitch = 0.0;
-    ogc->egt[0] = ogc->bank + fd_bank;        // flight director target roll
-    ogc->egt[1] = -ogc->pitch + fd_pitch; // flight director target pitch
-    ogc->egt[2] = ogc->heading + 15; // target heading bug
-    ogc->egt[3] = -ogc->vvi;         // target VVI bug
-    ogc->epr[0] = 1400 /*ogc->elevation - 50*/ ; // target altitude bug
-    ogc->epr[1] = ogc->v_kcas + 5;   // target speed bug
+    ogc->egt[0] = ogc->bank - healthpacket->target_roll_deg; // flight director target roll
+    ogc->egt[1] = -ogc->pitch + healthpacket->target_pitch_deg; // flight director target pitch
+    ogc->egt[2] = healthpacket->target_heading_deg; // target heading bug
+    ogc->egt[3] = healthpacket->target_climb_fps;   // target VVI bug
+    ogc->epr[0] = healthpacket->target_altitude_ft; // target altitude bug
+    ogc->epr[1] = 30.0;                              // target speed bug
     ogc->epr[2] = gps_status;   // gps status box
 }
 
@@ -1025,9 +1027,10 @@ int main( int argc, char **argv ) {
         UGCommand command;
 
         // add some test commands
-        command.add("apalt,1000");
-        command.add("home,158.0,32.5");
-        command.add("gohome");
+        //command.add("ap,alt,1000");
+        //command.add("home,158.0,32.5");
+        //command.add("go,home");
+        //command.add("go,route");
 
         while ( uavcom.is_enabled() ) {
            // cout << "looking for next message ..." << endl;
@@ -1084,9 +1087,9 @@ int main( int argc, char **argv ) {
                   fabs(gpspacket.alt) < 0.0001) )
             {
                 printf("WARNING: LOST GPS!!!\n");
-                gps_status = 1.0;
-            } else {
                 gps_status = -1.0;
+            } else {
+                gps_status = 1.0;
             }
 
             // Generate a ground station heart beat every 5 seconds