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
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 <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 );
// 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;
}

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

@ -128,8 +128,8 @@ class MIDGTrack {
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
@ -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();
}

34
utils/GPSsmooth/MIDG_main.cxx
View file

@ -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 <fdm output port #> ]" << endl;
cout << "\t[ --ctrls-port <ctrls output port #> ]" << 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] );
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 ) {