X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=utils%2FGPSsmooth%2FMIDG-II.cxx;h=91338d9133b90a6ceae7b4b919b7430cb3d24e97;hb=651a21f9d5d80e12820fc6c4b790caa72ce52ff2;hp=51b5e2eb9e5c031c365d71c4081dc7bff8534a70;hpb=7473730b857265eaa9d0d8883abbf95718f34354;p=flightgear.git diff --git a/utils/GPSsmooth/MIDG-II.cxx b/utils/GPSsmooth/MIDG-II.cxx index 51b5e2eb9..91338d913 100644 --- a/utils/GPSsmooth/MIDG-II.cxx +++ b/utils/GPSsmooth/MIDG-II.cxx @@ -1,11 +1,15 @@ -#include +#ifdef HAVE_CONFIG_H +# include +#endif -#include +#include #include +#include #include #include #include +#include #include "MIDG-II.hxx" @@ -25,11 +29,11 @@ static uint32_t read_swab( char *buf, size_t offset, size_t size ) { char *ptr = buf + offset; // MIDG data is big endian so swap if needed. - if ( ulIsLittleEndian ) { + if ( sgIsLittleEndian() ) { if ( size == 4 ) { - ulEndianSwap( (uint32_t *)ptr ); + sgEndianSwap( (uint32_t *)ptr ); } else if ( size == 2 ) { - ulEndianSwap( (uint16_t *)ptr ); + sgEndianSwap( (uint16_t *)ptr ); } } @@ -55,13 +59,17 @@ static bool validate_cksum( uint8_t id, uint8_t size, char *buf, c0 += id; c1 += c0; + // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 << endl; c0 += size; c1 += c0; + // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 << endl; for ( uint8_t i = 0; i < size; i++ ) { c0 += (uint8_t)buf[i]; c1 += c0; + // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 + // << " [" << (unsigned int)buf[i] << "]" << endl; } // cout << "c0 = " << (unsigned int)c0 << " (" << (unsigned int)cksum0 @@ -166,15 +174,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 +228,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 ); @@ -244,7 +238,7 @@ void MIDGTrack::parse_msg( const int id, char *buf, MIDGpos *pos, MIDGatt *att ) // cout << " pos = " << posx << "," << posy << "," << posz << endl; double xyz[3]; - xyz[0] = posx/100; xyz[1] = posy/100; xyz[2] = posz/100; + xyz[0] = (double)posx/100; xyz[1] = (double)posy/100; xyz[2] = (double)posz/100; double lat, lon, alt; sgCartToGeod(xyz, &lat, &lon, &alt); pos->lat_deg = lat * 180.0 / SG_PI; @@ -258,7 +252,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 +283,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 +326,208 @@ 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 ) { +bool MIDGTrack::load( const string &file ) { int count = 0; - posdata.clear(); - attdata.clear(); + MIDGpos pos; + MIDGatt att; + + uint32_t pos_time = 1; + uint32_t att_time = 1; - // openg the file - fd = fopen( file.c_str(), "r" ); + pos_data.clear(); + att_data.clear(); - if ( fd == NULL ) { + // open the file + SGFile input( file ); + if ( !input.open( SG_IO_IN ) ) { cout << "Cannot open file: " << file << endl; - return 0; + return false; } - vector tokens; - MIDGpos pos; - MIDGatt att; + while ( ! input.eof() ) { + // cout << "looking for next message ..." << endl; + int id = next_message( &input, NULL, &pos, &att ); + count++; + + 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 << "processed " << count << " messages" << endl; + return true; +} + + +// attempt to work around some system dependent issues. Our read can +// return < data than we want. +int myread( SGIOChannel *ch, SGIOChannel *log, char *buf, int length ) { + bool myeof = false; + int result = 0; + if ( !myeof ) { + result = ch->read( buf, length ); + // cout << "wanted " << length << " read " << result << " bytes" << endl; + if ( ch->get_type() == sgFileType ) { + myeof = ((SGFile *)ch)->eof(); + } + } - 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 ); + if ( result > 0 && log != NULL ) { + log->write( buf, result ); + } + + return result; +} + +// attempt to work around some system dependent issues. Our read can +// return < data than we want. +int serial_read( SGSerialPort *serial, char *buf, int length ) { + int result = 0; + int bytes_read = 0; + char *tmp = buf; + + while ( bytes_read < length ) { + result = serial->read_port( tmp, length - bytes_read ); + bytes_read += result; + tmp += result; + // cout << " read " << bytes_read << " of " << length << endl; + } + + return bytes_read; +} + +// load the next message of a real time data stream +int MIDGTrack::next_message( SGIOChannel *ch, SGIOChannel *log, + MIDGpos *pos, MIDGatt *att ) +{ + char tmpbuf[256]; + char savebuf[256]; + + // cout << "in next_message()" << endl; + + bool myeof = false; + + // 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]; + while ( (sync0 != 129 || sync1 != 161) && !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; + if ( ch->get_type() == sgFileType ) { + myeof = ((SGFile *)ch)->eof(); } + } - // cout << "start of message ..." << endl; + // cout << "found start of message ..." << endl; - // read message id and size - int id = fgetc( fd ); - int size = fgetc( fd ); - // cout << "message = " << id << " size = " << size << 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]; + // cout << "message = " << (int)id << " size = " << (int)size << endl; - // load message - char buf[256]; - fread( buf, size, 1, fd ); + // load message + if ( ch->get_type() == sgFileType ) { + int count = myread( ch, log, savebuf, size ); + if ( count != size ) { + cout << "ERROR: didn't read enough bytes!" << endl; + } + } else { +#ifdef READ_ONE_BY_ONE + for ( int i = 0; i < size; ++i ) { + myread( ch, log, tmpbuf, 1 ); savebuf[i] = tmpbuf[0]; + } +#else + myread( ch, log, savebuf, size ); +#endif + } - // read checksum - int cksum0 = fgetc( fd ); - int cksum1 = fgetc( fd ); + // 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]; - 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, savebuf, cksum0, cksum1 ) ) { + parse_msg( id, savebuf, pos, att ); + return id; + } + + cout << "Check sum failure!" << endl; + return -1; +} + + +// load the next message of a real time data stream +int MIDGTrack::next_message( SGSerialPort *serial, SGIOChannel *log, + MIDGpos *pos, MIDGatt *att ) +{ + char tmpbuf[256]; + char savebuf[256]; + int result = 0; + + cout << "in next_message()" << endl; + + bool myeof = false; + + // scan for sync characters + uint8_t sync0, sync1; + result = serial_read( serial, tmpbuf, 2 ); + sync0 = (unsigned char)tmpbuf[0]; + sync1 = (unsigned char)tmpbuf[1]; + while ( (sync0 != 129 || sync1 != 161) && !myeof ) { + sync0 = sync1; + serial_read( serial, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0]; + cout << "scanning for start of message " + << (unsigned int)sync0 << " " << (unsigned int)sync1 + << endl; } - return count; + cout << "found start of message ..." << endl; + + // read message id and size + serial_read( serial, 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 + serial_read( serial, savebuf, size ); + + // read checksum + serial_read( serial, tmpbuf, 2 ); + uint8_t cksum0 = (unsigned char)tmpbuf[0]; + uint8_t cksum1 = (unsigned char)tmpbuf[1]; + + if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) { + parse_msg( id, savebuf, pos, att ); + + // + // FIXME + // WRITE DATA TO LOG FILE + // + + return id; + } + + cout << "Check sum failure!" << endl; + return -1; + + }