Updates to parse health packets.

utils/GPSsmooth/UGear.cxx

@@ -83,7 +83,8 @@ static bool validate_cksum( uint8_t id, uint8_t size, char *buf,
 
 void UGEARTrack::parse_msg( const int id, char *buf,
 			    struct gps *gpspacket, imu *imupacket,
-			    nav *navpacket, servo *servopacket )
+			    nav *navpacket, servo *servopacket,
+			    health *healthpacket )
 {
     if ( id == GPS_PACKET ) {
       *gpspacket = *(struct gps *)buf;
@@ -125,6 +126,9 @@ void UGEARTrack::parse_msg( const int id, char *buf,
       *servopacket = *(struct servo *)buf;
       servopacket->time = sg_swap_double( (uint8_t *)buf, 20 );
       // printf("servo time = %.3f\n", servopacket->time);
+    } else if ( id == HEALTH_PACKET ) {
+      *healthpacket = *(struct health *)buf;
+      healthpacket->time = sg_swap_double( (uint8_t *)buf, 12 );
     } else {
         cout << "unknown id = " << id << endl;
     }
@@ -139,16 +143,19 @@ bool UGEARTrack::load( const string &file ) {
     imu imupacket;
     nav navpacket;
     servo servopacket;
+    health healthpacket;
 
     double gps_time = 0;
     double imu_time = 0;
     double nav_time = 0;
     double servo_time = 0;
+    double health_time = 0;
 
     gps_data.clear();
     imu_data.clear();
     nav_data.clear();
     servo_data.clear();
+    health_data.clear();
 
     // open the file
     SGFile input( file );
@@ -160,7 +167,7 @@ bool UGEARTrack::load( const string &file ) {
     while ( ! input.eof() ) {
         // cout << "looking for next message ..." << endl;
         int id = next_message( &input, NULL, &gpspacket, &imupacket,
-			       &navpacket, &servopacket );
+			       &navpacket, &servopacket, &healthpacket );
         count++;
 
         if ( id == GPS_PACKET ) {
@@ -168,28 +175,35 @@ bool UGEARTrack::load( const string &file ) {
                 gps_data.push_back( gpspacket );
                 gps_time = gpspacket.time;
             } else {
-                cout << "oops att back in time" << endl;
+                cout << "oops gps back in time" << endl;
             }
         } else if ( id == IMU_PACKET ) {
             if ( imupacket.time > imu_time ) {
                 imu_data.push_back( imupacket );
                 imu_time = imupacket.time;
             } else {
-                cout << "oops pos back in time" << endl;
+                cout << "oops imu back in time" << endl;
             }
         } else if ( id == NAV_PACKET ) {
             if ( navpacket.time > nav_time ) {
                 nav_data.push_back( navpacket );
                 nav_time = navpacket.time;
             } else {
-                cout << "oops pos back in time" << endl;
+                cout << "oops nav back in time" << endl;
             }
         } else if ( id == SERVO_PACKET ) {
             if ( servopacket.time > servo_time ) {
                 servo_data.push_back( servopacket );
                 servo_time = servopacket.time;
             } else {
-                cout << "oops pos back in time" << endl;
+                cout << "oops servo back in time" << endl;
+            }
+        } else if ( id == HEALTH_PACKET ) {
+            if ( healthpacket.time > health_time ) {
+                health_data.push_back( healthpacket );
+                health_time = healthpacket.time;
+            } else {
+                cout << "oops health back in time" << endl;
             }
         }
     }
@@ -244,7 +258,7 @@ int serial_read( SGSerialPort *serial, SGIOChannel *log,
 // load the next message of a real time data stream
 int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
 			      gps *gpspacket, imu *imupacket, nav *navpacket,
-			      servo *servopacket )
+			      servo *servopacket, health *healthpacket )
 {
     char tmpbuf[256];
     char savebuf[256];
@@ -296,7 +310,8 @@ int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
     myread( ch, log, tmpbuf, 1 ); uint8_t cksum1 = (unsigned char)tmpbuf[0];
     
     if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
-        parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket );
+        parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket,
+		   healthpacket );
         return id;
     }
 
@@ -308,7 +323,7 @@ int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
 // load the next message of a real time data stream
 int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
 			      gps *gpspacket, imu *imupacket, nav *navpacket,
-			      servo *servopacket )
+			      servo *servopacket, health *healthpacket )
 {
     char tmpbuf[256];
     char savebuf[256];
@@ -350,7 +365,8 @@ int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
     //      << endl;
     
     if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
-        parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket );
+        parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket,
+		   healthpacket );
 
         return id;
     }
@@ -442,3 +458,15 @@ servo UGEARInterpSERVO( const servo A, const servo B, const double percent )
     return p;
 }
 
