Fix a couple data packet decoding mistakes.

diff --git a/utils/GPSsmooth/UGear.cxx b/utils/GPSsmooth/UGear.cxx
index c707986da7ccd5cc791ec18268bd986c729a12d4..e1fe7c3af8f72211a905f32f8686e57a9f31f24f 100644 (file)
--- a/utils/GPSsmooth/UGear.cxx
+++ b/utils/GPSsmooth/UGear.cxx
@@ -94,7 +94,7 @@ void UGEARTrack::parse_msg( const int id, char *buf,
       gpspacket->vn   = sg_swap_double( (uint8_t *)buf, 24 );
       gpspacket->ve   = sg_swap_double( (uint8_t *)buf, 32 );
       gpspacket->vd   = sg_swap_double( (uint8_t *)buf, 40 );
-      gpspacket->time = sg_swap_double( (uint8_t *)buf, 56 );
+      gpspacket->time = sg_swap_double( (uint8_t *)buf, 52 );
     } else if ( id == IMU_PACKET ) {
       *imupacket = *(struct imu *)buf;
       imupacket->p    = sg_swap_double( (uint8_t *)buf, 0 );
@@ -128,7 +128,7 @@ void UGEARTrack::parse_msg( const int id, char *buf,
       // 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 );
+      healthpacket->time = sg_swap_double( (uint8_t *)buf, 16 );
     } else {
         cout << "unknown id = " << id << endl;
     }
@@ -175,7 +175,7 @@ bool UGEARTrack::load( const string &file ) {
                 gps_data.push_back( gpspacket );
                 gps_time = gpspacket.time;
             } else {
-                cout << "oops gps back in time" << endl;
+             cout << "oops gps back in time: " << gpspacket.time << " " << gps_time << endl;
             }
         } else if ( id == IMU_PACKET ) {
             if ( imupacket.time > imu_time ) {

diff --git a/utils/GPSsmooth/UGear_main.cxx b/utils/GPSsmooth/UGear_main.cxx
index b8062fe5fa4eb0bc6fdc8b8c218ff671d3c8ac7b..54599f2b5213abb613ef96d6356cac78cf41c178 100644 (file)
--- a/utils/GPSsmooth/UGear_main.cxx
+++ b/utils/GPSsmooth/UGear_main.cxx
@@ -707,7 +707,7 @@ int main( int argc, char **argv ) {
                     gps_time = gpspacket.time;
                     current_time = gps_time;
                 } else {
-                    cout << "oops gps back in time" << endl;
+                 cout << "oops gps back in time: " << gpspacket.time << " " << gps_time << endl;
                 }
            } else if ( id == IMU_PACKET ) {
                 if ( imupacket.time > imu_time ) {