Fix a bug in file logging (add an ignore checksum option just to humor

myself.)

diff --git a/utils/GPSsmooth/UGear.cxx b/utils/GPSsmooth/UGear.cxx
index e1fe7c3af8f72211a905f32f8686e57a9f31f24f..ae0adc4134e7ed2f7879a045a510f50ec3ac6936 100644 (file)
--- a/utils/GPSsmooth/UGear.cxx
+++ b/utils/GPSsmooth/UGear.cxx
@@ -49,8 +49,13 @@ static double sg_swap_double( uint8_t *buf, size_t offset ) {
 
 
 static bool validate_cksum( uint8_t id, uint8_t size, char *buf,
-                            uint8_t cksum0, uint8_t cksum1 )
+                            uint8_t cksum0, uint8_t cksum1,
+                           bool ignore_checksum )
 {
+    if ( ignore_checksum ) {
+        return true;
+    }
+
     uint8_t c0 = 0;
     uint8_t c1 = 0;
 
@@ -136,7 +141,7 @@ void UGEARTrack::parse_msg( const int id, char *buf,
 
 
 // load the specified file, return the number of records loaded
-bool UGEARTrack::load( const string &file ) {
+bool UGEARTrack::load( const string &file, bool ignore_checksum ) {
     int count = 0;
 
     gps gpspacket;
@@ -167,7 +172,8 @@ 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, &healthpacket );
+                              &navpacket, &servopacket, &healthpacket,
+                              ignore_checksum );
         count++;
 
         if ( id == GPS_PACKET ) {
@@ -244,21 +250,23 @@ int serial_read( SGSerialPort *serial, SGIOChannel *log,
 
     while ( bytes_read < length ) {
       result = serial->read_port( tmp, length - bytes_read );
-      if ( result > 0 && log != NULL ) {
-        log->write( buf, result );
-      }
       bytes_read += result;
       tmp += result;
       // cout << "  read " << bytes_read << " of " << length << endl;
     }
 
+    if ( bytes_read > 0 && log != NULL ) {
+      log->write( buf, bytes_read );
+    }
+
     return bytes_read;
 }
 
 // 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, health *healthpacket )
+                             servo *servopacket, health *healthpacket,
+                             bool ignore_checksum )
 {
     char tmpbuf[256];
     char savebuf[256];
@@ -269,14 +277,15 @@ int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
 
     // scan for sync characters
     uint8_t sync0, sync1;
-    myread( ch, log, tmpbuf, 1 ); sync0 = (unsigned char)tmpbuf[0];
-    myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
+    myread( ch, log, tmpbuf, 2 );
+    sync0 = (unsigned char)tmpbuf[0];
+    sync1 = (unsigned char)tmpbuf[1];
     while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) {
         sync0 = sync1;
         myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
-        // cout << "scanning for start of message "
-       //      << (unsigned int)sync0 << " " << (unsigned int)sync1
-       //      << ", eof = " << ch->eof() << endl;
+        cout << "scanning for start of message "
+            << (unsigned int)sync0 << " " << (unsigned int)sync1
+            << ", eof = " << ch->eof() << endl;
         if ( ch->get_type() == sgFileType ) {
             myeof = ((SGFile *)ch)->eof();
         }
@@ -285,8 +294,9 @@ int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
     cout << "found start of message ..." << endl;
 
     // read message id and size
-    myread( ch, log, tmpbuf, 1 ); uint8_t id = (unsigned char)tmpbuf[0];
-    myread( ch, log, tmpbuf, 1 ); uint8_t size = (unsigned char)tmpbuf[0];
+    myread( ch, log, tmpbuf, 2 );
+    uint8_t id = (unsigned char)tmpbuf[0];
+    uint8_t size = (unsigned char)tmpbuf[1];
     // cout << "message = " << (int)id << " size = " << (int)size << endl;
 
     // load message
@@ -306,10 +316,12 @@ int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
     }
 
     // read checksum
-    myread( ch, log, tmpbuf, 1 ); uint8_t cksum0 = (unsigned char)tmpbuf[0];
-    myread( ch, log, tmpbuf, 1 ); uint8_t cksum1 = (unsigned char)tmpbuf[0];
+    myread( ch, log, tmpbuf, 2 );
+    uint8_t cksum0 = (unsigned char)tmpbuf[0];
+    uint8_t cksum1 = (unsigned char)tmpbuf[1];
     
-    if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
+    if ( validate_cksum( id, size, savebuf, cksum0, cksum1, ignore_checksum ) )
+    {
         parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket,
                   healthpacket );
         return id;
@@ -323,7 +335,8 @@ 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, health *healthpacket )
+                             servo *servopacket, health *healthpacket,
+                             bool ignore_checksum )
 {
     char tmpbuf[256];
     char savebuf[256];
@@ -364,7 +377,8 @@ int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
     // cout << "cksum0 = " << (int)cksum0 << " cksum1 = " << (int)cksum1
     //      << endl;
     
-    if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
+    if ( validate_cksum( id, size, savebuf, cksum0, cksum1, ignore_checksum ) )
+    {
         parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket,
                   healthpacket );
 

diff --git a/utils/GPSsmooth/UGear.hxx b/utils/GPSsmooth/UGear.hxx
index 0cba24636199e3c7e773cf4c26915ec02b7bcedb..5bdba2ef5df86d5be9274ab8030b3c02adcdb949 100644 (file)
--- a/utils/GPSsmooth/UGear.hxx
+++ b/utils/GPSsmooth/UGear.hxx
@@ -99,13 +99,15 @@ public:
     // returns id # if a valid message found.
     int next_message( SGIOChannel *ch, SGIOChannel *log,
                       gps *gpspacket, imu *imupacket, nav *navpacket,
-                     servo *servopacket, health * healthpacket );
+                     servo *servopacket, health * healthpacket,
+                     bool ignore_checksum );
     int next_message( SGSerialPort *serial, SGIOChannel *log,
                       gps *gpspacket, imu *imupacket, nav *navpacket,
-                     servo *servopacket, health *healthpacket );
+                     servo *servopacket, health *healthpacket,
+                     bool ignore_checksum );
 
     // load the named file into internal buffers
