diff --git a/utils/GPSsmooth/MIDG-II.cxx b/utils/GPSsmooth/MIDG-II.cxx index 51b5e2eb9..bdcef9350 100644 --- a/utils/GPSsmooth/MIDG-II.cxx +++ b/utils/GPSsmooth/MIDG-II.cxx @@ -166,15 +166,8 @@ void MIDGTrack::parse_msg( const int id, char *buf, MIDGpos *pos, MIDGatt *att ) // timestamp ts = (uint32_t)read_swab( buf, 0, 4 ); - // cout << " time stamp = " << ts << endl; - if ( ts > att->get_msec() && att->get_msec() > 1.0 ) { - attdata.push_back( *att ); - } - if ( ts < att->get_msec() ) { - cout << "OOOPS moving back in time!!! " << ts << " < " << att->get_msec() << endl; - } else { - att->midg_time = MIDGTime( ts ); - } + // cout << " att time stamp = " << ts << endl; + att->midg_time = MIDGTime( ts ); // p, q, r p = (int16_t)read_swab( buf, 4, 2 ); @@ -227,15 +220,8 @@ void MIDGTrack::parse_msg( const int id, char *buf, MIDGpos *pos, MIDGatt *att ) // timestamp ts = (uint32_t)read_swab( buf, 0, 4 ); - // cout << " time stamp = " << ts << endl; - if ( ts > pos->get_msec() && pos->get_msec() > 1.0 ) { - posdata.push_back( *pos ); - } - if ( ts < pos->get_msec() ) { - cout << "OOOPS moving back in time!!! " << ts << " < " << pos->get_msec() << endl; - } else { - pos->midg_time = MIDGTime( ts ); - } + // cout << " pos time stamp = " << ts << endl; + pos->midg_time = MIDGTime( ts ); // posx, posy, posz posx = (int32_t)read_swab( buf, 4, 4 ); @@ -258,7 +244,8 @@ void MIDGTrack::parse_msg( const int id, char *buf, MIDGpos *pos, MIDGatt *att ) vely = (int32_t)read_swab( buf, 20, 4 ); velz = (int32_t)read_swab( buf, 24, 4 ); // cout << " vel = " << velx << "," << vely << "," << velz << endl; - double vel_cms = sqrt( velx*velx + vely*vely + velz*velz ); + double tmp1 = velx*velx + vely*vely + velz*velz; + double vel_cms = sqrt( tmp1 ); double vel_ms = vel_cms / 100.0; pos->speed_kts = vel_ms * SG_METER_TO_NM * 3600; @@ -288,14 +275,7 @@ void MIDGTrack::parse_msg( const int id, char *buf, MIDGpos *pos, MIDGatt *att ) // previous data or not, just roll it into the current data // independent of time stamp. gps_ts = (uint32_t)read_swab( buf, 0, 4 ); - // if ( ts > pt->get_msec() && pt->get_msec() > 1.0 ) { - // data.push_back( *pt ); - // } - // if ( ts < pt->get_msec() ) { - // cout << "OOOPS moving back in time!!! " << ts << " < " << pt->get_msec() << endl; - // } else { - // pt->midg_time = MIDGTime( ts ); - // } + // pt->midg_time = MIDGTime( ts ); gps_week = (uint16_t)read_swab( buf, 4, 2 ); // cout << " gps time stamp = " << gps_ts << " week = " << gps_week @@ -338,57 +318,82 @@ void MIDGTrack::parse_msg( const int id, char *buf, MIDGpos *pos, MIDGatt *att ) // load the specified file, return the number of records loaded -int MIDGTrack::load( const string &file ) { - int count = 0; +bool MIDGTrack::load( const string &file ) { - posdata.clear(); - attdata.clear(); + MIDGpos pos; + MIDGatt att; + + uint32_t pos_time = 1; + uint32_t att_time = 1; + + pos_data.clear(); + att_data.clear(); // openg the file fd = fopen( file.c_str(), "r" ); if ( fd == NULL ) { cout << "Cannot open file: " << file << endl; - return 0; + return false; } - vector tokens; - MIDGpos pos; - MIDGatt att; - while ( ! feof( fd ) ) { - // scan for sync characters - int sync0, sync1; - sync0 = fgetc( fd ); - sync1 = fgetc( fd ); - while ( (sync0 != 129 || sync1 != 161) && !feof(fd) ) { - sync0 = sync1; - sync1 = fgetc( fd ); - } + int id = next_message( fd, &pos, &att ); - // cout << "start of message ..." << endl; - - // read message id and size - int id = fgetc( fd ); - int size = fgetc( fd ); - // cout << "message = " << id << " size = " << size << endl; - - // load message - char buf[256]; - fread( buf, size, 1, fd ); - - // read checksum - int cksum0 = fgetc( fd ); - int cksum1 = fgetc( fd ); - - if ( validate_cksum( id, size, buf, cksum0, cksum1 ) ) { - parse_msg( id, buf, &pos, &att ); - } else { - cout << "Check sum failure!" << endl; + if ( id == 10 ) { + if ( att.get_msec() > att_time ) { + att_data.push_back( att ); + att_time = att.get_msec(); + } else { + cout << "oops att back in time" << endl; + } + } else if ( id == 12 ) { + if ( pos.get_msec() > pos_time ) { + pos_data.push_back( pos ); + pos_time = pos.get_msec(); + } else { + cout << "oops pos back in time" << endl; + } } } - return count; + return true; +} + + +// load the next message of a real time data stream +int MIDGTrack::next_message( FILE *fd, MIDGpos *pos, MIDGatt *att ) { + // scan for sync characters + int sync0, sync1; + sync0 = fgetc( fd ); + sync1 = fgetc( fd ); + while ( (sync0 != 129 || sync1 != 161) && !feof(fd) ) { + sync0 = sync1; + sync1 = fgetc( fd ); + } + + // cout << "start of message ..." << endl; + + // read message id and size + int id = fgetc( fd ); + int size = fgetc( fd ); + // cout << "message = " << id << " size = " << size << endl; + + // load message + char buf[256]; + fread( buf, size, 1, fd ); + + // read checksum + int cksum0 = fgetc( fd ); + int cksum1 = fgetc( fd ); + + if ( validate_cksum( id, size, buf, cksum0, cksum1 ) ) { + parse_msg( id, buf, pos, att ); + return id; + } + + cout << "Check sum failure!" << endl; + return -1; } diff --git a/utils/GPSsmooth/MIDG-II.hxx b/utils/GPSsmooth/MIDG-II.hxx index 31ef825ab..b1f216c75 100644 --- a/utils/GPSsmooth/MIDG-II.hxx +++ b/utils/GPSsmooth/MIDG-II.hxx @@ -128,8 +128,8 @@ class MIDGTrack { private: - vector posdata; - vector attdata; + vector pos_data; + vector att_data; FILE *fd; // parse message and put current data into vector if message has a @@ -141,23 +141,28 @@ public: MIDGTrack(); ~MIDGTrack(); - int load( const string &file ); + // read/parse the next message from the specified data stream, + // returns id # if a valid message found. + int next_message( FILE *fd, MIDGpos *pos, MIDGatt *att ); - inline int possize() const { return posdata.size(); } - inline int attsize() const { return attdata.size(); } + // load the named file into internal buffers + bool load( const string &file ); + + inline int pos_size() const { return pos_data.size(); } + inline int att_size() const { return att_data.size(); } inline MIDGpos get_pospt( const unsigned int i ) { - if ( i < posdata.size() ) { - return posdata[i]; + if ( i < pos_data.size() ) { + return pos_data[i]; } else { return MIDGpos(); } } inline MIDGatt get_attpt( const unsigned int i ) { - if ( i < attdata.size() ) { - return attdata[i]; + if ( i < att_data.size() ) { + return att_data[i]; } else { return MIDGatt(); } diff --git a/utils/GPSsmooth/MIDG_main.cxx b/utils/GPSsmooth/MIDG_main.cxx index 333877866..08981305b 100644 --- a/utils/GPSsmooth/MIDG_main.cxx +++ b/utils/GPSsmooth/MIDG_main.cxx @@ -48,6 +48,9 @@ SGTimeStamp current_time_stamp; // altitude offset double alt_offset = 0.0; +// skip initial seconds +double skip = 0.0; + // for speed estimate // double last_lat = 0.0, last_lon = 0.0; // double kts_filter = 0.0; @@ -267,7 +270,6 @@ static void midg2fg( const MIDGpos pos, const MIDGatt att, static void send_data( const MIDGpos pos, const MIDGatt att ) { int len; - int ctrlsize = sizeof( FGNetCtrls ); int fdmsize = sizeof( FGNetFDM ); // cout << "Running main loop" << endl; @@ -290,6 +292,7 @@ void usage( const string &argv0 ) { cout << "\t[ --fdm-port ]" << endl; cout << "\t[ --ctrls-port ]" << endl; cout << "\t[ --altitude-offset ]" << endl; + cout << "\t[ --skip-seconds ]" << endl; } @@ -353,6 +356,14 @@ int main( int argc, char **argv ) { usage( argv[0] ); exit( -1 ); } + } else if ( strcmp( argv[i], "--skip-seconds" ) == 0 ) { + ++i; + if ( i < argc ) { + skip = atof( argv[i] ); + } else { + usage( argv[0] ); + exit( -1 ); + } } else { usage( argv[0] ); exit( -1 ); @@ -365,8 +376,8 @@ int main( int argc, char **argv ) { exit(-1); } track.load( file ); - cout << "Loaded " << track.possize() << " position records." << endl; - cout << "Loaded " << track.attsize() << " attitude records." << endl; + cout << "Loaded " << track.pos_size() << " position records." << endl; + cout << "Loaded " << track.att_size() << " attitude records." << endl; // Setup up outgoing network connections @@ -407,7 +418,7 @@ int main( int argc, char **argv ) { } cout << "connected outgoing ctrls socket" << endl; - int size = track.possize(); + int size = track.pos_size(); double current_time = track.get_pospt(0).get_seconds(); cout << "Track begin time is " << current_time << endl; @@ -415,6 +426,9 @@ int main( int argc, char **argv ) { cout << "Track end time is " << end_time << endl; cout << "Duration = " << end_time - current_time << endl; + // advance skip seconds forward + current_time += skip; + frame_us = 1000000.0 / hertz; if ( frame_us < 0.0 ) { frame_us = 0.0; @@ -436,7 +450,9 @@ int main( int argc, char **argv ) { // << end_time << endl; // Advance position pointer - if ( current_time > pos1.get_seconds() ) { + while ( current_time > pos1.get_seconds() + && pos_count < track.pos_size() ) + { pos0 = pos1; ++pos_count; // cout << "count = " << count << endl; @@ -446,14 +462,16 @@ int main( int argc, char **argv ) { // << endl; // Advance attitude pointer - if ( current_time > att1.get_seconds() ) { + 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 << "p0 = " << p0.get_time() << " p1 = " << p1.get_time() - // << endl; + // cout << "pos0 = " << pos0.get_seconds() + // << " pos1 = " << pos1.get_seconds() << endl; double pos_percent; if ( fabs(pos1.get_seconds() - pos0.get_seconds()) < 0.00001 ) {