+
+health UGEARInterpHEALTH( const health A, const health B, const double percent )
+{
+    health p;
+    p.volts_raw = interp(A.volts_raw, B.volts_raw, percent);
+    p.volts = interp(A.volts, B.volts, percent);
+    p.est_seconds = (uint16_t)interp(A.est_seconds, B.est_seconds, percent);
+    p.time = interp(A.time, B.time, percent);
+
+    return p;
+}
+

utils/GPSsmooth/UGear.hxx

@@ -26,7 +26,8 @@ enum ugPacketType {
     GPS_PACKET = 0,
     IMU_PACKET = 1,
     NAV_PACKET = 2,
-    SERVO_PACKET = 3
+    SERVO_PACKET = 3,
+    HEALTH_PACKET = 4
 };
 
 struct imu {
@@ -62,6 +63,12 @@ struct servo {
    double time;
 };
 
+struct health {
+    float volts_raw;            /* raw volt reading */
+    float volts;                /* filtered volts */
+    uint16_t est_seconds;       /* estimated useful seconds remaining */
+    double time;
+};
 
 // Manage a saved ugear log (track file)
 class UGEARTrack {
@@ -72,12 +79,13 @@ private:
     vector <imu> imu_data;
     vector <nav> nav_data;
     vector <servo> servo_data;
+    vector <health> health_data;
 
     // parse message and put current data into vector if message has a
     // newer time stamp than existing data.
     void parse_msg( const int id, char *buf,
 		    gps *gpspacket, imu *imupacket, nav *navpacket,
-		    servo *servopacket );
+		    servo *servopacket, health *healthpacket );
 
 public:
 
@@ -88,10 +96,10 @@ public:
     // returns id # if a valid message found.
     int next_message( SGIOChannel *ch, SGIOChannel *log,
                       gps *gpspacket, imu *imupacket, nav *navpacket,
-		      servo *servopacket );
+		      servo *servopacket, health * healthpacket );
     int next_message( SGSerialPort *serial, SGIOChannel *log,
                       gps *gpspacket, imu *imupacket, nav *navpacket,
-		      servo *servopacket );
+		      servo *servopacket, health *healthpacket );
 
     // load the named file into internal buffers
     bool load( const string &file );
@@ -100,6 +108,7 @@ public:
     inline int imu_size() const { return imu_data.size(); }
     inline int nav_size() const { return nav_data.size(); }
     inline int servo_size() const { return servo_data.size(); }
+    inline int health_size() const { return health_data.size(); }
 
     inline gps get_gpspt( const unsigned int i )
     {
@@ -133,6 +142,14 @@ public:
             return servo();
         }
     }
+    inline health get_healthpt( const unsigned int i )
+    {
+        if ( i < health_data.size() ) {
+            return health_data[i];
+        } else {
+            return health();
+        }
+    }
        
 
 };
@@ -142,6 +159,8 @@ gps UGEARInterpGPS( const gps A, const gps B, const double percent );
 imu UGEARInterpIMU( const imu A, const imu B, const double percent );
 nav UGEARInterpNAV( const nav A, const nav B, const double percent );
 servo UGEARInterpSERVO( const servo A, const servo B, const double percent );
+health UGEARInterpHEALTH( const health A, const health B,
+			  const double percent );
 
 
 #endif // _FG_UGEAR_II_HXX

utils/GPSsmooth/UGear_main.cxx

