X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=utils%2FGPSsmooth%2FMIDG-II.cxx;h=fa3ce462d600e775bc16b3c2d8416cbc08346161;hb=79e93918e082e25870061047635c331753a71400;hp=f950a1139f50f741cbef8f23290f2c8b8f31d4ea;hpb=2f98caeba8da29c93a2cac921f36fc8616ea683b;p=flightgear.git diff --git a/utils/GPSsmooth/MIDG-II.cxx b/utils/GPSsmooth/MIDG-II.cxx index f950a1139..fa3ce462d 100644 --- a/utils/GPSsmooth/MIDG-II.cxx +++ b/utils/GPSsmooth/MIDG-II.cxx @@ -1,36 +1,43 @@ -#include +#ifdef HAVE_CONFIG_H +# include +#endif + +#include -#include +#include #include #include #include #include #include +#include #include "MIDG-II.hxx" -SG_USING_STD(cout); -SG_USING_STD(endl); +using std::cout; +using std::endl; MIDGTrack::MIDGTrack() {}; MIDGTrack::~MIDGTrack() {}; - - +/* + * Unused function + */ +#if(0) static uint32_t read_swab( char *buf, size_t offset, size_t size ) { uint32_t result = 0; 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 ); } } @@ -46,6 +53,7 @@ static uint32_t read_swab( char *buf, size_t offset, size_t size ) { return result; } +#endif static bool validate_cksum( uint8_t id, uint8_t size, char *buf, @@ -56,13 +64,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 @@ -79,6 +91,10 @@ static bool validate_cksum( uint8_t id, uint8_t size, char *buf, void MIDGTrack::parse_msg( const int id, char *buf, MIDGpos *pos, MIDGatt *att ) { +/* + * Completely unused parser results. Removed from compiling to remove the warnings + */ +#if(0) if ( id == 1 ) { uint32_t ts; uint16_t status; @@ -89,7 +105,7 @@ 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; - + // status status = (uint16_t)read_swab( buf, 4, 2 ); // cout << " status = " << status << endl; @@ -231,7 +247,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; @@ -303,18 +319,19 @@ void MIDGTrack::parse_msg( const int id, char *buf, MIDGpos *pos, MIDGatt *att ) // position dop pdop = (uint16_t)read_swab( buf, 32, 2 ); // cout << " pdop = " << pdop << endl; - + // position accuracy pacc = (uint16_t)read_swab( buf, 34, 2 ); // cout << " pacc = " << pacc << endl; - + // speed accuracy sacc = (uint16_t)read_swab( buf, 36, 2 ); // cout << " sacc = " << sacc << endl; - + } else { cout << "unknown id = " << id << endl; } +#endif } @@ -340,7 +357,7 @@ bool MIDGTrack::load( const string &file ) { while ( ! input.eof() ) { // cout << "looking for next message ..." << endl; - int id = next_message( &input, &pos, &att ); + int id = next_message( &input, NULL, &pos, &att ); count++; if ( id == 10 ) { @@ -365,34 +382,96 @@ bool MIDGTrack::load( const string &file ) { } +// 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(); + } + } + + 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, MIDGpos *pos, MIDGatt *att ) { +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; - ch->read( tmpbuf, 1 ); sync0 = (unsigned char)tmpbuf[0]; - ch->read( tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0]; - while ( (sync0 != 129 || sync1 != 161) && !ch->eof() ) { + 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; - ch->read( tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0]; + 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 - ch->read( tmpbuf, 1 ); uint8_t id = (unsigned char)tmpbuf[0]; - ch->read( tmpbuf, 1 ); uint8_t size = (unsigned char)tmpbuf[0]; + 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 - ch->read( savebuf, size ); + 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 - ch->read( tmpbuf, 1 ); uint8_t cksum0 = (unsigned char)tmpbuf[0]; - ch->read( tmpbuf, 1 ); uint8_t cksum1 = (unsigned char)tmpbuf[0]; - + 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, savebuf, cksum0, cksum1 ) ) { parse_msg( id, savebuf, pos, att ); return id; @@ -403,6 +482,64 @@ int MIDGTrack::next_message( SGIOChannel *ch, MIDGpos *pos, MIDGatt *att ) { } +// 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]; + + cout << "in next_message()" << endl; + + bool myeof = false; + + // scan for sync characters + uint8_t sync0, sync1; + 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; + } + + 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; + + +} + + static double interp( double a, double b, double p, bool rotational = false ) { double diff = b - a; if ( rotational ) {