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

8
utils/GPSsmooth/UGear.hxx
View file

@ -99,13 +99,15 @@ public:
// returns id # if a valid message found.
int next_message( SGIOChannel *ch, SGIOChannel *log,
gps *gpspacket, imu *imupacket, nav *navpacket,
servo *servopacket, health * healthpacket );
servo *servopacket, health * healthpacket,
bool ignore_checksum );
int next_message( SGSerialPort *serial, SGIOChannel *log,
gps *gpspacket, imu *imupacket, nav *navpacket,
servo *servopacket, health *healthpacket );
servo *servopacket, health *healthpacket,
bool ignore_checksum );
// 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 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 ignore_checksum = false;
// The function htond is defined this way due to the way some
// 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[ --altitude-offset <meters> ]" << 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] );
exit( -1 );
}
} else if ( strcmp( argv[i], "--ignore-checksum" ) == 0 ) {
ignore_checksum = true;
} else {
usage( argv[0] );
exit( -1 );
@ -447,7 +452,7 @@ int main( int argc, char **argv ) {
if ( infile.length() ) {
// 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.imu_size() << " imu 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;
int id = track.next_message( &input, &output, &gpspacket,
&imupacket, &navpacket, &servopacket,
&healthpacket );
&healthpacket, ignore_checksum );
// cout << "message id = " << id << endl;
count++;