// 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 );
// 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 );
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;
// 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
// 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 <string> 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 );
+
+ 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;
+ }
}
+ }
- // cout << "start of message ..." << endl;
+ return true;
+}
- // 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 );
+// 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;
- // read checksum
- int cksum0 = fgetc( fd );
- int cksum1 = fgetc( fd );
+ // 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 ( validate_cksum( id, size, buf, cksum0, cksum1 ) ) {
+ parse_msg( id, buf, pos, att );
+ return id;
}
- return count;
+ cout << "Check sum failure!" << endl;
+ return -1;
}
private:
- vector <MIDGpos> posdata;
- vector <MIDGatt> attdata;
+ vector <MIDGpos> pos_data;
+ vector <MIDGatt> att_data;
FILE *fd;
// parse message and put current data into vector if message has a
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();
}
// 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;
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;
cout << "\t[ --fdm-port <fdm output port #> ]" << endl;
cout << "\t[ --ctrls-port <ctrls output port #> ]" << endl;
cout << "\t[ --altitude-offset <meters> ]" << endl;
+ cout << "\t[ --skip-seconds <seconds> ]" << endl;
}
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 );
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
}
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;
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;
// << 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;
// << 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 ) {
projects / flightgear.git / commitdiff