Add a pass through so we can capture IMU/INS/GPS data to a file at the same

time as using it to drive a copy of FlightGear.

utils/GPSsmooth/MIDG-II.cxx

@@ -344,7 +344,7 @@ bool MIDGTrack::load( const string &file ) {
 
     while ( ! input.eof() ) {
         // cout << "looking for next message ..." << endl;
-        int id = next_message( &input, &pos, &att );
+        int id = next_message( &input, NULL, &pos, &att );
         count++;
 
         if ( id == 10 ) {
@@ -371,7 +371,7 @@ bool MIDGTrack::load( const string &file ) {
 
 // attempt to work around some system dependent issues.  Our read can
 // return < data than we want.
-int myread( SGIOChannel *ch, char *buf, int length ) {
+int myread( SGIOChannel *ch, SGIOChannel *log, char *buf, int length ) {
     bool myeof = false;
     int result = 0;
     while ( result != length && !myeof ) {
@@ -381,11 +381,17 @@ int myread( SGIOChannel *ch, char *buf, int length ) {
         }
     }
 
+    if ( result > 0 && log != NULL ) {
+        log->write( buf, result );
+    }
+
     return result;
 }
 
 // load the next message of a real time data stream
-int MIDGTrack::next_message( SGIOChannel *ch, MIDGpos *pos, MIDGatt *att ) {
+int MIDGTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
+                             MIDGpos *pos, MIDGatt *att )
+{
     char tmpbuf[256];
     char savebuf[256];
 
@@ -395,11 +401,11 @@ int MIDGTrack::next_message( SGIOChannel *ch, MIDGpos *pos, MIDGatt *att ) {
 
     // scan for sync characters
     uint8_t sync0, sync1;
-    myread( ch, tmpbuf, 1 ); sync0 = (unsigned char)tmpbuf[0];
+    myread( ch, log, tmpbuf, 1 ); sync0 = (unsigned char)tmpbuf[0];
-    myread( ch, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
+    myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
     while ( (sync0 != 129 || sync1 != 161) && !myeof ) {
         sync0 = sync1;
-        myread( ch, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
+        myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
         // cout << "scanning for start of message, eof = " << ch->eof() << endl;
         if ( ch->get_type() == sgFileType ) {
             myeof = ((SGFile *)ch)->eof();
@@ -409,25 +415,25 @@ int MIDGTrack::next_message( SGIOChannel *ch, MIDGpos *pos, MIDGatt *att ) {
     // cout << "found start of message ..." << endl;
 
     // read message id and size
-    myread( ch, tmpbuf, 1 ); uint8_t id = (unsigned char)tmpbuf[0];
+    myread( ch, log, tmpbuf, 1 ); uint8_t id = (unsigned char)tmpbuf[0];
-    myread( ch, tmpbuf, 1 ); uint8_t size = (unsigned char)tmpbuf[0];
+    myread( ch, log, tmpbuf, 1 ); uint8_t size = (unsigned char)tmpbuf[0];
     // cout << "message = " << (int)id << " size = " << (int)size << endl;
 
     // load message
     if ( ch->get_type() == sgFileType ) {
-        int count = myread( ch, savebuf, size );
+        int count = myread( ch, log, savebuf, size );
         if ( count != size ) {
             cout << "ERROR: didn't read enough bytes!" << endl;
         }
     } else {
         for ( int i = 0; i < size; ++i ) {
-            myread( ch, tmpbuf, 1 ); savebuf[i] = tmpbuf[0];
+            myread( ch, log, tmpbuf, 1 ); savebuf[i] = tmpbuf[0];
         }
     }
 
     // read checksum
-    myread( ch, tmpbuf, 1 ); uint8_t cksum0 = (unsigned char)tmpbuf[0];
+    myread( ch, log, tmpbuf, 1 ); uint8_t cksum0 = (unsigned char)tmpbuf[0];
-    myread( ch, tmpbuf, 1 ); uint8_t cksum1 = (unsigned char)tmpbuf[0];
+    myread( ch, log, tmpbuf, 1 ); uint8_t cksum1 = (unsigned char)tmpbuf[0];
     
     if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
         parse_msg( id, savebuf, pos, att );

utils/GPSsmooth/MIDG-II.hxx

@@ -141,7 +141,8 @@ public:
 
     // read/parse the next message from the specified data stream,
     // returns id # if a valid message found.
-    int next_message( SGIOChannel *ch, MIDGpos *pos, MIDGatt *att );
+    int next_message( SGIOChannel *ch, SGIOChannel *log,
+                      MIDGpos *pos, MIDGatt *att );
 
     // load the named file into internal buffers
     bool load( const string &file );

utils/GPSsmooth/MIDG_main.cxx

@@ -10,6 +10,7 @@
 
 #include <simgear/constants.h>
 #include <simgear/io/lowlevel.hxx> // endian tests
+#include <simgear/io/sg_file.hxx>
 #include <simgear/io/sg_serial.hxx>
 #include <simgear/math/sg_geodesy.hxx>
 #include <simgear/timing/timestamp.hxx>
@@ -36,8 +37,9 @@ static int fdm_port = 5505;
 static int ctrls_port = 5506;
 
 // Default path
-static string file = "";
+static string infile = "";
 static string serialdev = "";
+static string outfile = "";
 
 // Master time counter
 float sim_time = 0.0f;
@@ -287,7 +289,9 @@ static void send_data( const MIDGpos pos, const MIDGatt att ) {
 void usage( const string &argv0 ) {
     cout << "Usage: " << argv0 << endl;
     cout << "\t[ --help ]" << endl;
-    cout << "\t[ --file <file_name>" << endl;
+    cout << "\t[ --infile <infile_name>" << endl;
+    cout << "\t[ --serial <dev_name>" << endl;
+    cout << "\t[ --outfile <outfile_name> (capture the data to a file)" << endl;
     cout << "\t[ --hertz <hertz> ]" << endl;
     cout << "\t[ --host <hostname> ]" << endl;
     cout << "\t[ --broadcast ]" << endl;
@@ -316,10 +320,18 @@ int main( int argc, char **argv ) {
                 usage( argv[0] );
                 exit( -1 );
             }
-        } else if ( strcmp( argv[i], "--file" ) == 0 ) {
+        } else if ( strcmp( argv[i], "--infile" ) == 0 ) {
             ++i;
             if ( i < argc ) {
-                file = argv[i];
+                infile = argv[i];
+            } else {
+                usage( argv[0] );
+                exit( -1 );
+            }
+        } else if ( strcmp( argv[i], "--outfile" ) == 0 ) {
+            ++i;
+            if ( i < argc ) {
+                outfile = argv[i];
             } else {
                 usage( argv[0] );
                 exit( -1 );
@@ -419,9 +431,9 @@ int main( int argc, char **argv ) {
     }
     cout << "connected outgoing ctrls socket" << endl;
 
-    if ( file.length() ) {
+    if ( infile.length() ) {
         // Load data from a track data
-        track.load( file );
+        track.load( infile );
         cout << "Loaded " << track.pos_size() << " position records." << endl;
         cout << "Loaded " << track.att_size() << " attitude records." << endl;
 
@@ -554,16 +566,28 @@ int main( int argc, char **argv ) {
         uint32_t pos_time = 1;
         uint32_t att_time = 1;
 
-        // open the file
+        // open the serial port device
         SGSerial input( serialdev, "115200" );
         if ( !input.open( SG_IO_IN ) ) {
-            cout << "Cannot open file: " << file << endl;
+            cout << "Cannot open: " << serialdev << endl;
+            return false;
+        }
+
+        // open up the data log file if requested
+        if ( !outfile.length() ) {
+            cout << "no --outfile <name> specified, cannot capture data!"
+                 << endl;
+            return false;
+        }
+        SGFile output( outfile );
+        if ( !output.open( SG_IO_OUT ) ) {
+            cout << "Cannot open: " << outfile << endl;
             return false;
         }
 
         while ( ! input.eof() ) {
             // cout << "looking for next message ..." << endl;
-            int id = track.next_message( &input, &pos, &att );
+            int id = track.next_message( &input, &output, &pos, &att );
             count++;
 
             if ( id == 10 ) {