diff --git a/utils/GPSsmooth/MIDG-II.cxx b/utils/GPSsmooth/MIDG-II.cxx index b68e6ac66..3d84d9d2b 100644 --- a/utils/GPSsmooth/MIDG-II.cxx +++ b/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, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0]; + myread( ch, log, tmpbuf, 1 ); sync0 = (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, tmpbuf, 1 ); uint8_t size = (unsigned char)tmpbuf[0]; + myread( ch, log, tmpbuf, 1 ); uint8_t id = (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, tmpbuf, 1 ); uint8_t cksum1 = (unsigned char)tmpbuf[0]; + myread( ch, log, tmpbuf, 1 ); uint8_t cksum0 = (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 ); diff --git a/utils/GPSsmooth/MIDG-II.hxx b/utils/GPSsmooth/MIDG-II.hxx index c9ebcfdac..cfa5daa55 100644 --- a/utils/GPSsmooth/MIDG-II.hxx +++ b/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 ); diff --git a/utils/GPSsmooth/MIDG_main.cxx b/utils/GPSsmooth/MIDG_main.cxx index 59211bb91..963d908e9 100644 --- a/utils/GPSsmooth/MIDG_main.cxx +++ b/utils/GPSsmooth/MIDG_main.cxx @@ -10,6 +10,7 @@ #include #include // endian tests +#include #include #include #include @@ -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 " << endl; + cout << "\t[ --infile " << endl; + cout << "\t[ --serial " << endl; + cout << "\t[ --outfile (capture the data to a file)" << endl; cout << "\t[ --hertz ]" << endl; cout << "\t[ --host ]" << 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 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 ) {