fly/flightgear
1
0
Fork 0
Code Activity

Fix a bug in file logging (add an ignore checksum option just to humor

Browse source 
myself.)
This commit is contained in:
curt 2007-02-21 21:11:15 +00:00
parent b6f98f91b9
commit 006a7c1bd8
3 changed files with 45 additions and 24 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

52
utils/GPSsmooth/UGear.cxx
View file

@ -49,8 +49,13 @@ static double sg_swap_double( uint8_t *buf, size_t offset ) {
static bool validate_cksum( uint8_t id, uint8_t size, char *buf, static bool validate_cksum( uint8_t id, uint8_t size, char *buf,
uint8_t cksum0, uint8_t cksum1 ) uint8_t cksum0, uint8_t cksum1,
bool ignore_checksum )
{ {
if ( ignore_checksum ) {
return true;
}
uint8_t c0 = 0; uint8_t c0 = 0;
uint8_t c1 = 0; uint8_t c1 = 0;
@ -136,7 +141,7 @@ void UGEARTrack::parse_msg( const int id, char *buf,
// load the specified file, return the number of records loaded // load the specified file, return the number of records loaded
bool UGEARTrack::load( const string &file ) { bool UGEARTrack::load( const string &file, bool ignore_checksum ) {
int count = 0; int count = 0;
gps gpspacket; gps gpspacket;
@ -167,7 +172,8 @@ bool UGEARTrack::load( const string &file ) {
while ( ! input.eof() ) { while ( ! input.eof() ) {
// cout << "looking for next message ..." << endl; // cout << "looking for next message ..." << endl;
int id = next_message( &input, NULL, &gpspacket, &imupacket, int id = next_message( &input, NULL, &gpspacket, &imupacket,
&navpacket, &servopacket, &healthpacket ); &navpacket, &servopacket, &healthpacket,
ignore_checksum );
count++; count++;
if ( id == GPS_PACKET ) { if ( id == GPS_PACKET ) {
@ -244,21 +250,23 @@ int serial_read( SGSerialPort *serial, SGIOChannel *log,
while ( bytes_read < length ) { while ( bytes_read < length ) {
result = serial->read_port( tmp, length - bytes_read ); result = serial->read_port( tmp, length - bytes_read );
if ( result > 0 && log != NULL ) {
log->write( buf, result );
}
bytes_read += result; bytes_read += result;
tmp += result; tmp += result;
// cout << " read " << bytes_read << " of " << length << endl; // cout << " read " << bytes_read << " of " << length << endl;
} }
if ( bytes_read > 0 && log != NULL ) {
log->write( buf, bytes_read );
}
return bytes_read; return bytes_read;
} }
// load the next message of a real time data stream // load the next message of a real time data stream
int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log, int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
gps *gpspacket, imu *imupacket, nav *navpacket, gps *gpspacket, imu *imupacket, nav *navpacket,
servo *servopacket, health *healthpacket ) servo *servopacket, health *healthpacket,
bool ignore_checksum )
{ {
char tmpbuf[256]; char tmpbuf[256];
char savebuf[256]; char savebuf[256];
@ -269,14 +277,15 @@ int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
// scan for sync characters // scan for sync characters
uint8_t sync0, sync1; uint8_t sync0, sync1;
myread( ch, log, tmpbuf, 1 ); sync0 = (unsigned char)tmpbuf[0]; myread( ch, log, tmpbuf, 2 );
myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0]; sync0 = (unsigned char)tmpbuf[0];
sync1 = (unsigned char)tmpbuf[1];
while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) { while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) {
sync0 = sync1; sync0 = sync1;
myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0]; myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
// cout << "scanning for start of message " cout << "scanning for start of message "
// << (unsigned int)sync0 << " " << (unsigned int)sync1 << (unsigned int)sync0 << " " << (unsigned int)sync1
// << ", eof = " << ch->eof() << endl; << ", eof = " << ch->eof() << endl;
if ( ch->get_type() == sgFileType ) { if ( ch->get_type() == sgFileType ) {
myeof = ((SGFile *)ch)->eof(); myeof = ((SGFile *)ch)->eof();
} }
@ -285,8 +294,9 @@ int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
cout << "found start of message ..." << endl; cout << "found start of message ..." << endl;
// read message id and size // read message id and size
myread( ch, log, tmpbuf, 1 ); uint8_t id = (unsigned char)tmpbuf[0]; myread( ch, log, tmpbuf, 2 );
myread( ch, log, tmpbuf, 1 ); uint8_t size = (unsigned char)tmpbuf[0]; uint8_t id = (unsigned char)tmpbuf[0];
uint8_t size = (unsigned char)tmpbuf[1];
// cout << "message = " << (int)id << " size = " << (int)size << endl; // cout << "message = " << (int)id << " size = " << (int)size << endl;
// load message // load message
@ -306,10 +316,12 @@ int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
} }
// read checksum // read checksum
myread( ch, log, tmpbuf, 1 ); uint8_t cksum0 = (unsigned char)tmpbuf[0]; myread( ch, log, tmpbuf, 2 );
myread( ch, log, tmpbuf, 1 ); uint8_t cksum1 = (unsigned char)tmpbuf[0]; uint8_t cksum0 = (unsigned char)tmpbuf[0];
uint8_t cksum1 = (unsigned char)tmpbuf[1];
if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) { if ( validate_cksum( id, size, savebuf, cksum0, cksum1, ignore_checksum ) )
{
parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket, parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket,
healthpacket ); healthpacket );
return id; return id;
@ -323,7 +335,8 @@ int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
// load the next message of a real time data stream // load the next message of a real time data stream
int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log, int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
gps *gpspacket, imu *imupacket, nav *navpacket, gps *gpspacket, imu *imupacket, nav *navpacket,
servo *servopacket, health *healthpacket ) servo *servopacket, health *healthpacket,
bool ignore_checksum )
{ {
char tmpbuf[256]; char tmpbuf[256];
char savebuf[256]; char savebuf[256];
@ -364,7 +377,8 @@ int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
// cout << "cksum0 = " << (int)cksum0 << " cksum1 = " << (int)cksum1 // cout << "cksum0 = " << (int)cksum0 << " cksum1 = " << (int)cksum1
// << endl; // << endl;
if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) { if ( validate_cksum( id, size, savebuf, cksum0, cksum1, ignore_checksum ) )
{
parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket, parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket,
healthpacket ); healthpacket );

8
utils/GPSsmooth/UGear.hxx
View file

@ -99,13 +99,15 @@ public:
// returns id # if a valid message found. // returns id # if a valid message found.
int next_message( SGIOChannel *ch, SGIOChannel *log, int next_message( SGIOChannel *ch, SGIOChannel *log,
gps *gpspacket, imu *imupacket, nav *navpacket, gps *gpspacket, imu *imupacket, nav *navpacket,
servo *servopacket, health * healthpacket ); servo *servopacket, health * healthpacket,
bool ignore_checksum );
int next_message( SGSerialPort *serial, SGIOChannel *log, int next_message( SGSerialPort *serial, SGIOChannel *log,
gps *gpspacket, imu *imupacket, nav *navpacket, gps *gpspacket, imu *imupacket, nav *navpacket,
servo *servopacket, health *healthpacket ); servo *servopacket, health *healthpacket,
bool ignore_checksum );
// load the named file into internal buffers // load the named file into internal buffers
bool load( const string &file ); bool load( const string &file, bool ignore_checksum );
inline int gps_size() const { return gps_data.size(); } inline int gps_size() const { return gps_data.size(); }
inline int imu_size() const { return imu_data.size(); } inline int imu_size() const { return imu_data.size(); }

9
utils/GPSsmooth/UGear_main.cxx
View file

@ -66,6 +66,8 @@ double skip = 0.0;
bool inited = false; bool inited = false;
bool ignore_checksum = false;
// The function htond is defined this way due to the way some // The function htond is defined this way due to the way some
// processors and OSes treat floating point values. Some will raise // processors and OSes treat floating point values. Some will raise
@ -313,6 +315,7 @@ void usage( const string &argv0 ) {
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; cout << "\t[ --skip-seconds <seconds> ]" << endl;
cout << "\t[ --ignore-checksum ]" << endl;
} }
@ -400,6 +403,8 @@ int main( int argc, char **argv ) {
usage( argv[0] ); usage( argv[0] );
exit( -1 ); exit( -1 );
} }
} else if ( strcmp( argv[i], "--ignore-checksum" ) == 0 ) {
ignore_checksum = true;
} else { } else {
usage( argv[0] ); usage( argv[0] );
exit( -1 ); exit( -1 );
@ -447,7 +452,7 @@ int main( int argc, char **argv ) {
if ( infile.length() ) { if ( infile.length() ) {
// Load data from a track data // Load data from a track data
track.load( infile ); track.load( infile, ignore_checksum );
cout << "Loaded " << track.gps_size() << " gps records." << endl; cout << "Loaded " << track.gps_size() << " gps records." << endl;
cout << "Loaded " << track.imu_size() << " imu records." << endl; cout << "Loaded " << track.imu_size() << " imu records." << endl;
cout << "Loaded " << track.nav_size() << " nav records." << endl; cout << "Loaded " << track.nav_size() << " nav records." << endl;
@ -698,7 +703,7 @@ int main( int argc, char **argv ) {
// cout << "looking for next message ..." << endl; // cout << "looking for next message ..." << endl;
int id = track.next_message( &input, &output, &gpspacket, int id = track.next_message( &input, &output, &gpspacket,
&imupacket, &navpacket, &servopacket, &imupacket, &navpacket, &servopacket,
&healthpacket ); &healthpacket, ignore_checksum );
// cout << "message id = " << id << endl; // cout << "message id = " << id << endl;
count++; count++;