fly/flightgear
1
0
Fork 0
Code Activity

Fix one small type ambiguity and then do some code restructuring.

Browse source
This commit is contained in:
curt 2005-09-23 19:55:52 +00:00
parent 22423d0bab
commit 4f72904235
3 changed files with 109 additions and 81 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

133
utils/GPSsmooth/MIDG-II.cxx
View file

@ -166,15 +166,8 @@ void MIDGTrack::parse_msg( const int id, char *buf, MIDGpos *pos, MIDGatt *att )
// timestamp // timestamp
ts = (uint32_t)read_swab( buf, 0, 4 ); ts = (uint32_t)read_swab( buf, 0, 4 );
// cout << " time stamp = " << ts << endl; // cout << " att time stamp = " << ts << endl;
if ( ts > att->get_msec() && att->get_msec() > 1.0 ) { att->midg_time = MIDGTime( ts );
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 );
}
// p, q, r // p, q, r
p = (int16_t)read_swab( buf, 4, 2 ); 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 // timestamp
ts = (uint32_t)read_swab( buf, 0, 4 ); ts = (uint32_t)read_swab( buf, 0, 4 );
// cout << " time stamp = " << ts << endl; // cout << " pos time stamp = " << ts << endl;
if ( ts > pos->get_msec() && pos->get_msec() > 1.0 ) { pos->midg_time = MIDGTime( ts );
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 );
}
// posx, posy, posz // posx, posy, posz
posx = (int32_t)read_swab( buf, 4, 4 ); 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 ); vely = (int32_t)read_swab( buf, 20, 4 );
velz = (int32_t)read_swab( buf, 24, 4 ); velz = (int32_t)read_swab( buf, 24, 4 );
// cout << " vel = " << velx << "," << vely << "," << velz << endl; // 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; double vel_ms = vel_cms / 100.0;
pos->speed_kts = vel_ms * SG_METER_TO_NM * 3600; 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 // previous data or not, just roll it into the current data
// independent of time stamp. // independent of time stamp.
gps_ts = (uint32_t)read_swab( buf, 0, 4 ); gps_ts = (uint32_t)read_swab( buf, 0, 4 );
// if ( ts > pt->get_msec() && pt->get_msec() > 1.0 ) { // pt->midg_time = MIDGTime( ts );
// 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 );
// }
gps_week = (uint16_t)read_swab( buf, 4, 2 ); gps_week = (uint16_t)read_swab( buf, 4, 2 );
// cout << " gps time stamp = " << gps_ts << " week = " << gps_week // 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 // load the specified file, return the number of records loaded
int MIDGTrack::load( const string &file ) { bool MIDGTrack::load( const string &file ) {
int count = 0;
posdata.clear(); MIDGpos pos;
attdata.clear(); MIDGatt att;
uint32_t pos_time = 1;
uint32_t att_time = 1;
pos_data.clear();
att_data.clear();
// openg the file // openg the file
fd = fopen( file.c_str(), "r" ); fd = fopen( file.c_str(), "r" );
if ( fd == NULL ) { if ( fd == NULL ) {
cout << "Cannot open file: " << file << endl; cout << "Cannot open file: " << file << endl;
return 0; return false;
} }
vector <string> tokens;
MIDGpos pos;
MIDGatt att;
while ( ! feof( fd ) ) { while ( ! feof( fd ) ) {
// scan for sync characters int id = next_message( fd, &pos, &att );
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; if ( id == 10 ) {
if ( att.get_msec() > att_time ) {
// read message id and size att_data.push_back( att );
int id = fgetc( fd ); att_time = att.get_msec();
int size = fgetc( fd ); } else {
// cout << "message = " << id << " size = " << size << endl; cout << "oops att back in time" << endl;
}
// load message } else if ( id == 12 ) {
char buf[256]; if ( pos.get_msec() > pos_time ) {
fread( buf, size, 1, fd ); pos_data.push_back( pos );
pos_time = pos.get_msec();
// read checksum } else {
int cksum0 = fgetc( fd ); cout << "oops pos back in time" << endl;
int cksum1 = fgetc( fd ); }
if ( validate_cksum( id, size, buf, cksum0, cksum1 ) ) {
parse_msg( id, buf, &pos, &att );
} else {
cout << "Check sum failure!" << 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;
} }

23
utils/GPSsmooth/MIDG-II.hxx
View file

@ -128,8 +128,8 @@ class MIDGTrack {
private: private:
vector <MIDGpos> posdata; vector <MIDGpos> pos_data;
vector <MIDGatt> attdata; vector <MIDGatt> att_data;
FILE *fd; FILE *fd;
// parse message and put current data into vector if message has a // parse message and put current data into vector if message has a
@ -141,23 +141,28 @@ public:
MIDGTrack(); MIDGTrack();
~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(); } // load the named file into internal buffers
inline int attsize() const { return attdata.size(); } 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 ) inline MIDGpos get_pospt( const unsigned int i )
{ {
if ( i < posdata.size() ) { if ( i < pos_data.size() ) {
return posdata[i]; return pos_data[i];
} else { } else {
return MIDGpos(); return MIDGpos();
} }
} }
inline MIDGatt get_attpt( const unsigned int i ) inline MIDGatt get_attpt( const unsigned int i )
{ {
if ( i < attdata.size() ) { if ( i < att_data.size() ) {
return attdata[i]; return att_data[i];
} else { } else {
return MIDGatt(); return MIDGatt();
} }

34
utils/GPSsmooth/MIDG_main.cxx
View file

@ -48,6 +48,9 @@ SGTimeStamp current_time_stamp;
// altitude offset // altitude offset
double alt_offset = 0.0; double alt_offset = 0.0;
// skip initial seconds
double skip = 0.0;
// for speed estimate // for speed estimate
// double last_lat = 0.0, last_lon = 0.0; // double last_lat = 0.0, last_lon = 0.0;
// double kts_filter = 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 ) { static void send_data( const MIDGpos pos, const MIDGatt att ) {
int len; int len;
int ctrlsize = sizeof( FGNetCtrls );
int fdmsize = sizeof( FGNetFDM ); int fdmsize = sizeof( FGNetFDM );
// cout << "Running main loop" << endl; // cout << "Running main loop" << endl;
@ -290,6 +292,7 @@ void usage( const string &argv0 ) {
cout << "\t[ --fdm-port <fdm output port #> ]" << endl; cout << "\t[ --fdm-port <fdm output port #> ]" << endl;
cout << "\t[ --ctrls-port <ctrls output port #> ]" << endl; cout << "\t[ --ctrls-port <ctrls output port #> ]" << endl;
cout << "\t[ --altitude-offset <meters> ]" << endl; cout << "\t[ --altitude-offset <meters> ]" << endl;
cout << "\t[ --skip-seconds <seconds> ]" << endl;
} }
@ -353,6 +356,14 @@ int main( int argc, char **argv ) {
usage( argv[0] ); usage( argv[0] );
exit( -1 ); 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 { } else {
usage( argv[0] ); usage( argv[0] );
exit( -1 ); exit( -1 );
@ -365,8 +376,8 @@ int main( int argc, char **argv ) {
exit(-1); exit(-1);
} }
track.load( file ); track.load( file );
cout << "Loaded " << track.possize() << " position records." << endl; cout << "Loaded " << track.pos_size() << " position records." << endl;
cout << "Loaded " << track.attsize() << " attitude records." << endl; cout << "Loaded " << track.att_size() << " attitude records." << endl;
// Setup up outgoing network connections // Setup up outgoing network connections
@ -407,7 +418,7 @@ int main( int argc, char **argv ) {
} }
cout << "connected outgoing ctrls socket" << endl; cout << "connected outgoing ctrls socket" << endl;
int size = track.possize(); int size = track.pos_size();
double current_time = track.get_pospt(0).get_seconds(); double current_time = track.get_pospt(0).get_seconds();
cout << "Track begin time is " << current_time << endl; 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 << "Track end time is " << end_time << endl;
cout << "Duration = " << end_time - current_time << endl; cout << "Duration = " << end_time - current_time << endl;
// advance skip seconds forward
current_time += skip;
frame_us = 1000000.0 / hertz; frame_us = 1000000.0 / hertz;
if ( frame_us < 0.0 ) { if ( frame_us < 0.0 ) {
frame_us = 0.0; frame_us = 0.0;
@ -436,7 +450,9 @@ int main( int argc, char **argv ) {
// << end_time << endl; // << end_time << endl;
// Advance position pointer // Advance position pointer
if ( current_time > pos1.get_seconds() ) { while ( current_time > pos1.get_seconds()
&& pos_count < track.pos_size() )
{
pos0 = pos1; pos0 = pos1;
++pos_count; ++pos_count;
// cout << "count = " << count << endl; // cout << "count = " << count << endl;
@ -446,14 +462,16 @@ int main( int argc, char **argv ) {
// << endl; // << endl;
// Advance attitude pointer // Advance attitude pointer
if ( current_time > att1.get_seconds() ) { while ( current_time > att1.get_seconds()
&& att_count < track.att_size() )
{
att0 = att1; att0 = att1;
++att_count; ++att_count;
// cout << "count = " << count << endl; // cout << "count = " << count << endl;
att1 = track.get_attpt( att_count ); att1 = track.get_attpt( att_count );
} }
// cout << "p0 = " << p0.get_time() << " p1 = " << p1.get_time() // cout << "pos0 = " << pos0.get_seconds()
// << endl; // << " pos1 = " << pos1.get_seconds() << endl;
double pos_percent; double pos_percent;
if ( fabs(pos1.get_seconds() - pos0.get_seconds()) < 0.00001 ) { if ( fabs(pos1.get_seconds() - pos0.get_seconds()) < 0.00001 ) {