@@ -105,7 +105,7 @@ static void htonf (float &x)
 
 
 static void ugear2fg( gps *gpspacket, imu *imupacket, nav *navpacket,
-		      servo *servopacket,
+		      servo *servopacket, health *healthpacket,
 		      FGNetFDM *fdm, FGNetCtrls *ctrls )
 {
     unsigned int i;
@@ -278,7 +278,7 @@ static void ugear2fg( gps *gpspacket, imu *imupacket, nav *navpacket,
 
 
 static void send_data( gps *gpspacket, imu *imupacket, nav *navpacket,
-		       servo *servopacket ) {
+		       servo *servopacket, health *healthpacket ) {
     int len;
     int fdmsize = sizeof( FGNetFDM );
 
@@ -287,7 +287,8 @@ static void send_data( gps *gpspacket, imu *imupacket, nav *navpacket,
     FGNetFDM fgfdm;
     FGNetCtrls fgctrls;
 
-    ugear2fg( gpspacket, imupacket, navpacket, servopacket, &fgfdm, &fgctrls );
+    ugear2fg( gpspacket, imupacket, navpacket, servopacket, healthpacket,
+	      &fgfdm, &fgctrls );
     len = fdm_sock.send(&fgfdm, fdmsize, 0);
 }
 
@@ -444,6 +445,7 @@ int main( int argc, char **argv ) {
         cout << "Loaded " << track.imu_size() << " imu records." << endl;
         cout << "Loaded " << track.nav_size() << " nav records." << endl;
         cout << "Loaded " << track.servo_size() << " servo records." << endl;
+        cout << "Loaded " << track.health_size() << " health records." << endl;
 
         int size = track.imu_size();
 
@@ -467,6 +469,7 @@ int main( int argc, char **argv ) {
         int imu_count = 0;
         int nav_count = 0;
         int servo_count = 0;
+        int health_count = 0;
 
         gps gps0, gps1;
         gps0 = gps1 = track.get_gpspt( 0 );
@@ -480,6 +483,9 @@ int main( int argc, char **argv ) {
         servo servo0, servo1;
         servo0 = servo1 = track.get_servopt( 0 );
     
+        health health0, health1;
+        health0 = health1 = track.get_healthpt( 0 );
+    
         while ( current_time < end_time ) {
             // cout << "current_time = " << current_time << " end_time = "
             //      << end_time << endl;
@@ -532,6 +538,18 @@ int main( int argc, char **argv ) {
             //  cout << "pos0 = " << pos0.get_seconds()
             // << " pos1 = " << pos1.get_seconds() << endl;
 
+            // Advance health pointer
+            while ( current_time > health1.time
+                    && health_count < track.health_size() )
+            {
+                health0 = health1;
+                ++health_count;
+                // cout << "count = " << count << endl;
+                health1 = track.get_healthpt( health_count );
+            }
+            //  cout << "pos0 = " << pos0.get_seconds()
+            // << " pos1 = " << pos1.get_seconds() << endl;
+
             double gps_percent;
             if ( fabs(gps1.time - gps0.time) < 0.00001 ) {
                 gps_percent = 0.0;
@@ -572,11 +590,23 @@ int main( int argc, char **argv ) {
             }
             // cout << "Percent = " << percent << endl;
 
+            double health_percent;
+            if ( fabs(health1.time - health0.time) < 0.00001 ) {
+                health_percent = 0.0;
+            } else {
+                health_percent =
+                    (current_time - health0.time) /
+                    (health1.time - health0.time);
+            }
+            // cout << "Percent = " << percent << endl;
+
             gps gpspacket = UGEARInterpGPS( gps0, gps1, gps_percent );
             imu imupacket = UGEARInterpIMU( imu0, imu1, imu_percent );
             nav navpacket = UGEARInterpNAV( nav0, nav1, nav_percent );
             servo servopacket = UGEARInterpSERVO( servo0, servo1,
 						  servo_percent );
+            health healthpacket = UGEARInterpHEALTH( health0, health1,
+						  health_percent );
 
             // cout << current_time << " " << p0.lat_deg << ", " << p0.lon_deg
             //      << endl;
@@ -591,7 +621,8 @@ int main( int argc, char **argv ) {
                     imupacket.psi, imupacket.the, imupacket.phi );
 	    }
 
-            send_data( &gpspacket, &imupacket, &navpacket, &servopacket );
+            send_data( &gpspacket, &imupacket, &navpacket, &servopacket,
+		       &healthpacket );
 
             // Update the elapsed time.
             static bool first_time = true;
@@ -629,11 +660,13 @@ int main( int argc, char **argv ) {
 	imu imupacket;
 	nav navpacket;
 	servo servopacket;
+	health healthpacket;
 
         double gps_time = 0;
         double imu_time = 0;
         double nav_time = 0;
         double servo_time = 0;
+        double health_time = 0;
 
         // open the serial port device
         SGSerialPort input( serialdev, 115200 );
@@ -657,7 +690,8 @@ int main( int argc, char **argv ) {
         while ( input.is_enabled() ) {
             // cout << "looking for next message ..." << endl;
             int id = track.next_message( &input, &output, &gpspacket,
-					 &imupacket, &navpacket, &servopacket );
+					 &imupacket, &navpacket, &servopacket,
+					 &healthpacket );
             cout << "message id = " << id << endl;
             count++;
 
@@ -689,6 +723,13 @@ int main( int argc, char **argv ) {
                 } else {
                     cout << "oops servo back in time" << endl;
                 }
+	    } else if ( id == HEALTH_PACKET ) {
+                if ( healthpacket.time > health_time ) {
+                    health_time = healthpacket.time;
+                    current_time = health_time;
+                } else {
+                    cout << "oops health back in time" << endl;
+                }
             }
 
 	    // if ( gpspacket.lat > -500 ) {
@@ -700,7 +741,8 @@ int main( int argc, char **argv ) {
 		    imupacket.psi*SGD_RADIANS_TO_DEGREES );
 	    // }
 
-            send_data( &gpspacket, &imupacket, &navpacket, &servopacket );
+            send_data( &gpspacket, &imupacket, &navpacket, &servopacket,
+		       &healthpacket );
         }
     }