X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=utils%2FGPSsmooth%2FMIDG_main.cxx;h=613f1d39d8c77d40e0677b0397a410c0d7d29a0d;hb=4dc76922c88187269957737f7a5d23017f8d51ad;hp=333877866d632e15078130ee187fb8b84722c421;hpb=7473730b857265eaa9d0d8883abbf95718f34354;p=flightgear.git diff --git a/utils/GPSsmooth/MIDG_main.cxx b/utils/GPSsmooth/MIDG_main.cxx index 333877866..613f1d39d 100644 --- a/utils/GPSsmooth/MIDG_main.cxx +++ b/utils/GPSsmooth/MIDG_main.cxx @@ -10,6 +10,8 @@ #include #include // endian tests +#include +#include #include #include @@ -19,9 +21,9 @@ #include "MIDG-II.hxx" -SG_USING_STD(cout); -SG_USING_STD(endl); -SG_USING_STD(string); +using std::cout; +using std::endl; +using std::string; // Network channels @@ -35,7 +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; @@ -48,6 +52,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 +274,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; @@ -283,13 +289,16 @@ 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; cout << "\t[ --fdm-port ]" << endl; cout << "\t[ --ctrls-port ]" << endl; cout << "\t[ --altitude-offset ]" << endl; + cout << "\t[ --skip-seconds ]" << endl; } @@ -311,10 +320,26 @@ 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 ) { + infile = argv[i]; + } else { + usage( argv[0] ); + exit( -1 ); + } + } else if ( strcmp( argv[i], "--outfile" ) == 0 ) { ++i; if ( i < argc ) { - file = argv[i]; + outfile = argv[i]; + } else { + 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 ); @@ -353,21 +378,20 @@ 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 ); } } - // Load the track data - if ( file == "" ) { - cout << "No track file specified" << endl; - exit(-1); - } - track.load( file ); - cout << "Loaded " << track.possize() << " position records." << endl; - cout << "Loaded " << track.attsize() << " attitude records." << endl; - // Setup up outgoing network connections netInit( &argc,argv ); // We must call this before any other net stuff @@ -407,115 +431,196 @@ int main( int argc, char **argv ) { } cout << "connected outgoing ctrls socket" << endl; - int size = track.possize(); + if ( infile.length() ) { + // Load data from a track data + track.load( infile ); + 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(); - frame_us = 1000000.0 / hertz; - if ( frame_us < 0.0 ) { - frame_us = 0.0; - } + 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; + + // advance skip seconds forward + current_time += skip; + + frame_us = 1000000.0 / hertz; + if ( frame_us < 0.0 ) { + frame_us = 0.0; + } - SGTimeStamp start_time; - start_time.stamp(); - int pos_count = 0; - int att_count = 0; + SGTimeStamp start_time; + start_time.stamp(); + int pos_count = 0; + int att_count = 0; - MIDGpos pos0, pos1; - pos0 = pos1 = track.get_pospt( 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; - - // Advance position pointer - if ( current_time > pos1.get_seconds() ) { - 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 - if ( current_time > att1.get_seconds() ) { - att0 = att1; - ++att_count; - // cout << "count = " << count << endl; - att1 = track.get_attpt( att_count ); - } - // cout << "p0 = " << p0.get_time() << " p1 = " << p1.get_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 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()); + 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; + if ( pos.lat_deg > -500 ) { + 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 ); + + // 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).toUSecs(); + 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).toUSecs() < frame_us ) { + current_time_stamp.stamp(); + } + + current_time += (frame_us / 1000000.0); + last_time_stamp = current_time_stamp; } - // 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 ); - - send_data( pos, att ); - - // Update the elapsed time. - static bool first_time = true; - if ( first_time ) { - last_time_stamp.stamp(); - first_time = false; + + cout << "Processed " << pos_count << " entries in " + << current_time_stamp - start_time << " seconds." + << endl; + } else if ( serialdev.length() ) { + // process incoming data from the serial port + + int count = 0; + double current_time = 0.0; + + MIDGpos pos; + MIDGatt att; + + uint32_t pos_time = 1; + uint32_t att_time = 1; + + // open the serial port device + SGSerialPort input( serialdev, 115200 ); + if ( !input.is_enabled() ) { + cout << "Cannot open: " << serialdev << endl; + return 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) ) ; + // open up the data log file if requested + if ( !outfile.length() ) { + cout << "no --outfile specified, cannot capture data!" + << endl; + return false; } - current_time_stamp.stamp(); - while ( current_time_stamp - last_time_stamp < frame_us ) { - current_time_stamp.stamp(); + SGFile output( outfile ); + if ( !output.open( SG_IO_OUT ) ) { + cout << "Cannot open: " << outfile << endl; + return false; } - current_time += (frame_us / 1000000.0); - last_time_stamp = current_time_stamp; - } + while ( input.is_enabled() ) { + // cout << "looking for next message ..." << endl; + int id = track.next_message( &input, &output, &pos, &att ); + cout << "message id = " << id << endl; + 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; + } + } - cout << "Processed " << pos_count << " entries in " - << (current_time_stamp - start_time) / 1000000 << " seconds." << endl; + if ( pos.lat_deg > -500 ) { + // 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; }