-    bool load( const string &file );
+    bool load( const string &file, bool ignore_checksum );
 
     inline int gps_size() const { return gps_data.size(); }
     inline int imu_size() const { return imu_data.size(); }

diff --git a/utils/GPSsmooth/UGear_main.cxx b/utils/GPSsmooth/UGear_main.cxx
index 54599f2b5213abb613ef96d6356cac78cf41c178..a4513560d87b828905139cd441d451708e4c971a 100644 (file)
--- a/utils/GPSsmooth/UGear_main.cxx
+++ b/utils/GPSsmooth/UGear_main.cxx
@@ -66,6 +66,8 @@ double skip = 0.0;
 
 bool inited = false;
 
+bool ignore_checksum = false;
+
 
 // The function htond is defined this way due to the way some
 // processors and OSes treat floating point values.  Some will raise
@@ -313,6 +315,7 @@ void usage( const string &argv0 ) {
     cout << "\t[ --ctrls-port <ctrls output port #> ]" << endl;
     cout << "\t[ --altitude-offset <meters> ]" << endl;
     cout << "\t[ --skip-seconds <seconds> ]" << endl;
+    cout << "\t[ --ignore-checksum ]" << endl;
 }
 
 
@@ -400,6 +403,8 @@ int main( int argc, char **argv ) {
                 usage( argv[0] );
                 exit( -1 );
             }
+       } else if ( strcmp( argv[i], "--ignore-checksum" ) == 0 ) {
+         ignore_checksum = true;
         } else {
             usage( argv[0] );
             exit( -1 );
@@ -447,7 +452,7 @@ int main( int argc, char **argv ) {
 
     if ( infile.length() ) {
         // Load data from a track data
-        track.load( infile );
+        track.load( infile, ignore_checksum );
         cout << "Loaded " << track.gps_size() << " gps records." << endl;
         cout << "Loaded " << track.imu_size() << " imu records." << endl;
         cout << "Loaded " << track.nav_size() << " nav records." << endl;
@@ -698,7 +703,7 @@ int main( int argc, char **argv ) {
            // cout << "looking for next message ..." << endl;
             int id = track.next_message( &input, &output, &gpspacket,
                                         &imupacket, &navpacket, &servopacket,
-                                        &healthpacket );
+                                        &healthpacket, ignore_checksum );
             // cout << "message id = " << id << endl;
             count++;