diff --git a/utils/GPSsmooth/MIDG-II.cxx b/utils/GPSsmooth/MIDG-II.cxx index f950a1139..b68e6ac66 100644 --- a/utils/GPSsmooth/MIDG-II.cxx +++ b/utils/GPSsmooth/MIDG-II.cxx @@ -56,13 +56,17 @@ static bool validate_cksum( uint8_t id, uint8_t size, char *buf, c0 += id; c1 += c0; + // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 << endl; c0 += size; c1 += c0; + // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 << endl; for ( uint8_t i = 0; i < size; i++ ) { c0 += (uint8_t)buf[i]; c1 += c0; + // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 + // << " [" << (unsigned int)buf[i] << "]" << endl; } // cout << "c0 = " << (unsigned int)c0 << " (" << (unsigned int)cksum0 @@ -365,33 +369,65 @@ 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 ) { + bool myeof = false; + int result = 0; + while ( result != length && !myeof ) { + result = ch->read( buf, length ); + if ( ch->get_type() == sgFileType ) { + myeof = ((SGFile *)ch)->eof(); + } + } + + return result; +} + // load the next message of a real time data stream int MIDGTrack::next_message( SGIOChannel *ch, MIDGpos *pos, MIDGatt *att ) { char tmpbuf[256]; char savebuf[256]; + // cout << "in next_message()" << endl; + + bool myeof = false; + // scan for sync characters uint8_t sync0, sync1; - ch->read( tmpbuf, 1 ); sync0 = (unsigned char)tmpbuf[0]; - ch->read( tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0]; - while ( (sync0 != 129 || sync1 != 161) && !ch->eof() ) { + myread( ch, tmpbuf, 1 ); sync0 = (unsigned char)tmpbuf[0]; + myread( ch, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0]; + while ( (sync0 != 129 || sync1 != 161) && !myeof ) { sync0 = sync1; - ch->read( tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0]; + myread( ch, 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(); + } } - // cout << "start of message ..." << endl; + // cout << "found start of message ..." << endl; // read message id and size - ch->read( tmpbuf, 1 ); uint8_t id = (unsigned char)tmpbuf[0]; - ch->read( tmpbuf, 1 ); uint8_t size = (unsigned char)tmpbuf[0]; + myread( ch, tmpbuf, 1 ); uint8_t id = (unsigned char)tmpbuf[0]; + myread( ch, tmpbuf, 1 ); uint8_t size = (unsigned char)tmpbuf[0]; // cout << "message = " << (int)id << " size = " << (int)size << endl; // load message - ch->read( savebuf, size ); + if ( ch->get_type() == sgFileType ) { + int count = myread( ch, 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]; + } + } // read checksum - ch->read( tmpbuf, 1 ); uint8_t cksum0 = (unsigned char)tmpbuf[0]; - ch->read( tmpbuf, 1 ); uint8_t cksum1 = (unsigned char)tmpbuf[0]; + myread( ch, tmpbuf, 1 ); uint8_t cksum0 = (unsigned char)tmpbuf[0]; + myread( ch, 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_main.cxx b/utils/GPSsmooth/MIDG_main.cxx index 08981305b..59211bb91 100644 --- a/utils/GPSsmooth/MIDG_main.cxx +++ b/utils/GPSsmooth/MIDG_main.cxx @@ -10,6 +10,7 @@ #include #include // endian tests +#include #include #include @@ -36,6 +37,7 @@ static int ctrls_port = 5506; // Default path static string file = ""; +static string serialdev = ""; // Master time counter float sim_time = 0.0f; @@ -322,6 +324,14 @@ int main( int argc, char **argv ) { usage( argv[0] ); exit( -1 ); } + } else if ( strcmp( argv[i], "--serial" ) == 0 ) { + ++i; + if ( i < argc ) { + serialdev = argv[i]; + } else { + usage( argv[0] ); + exit( -1 ); + } } else if ( strcmp( argv[i], "--host" ) == 0 ) { ++i; if ( i < argc ) { @@ -370,15 +380,6 @@ int main( int argc, char **argv ) { } } - // Load the track data - if ( file == "" ) { - cout << "No track file specified" << endl; - exit(-1); - } - track.load( file ); - cout << "Loaded " << track.pos_size() << " position records." << endl; - cout << "Loaded " << track.att_size() << " attitude records." << endl; - // Setup up outgoing network connections netInit( &argc,argv ); // We must call this before any other net stuff @@ -418,122 +419,179 @@ int main( int argc, char **argv ) { } cout << "connected outgoing ctrls socket" << endl; - int size = track.pos_size(); + if ( file.length() ) { + // Load data from a track data + track.load( file ); + cout << "Loaded " << track.pos_size() << " position records." << endl; + cout << "Loaded " << track.att_size() << " attitude records." << endl; - double current_time = track.get_pospt(0).get_seconds(); - cout << "Track begin time is " << current_time << endl; - double end_time = track.get_pospt(size-1).get_seconds(); - cout << "Track end time is " << end_time << endl; - cout << "Duration = " << end_time - current_time << endl; + int size = track.pos_size(); - // advance skip seconds forward - current_time += skip; + double current_time = track.get_pospt(0).get_seconds(); + cout << "Track begin time is " << current_time << endl; + double end_time = track.get_pospt(size-1).get_seconds(); + cout << "Track end time is " << end_time << endl; + cout << "Duration = " << end_time - current_time << endl; - frame_us = 1000000.0 / hertz; - if ( frame_us < 0.0 ) { - frame_us = 0.0; - } + // advance skip seconds forward + current_time += skip; - SGTimeStamp start_time; - start_time.stamp(); - int pos_count = 0; - int att_count = 0; + frame_us = 1000000.0 / hertz; + if ( frame_us < 0.0 ) { + frame_us = 0.0; + } - MIDGpos pos0, pos1; - pos0 = pos1 = track.get_pospt( 0 ); + SGTimeStamp start_time; + start_time.stamp(); + int pos_count = 0; + int att_count = 0; + + MIDGpos pos0, pos1; + pos0 = pos1 = track.get_pospt( 0 ); - MIDGatt att0, att1; - att0 = att1 = track.get_attpt( 0 ); + MIDGatt att0, att1; + att0 = att1 = track.get_attpt( 0 ); - while ( current_time < end_time ) { - // cout << "current_time = " << current_time << " end_time = " - // << end_time << endl; + while ( current_time < end_time ) { + // cout << "current_time = " << current_time << " end_time = " + // << end_time << endl; - // Advance position pointer - while ( current_time > pos1.get_seconds() - && pos_count < track.pos_size() ) - { - pos0 = pos1; - ++pos_count; - // cout << "count = " << count << endl; - pos1 = track.get_pospt( pos_count ); - } - // cout << "p0 = " << p0.get_time() << " p1 = " << p1.get_time() - // << endl; + // Advance position pointer + while ( current_time > pos1.get_seconds() + && pos_count < track.pos_size() ) + { + pos0 = pos1; + ++pos_count; + // cout << "count = " << count << endl; + pos1 = track.get_pospt( pos_count ); + } + // cout << "p0 = " << p0.get_time() << " p1 = " << p1.get_time() + // << endl; - // Advance attitude pointer - while ( current_time > att1.get_seconds() - && att_count < track.att_size() ) - { - att0 = att1; - ++att_count; - // cout << "count = " << count << endl; - att1 = track.get_attpt( att_count ); - } - // cout << "pos0 = " << pos0.get_seconds() - // << " pos1 = " << pos1.get_seconds() << endl; + // Advance attitude pointer + while ( current_time > att1.get_seconds() + && att_count < track.att_size() ) + { + att0 = att1; + ++att_count; + // cout << "count = " << count << endl; + att1 = track.get_attpt( att_count ); + } + // cout << "pos0 = " << pos0.get_seconds() + // << " pos1 = " << pos1.get_seconds() << endl; - double pos_percent; - if ( fabs(pos1.get_seconds() - pos0.get_seconds()) < 0.00001 ) { - pos_percent = 0.0; - } else { - pos_percent = - (current_time - pos0.get_seconds()) / - (pos1.get_seconds() - pos0.get_seconds()); - } - // cout << "Percent = " << percent << endl; - double att_percent; - if ( fabs(att1.get_seconds() - att0.get_seconds()) < 0.00001 ) { - att_percent = 0.0; - } else { - att_percent = - (current_time - att0.get_seconds()) / - (att1.get_seconds() - att0.get_seconds()); - } - // cout << "Percent = " << percent << endl; + double pos_percent; + if ( fabs(pos1.get_seconds() - pos0.get_seconds()) < 0.00001 ) { + pos_percent = 0.0; + } else { + pos_percent = + (current_time - pos0.get_seconds()) / + (pos1.get_seconds() - pos0.get_seconds()); + } + // cout << "Percent = " << percent << endl; + double att_percent; + if ( fabs(att1.get_seconds() - att0.get_seconds()) < 0.00001 ) { + att_percent = 0.0; + } else { + att_percent = + (current_time - att0.get_seconds()) / + (att1.get_seconds() - att0.get_seconds()); + } + // cout << "Percent = " << percent << endl; - MIDGpos pos = MIDGInterpPos( pos0, pos1, pos_percent ); - MIDGatt att = MIDGInterpAtt( att0, att1, att_percent ); - // cout << current_time << " " << p0.lat_deg << ", " << p0.lon_deg - // << endl; - // cout << current_time << " " << p1.lat_deg << ", " << p1.lon_deg - // << endl; - // cout << (double)current_time << " " << pos.lat_deg << ", " - // << pos.lon_deg << " " << att.yaw_deg << endl; - printf( "%.3f %.4f %.4f %.1f %.2f %.2f %.2f\n", - current_time, - pos.lat_deg, pos.lon_deg, pos.altitude_msl, - att.yaw_rad * 180.0 / SG_PI, - att.pitch_rad * 180.0 / SG_PI, - att.roll_rad * 180.0 / SG_PI ); + MIDGpos pos = MIDGInterpPos( pos0, pos1, pos_percent ); + MIDGatt att = MIDGInterpAtt( att0, att1, att_percent ); + // cout << current_time << " " << p0.lat_deg << ", " << p0.lon_deg + // << endl; + // cout << current_time << " " << p1.lat_deg << ", " << p1.lon_deg + // << endl; + // cout << (double)current_time << " " << pos.lat_deg << ", " + // << pos.lon_deg << " " << att.yaw_deg << endl; + printf( "%.3f %.4f %.4f %.1f %.2f %.2f %.2f\n", + current_time, + pos.lat_deg, pos.lon_deg, pos.altitude_msl, + att.yaw_rad * 180.0 / SG_PI, + att.pitch_rad * 180.0 / SG_PI, + att.roll_rad * 180.0 / SG_PI ); - send_data( pos, att ); + send_data( pos, att ); - // Update the elapsed time. - static bool first_time = true; - if ( first_time ) { - last_time_stamp.stamp(); - first_time = false; - } + // Update the elapsed time. + static bool first_time = true; + if ( first_time ) { + last_time_stamp.stamp(); + first_time = false; + } - current_time_stamp.stamp(); - /* Convert to ms */ - double elapsed_us = current_time_stamp - last_time_stamp; - if ( elapsed_us < (frame_us - 2000) ) { - double requested_us = (frame_us - elapsed_us) - 2000 ; - ulMilliSecondSleep ( (int)(requested_us / 1000.0) ) ; - } - current_time_stamp.stamp(); - while ( current_time_stamp - last_time_stamp < frame_us ) { current_time_stamp.stamp(); + /* Convert to ms */ + double elapsed_us = current_time_stamp - last_time_stamp; + if ( elapsed_us < (frame_us - 2000) ) { + double requested_us = (frame_us - elapsed_us) - 2000 ; + ulMilliSecondSleep ( (int)(requested_us / 1000.0) ) ; + } + current_time_stamp.stamp(); + while ( current_time_stamp - last_time_stamp < frame_us ) { + current_time_stamp.stamp(); + } + + current_time += (frame_us / 1000000.0); + last_time_stamp = current_time_stamp; } - current_time += (frame_us / 1000000.0); - last_time_stamp = current_time_stamp; - } + cout << "Processed " << pos_count << " entries in " + << (current_time_stamp - start_time) / 1000000 << " seconds." + << endl; + } else if ( serialdev.length() ) { + // process incoming data from the serial port - cout << "Processed " << pos_count << " entries in " - << (current_time_stamp - start_time) / 1000000 << " seconds." << endl; + int count = 0; + double current_time = 0.0; + + MIDGpos pos; + MIDGatt att; + + uint32_t pos_time = 1; + uint32_t att_time = 1; + + // open the file + SGSerial input( serialdev, "115200" ); + if ( !input.open( SG_IO_IN ) ) { + cout << "Cannot open file: " << file << endl; + return false; + } + + while ( ! input.eof() ) { + // cout << "looking for next message ..." << endl; + int id = track.next_message( &input, &pos, &att ); + count++; + + if ( id == 10 ) { + if ( att.get_msec() > att_time ) { + att_time = att.get_msec(); + current_time = att_time; + } else { + cout << "oops att back in time" << endl; + } + } else if ( id == 12 ) { + if ( pos.get_msec() > pos_time ) { + pos_time = pos.get_msec(); + current_time = pos_time; + } else { + cout << "oops pos back in time" << endl; + } + } + + printf( "%.3f %.4f %.4f %.1f %.2f %.2f %.2f\n", + current_time, + pos.lat_deg, pos.lon_deg, pos.altitude_msl, + att.yaw_rad * 180.0 / SG_PI, + att.pitch_rad * 180.0 / SG_PI, + att.roll_rad * 180.0 / SG_PI ); + + send_data( pos, att ); + } + } return 0; } diff --git a/utils/GPSsmooth/Makefile.am b/utils/GPSsmooth/Makefile.am index d8a289f5a..33bd55987 100644 --- a/utils/GPSsmooth/Makefile.am +++ b/utils/GPSsmooth/Makefile.am @@ -13,7 +13,7 @@ MIDGsmooth_SOURCES = \ MIDG_main.cxx MIDGsmooth_LDADD = \ - -lsgio -lsgtiming -lsgmath -lsgbucket -lsgmisc -lsgdebug \ + -lsgio -lsgserial -lsgtiming -lsgmath -lsgbucket -lsgmisc -lsgdebug \ -lplibnet -lplibul \ $(joystick_LIBS) $(network_LIBS) $(base_LIBS) -lz