From 651a21f9d5d80e12820fc6c4b790caa72ce52ff2 Mon Sep 17 00:00:00 2001 From: curt Date: Sat, 11 Nov 2006 18:43:17 +0000 Subject: [PATCH] Initial revision of code to read MicroGear serial output and parse, interpolate, and feed to FlightGear either in real time or replaying the data file. --- utils/GPSsmooth/MIDG-II.cxx | 97 ++++- utils/GPSsmooth/MIDG-II.hxx | 3 + utils/GPSsmooth/MIDG_main.cxx | 23 +- utils/GPSsmooth/Makefile.am | 12 +- utils/GPSsmooth/UGear.cxx | 437 ++++++++++++++++++++ utils/GPSsmooth/UGear.hxx | 147 +++++++ utils/GPSsmooth/UGear_main.cxx | 706 +++++++++++++++++++++++++++++++++ 7 files changed, 1406 insertions(+), 19 deletions(-) create mode 100644 utils/GPSsmooth/UGear.cxx create mode 100644 utils/GPSsmooth/UGear.hxx create mode 100644 utils/GPSsmooth/UGear_main.cxx diff --git a/utils/GPSsmooth/MIDG-II.cxx b/utils/GPSsmooth/MIDG-II.cxx index 0bb2f79a7..91338d913 100644 --- a/utils/GPSsmooth/MIDG-II.cxx +++ b/utils/GPSsmooth/MIDG-II.cxx @@ -377,11 +377,12 @@ bool MIDGTrack::load( const string &file ) { int myread( SGIOChannel *ch, SGIOChannel *log, char *buf, int length ) { bool myeof = false; int result = 0; - while ( result != length && !myeof ) { - result = ch->read( buf, length ); - if ( ch->get_type() == sgFileType ) { - myeof = ((SGFile *)ch)->eof(); - } + 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 ) { @@ -391,6 +392,23 @@ int myread( SGIOChannel *ch, SGIOChannel *log, char *buf, int length ) { 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 ) @@ -409,7 +427,9 @@ int MIDGTrack::next_message( SGIOChannel *ch, SGIOChannel *log, while ( (sync0 != 129 || sync1 != 161) && !myeof ) { sync0 = sync1; myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0]; - // cout << "scanning for start of message, 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(); } @@ -429,9 +449,13 @@ int MIDGTrack::next_message( SGIOChannel *ch, SGIOChannel *log, 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 @@ -443,11 +467,70 @@ int MIDGTrack::next_message( SGIOChannel *ch, SGIOChannel *log, return id; } - // cout << "Check sum failure!" << endl; + 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; + } + + 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 ) { diff --git a/utils/GPSsmooth/MIDG-II.hxx b/utils/GPSsmooth/MIDG-II.hxx index e35ffa96a..2892faa3e 100644 --- a/utils/GPSsmooth/MIDG-II.hxx +++ b/utils/GPSsmooth/MIDG-II.hxx @@ -14,6 +14,7 @@ #include #include +#include SG_USING_STD(cout); SG_USING_STD(endl); @@ -133,6 +134,8 @@ public: // returns id # if a valid message found. int next_message( SGIOChannel *ch, SGIOChannel *log, MIDGpos *pos, MIDGatt *att ); + int next_message( SGSerialPort *serial, SGIOChannel *log, + MIDGpos *pos, MIDGatt *att ); // load the named file into internal buffers bool load( const string &file ); diff --git a/utils/GPSsmooth/MIDG_main.cxx b/utils/GPSsmooth/MIDG_main.cxx index 6cbfdce10..29a8cde7b 100644 --- a/utils/GPSsmooth/MIDG_main.cxx +++ b/utils/GPSsmooth/MIDG_main.cxx @@ -519,7 +519,7 @@ int main( int argc, char **argv ) { // << endl; // cout << (double)current_time << " " << pos.lat_deg << ", " // << pos.lon_deg << " " << att.yaw_deg << endl; - if ( pos.lat_deg > 50 ) { + if ( pos.lat_deg > -500 ) { printf( "%.3f %.4f %.4f %.1f %.2f %.2f %.2f\n", current_time, pos.lat_deg, pos.lon_deg, pos.altitude_msl, @@ -569,8 +569,8 @@ int main( int argc, char **argv ) { uint32_t att_time = 1; // open the serial port device - SGSerial input( serialdev, "115200" ); - if ( !input.open( SG_IO_IN ) ) { + SGSerialPort input( serialdev, 115200 ); + if ( !input.is_enabled() ) { cout << "Cannot open: " << serialdev << endl; return false; } @@ -587,9 +587,10 @@ int main( int argc, char **argv ) { return false; } - while ( ! input.eof() ) { + while ( input.is_enabled() ) { // cout << "looking for next message ..." << endl; int id = track.next_message( &input, &output, &pos, &att ); + cout << "message id = " << id << endl; count++; if ( id == 10 ) { @@ -608,13 +609,13 @@ int main( int argc, char **argv ) { } } - if ( pos.lat_deg > 50 ) { - printf( "%.3f %.4f %.4f %.1f %.2f %.2f %.2f\n", - current_time, - pos.lat_deg, pos.lon_deg, pos.altitude_msl, - att.yaw_rad * 180.0 / SG_PI, - att.pitch_rad * 180.0 / SG_PI, - att.roll_rad * 180.0 / SG_PI ); + if ( pos.lat_deg > -500 ) { + // printf( "%.3f %.4f %.4f %.1f %.2f %.2f %.2f\n", + // current_time, + // pos.lat_deg, pos.lon_deg, pos.altitude_msl, + // att.yaw_rad * 180.0 / SG_PI, + // att.pitch_rad * 180.0 / SG_PI, + // att.roll_rad * 180.0 / SG_PI ); } send_data( pos, att ); diff --git a/utils/GPSsmooth/Makefile.am b/utils/GPSsmooth/Makefile.am index 33bd55987..326e6317c 100644 --- a/utils/GPSsmooth/Makefile.am +++ b/utils/GPSsmooth/Makefile.am @@ -1,4 +1,4 @@ -noinst_PROGRAMS = GPSsmooth MIDGsmooth +noinst_PROGRAMS = GPSsmooth MIDGsmooth UGsmooth GPSsmooth_SOURCES = \ gps.cxx gps.hxx \ @@ -17,4 +17,14 @@ MIDGsmooth_LDADD = \ -lplibnet -lplibul \ $(joystick_LIBS) $(network_LIBS) $(base_LIBS) -lz +UGsmooth_SOURCES = \ + UGear.cxx UGear.hxx \ + UGear_main.cxx + +UGsmooth_LDADD = \ + -lsgio -lsgserial -lsgtiming -lsgmath -lsgbucket -lsgmisc -lsgdebug \ + -lplibnet -lplibul \ + $(joystick_LIBS) $(network_LIBS) $(base_LIBS) -lz + + INCLUDES = -I$(top_srcdir)/src diff --git a/utils/GPSsmooth/UGear.cxx b/utils/GPSsmooth/UGear.cxx new file mode 100644 index 000000000..59dabc2cc --- /dev/null +++ b/utils/GPSsmooth/UGear.cxx @@ -0,0 +1,437 @@ +#ifdef HAVE_CONFIG_H +# include +#endif + +#include + +#include +#include +#include +#include +#include +#include + +#include "UGear.hxx" + +SG_USING_STD(cout); +SG_USING_STD(endl); + + +#define START_OF_MSG0 147 +#define START_OF_MSG1 224 + + +UGEARTrack::UGEARTrack() {}; +UGEARTrack::~UGEARTrack() {}; + + +// swap the 1st 4 bytes with the last 4 bytes of a stargate double so +// it matches the PC representation +static double sg_swap_double( uint8_t *buf, size_t offset ) { + double *result; + uint8_t tmpbuf[10]; + + for ( size_t i = 0; i < 4; ++i ) { + tmpbuf[i] = buf[offset + i + 4]; + } + for ( size_t i = 0; i < 4; ++i ) { + tmpbuf[i + 4] = buf[offset + i]; + } + + // for ( size_t i = 0; i < 8; ++i ) { + // printf("%d ", tmpbuf[i]); + // } + // printf("\n"); + + result = (double *)tmpbuf; + return *result; +} + + +static bool validate_cksum( uint8_t id, uint8_t size, char *buf, + uint8_t cksum0, uint8_t cksum1 ) +{ + uint8_t c0 = 0; + uint8_t c1 = 0; + + 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 + // << ") c1 = " << (unsigned int)c1 << " (" << (unsigned int)cksum1 + // << ")" << endl; + + if ( c0 == cksum0 && c1 == cksum1 ) { + return true; + } else { + return false; + } +} + + +void UGEARTrack::parse_msg( const int id, char *buf, + struct gps *gpspacket, imu *imupacket, + nav *navpacket, servo *servopacket ) +{ + if ( id == GPS_PACKET ) { + *gpspacket = *(struct gps *)buf; + gpspacket->lon = sg_swap_double( (uint8_t *)buf, 0 ); + gpspacket->lat = sg_swap_double( (uint8_t *)buf, 8 ); + gpspacket->alt = sg_swap_double( (uint8_t *)buf, 16 ); + gpspacket->vn = sg_swap_double( (uint8_t *)buf, 24 ); + gpspacket->ve = sg_swap_double( (uint8_t *)buf, 32 ); + gpspacket->vd = sg_swap_double( (uint8_t *)buf, 40 ); + gpspacket->time = sg_swap_double( (uint8_t *)buf, 56 ); + } else if ( id == IMU_PACKET ) { + *imupacket = *(struct imu *)buf; + imupacket->p = sg_swap_double( (uint8_t *)buf, 0 ); + imupacket->q = sg_swap_double( (uint8_t *)buf, 8 ); + imupacket->r = sg_swap_double( (uint8_t *)buf, 16 ); + imupacket->ax = sg_swap_double( (uint8_t *)buf, 24 ); + imupacket->ay = sg_swap_double( (uint8_t *)buf, 32 ); + imupacket->az = sg_swap_double( (uint8_t *)buf, 40 ); + imupacket->hx = sg_swap_double( (uint8_t *)buf, 48 ); + imupacket->hy = sg_swap_double( (uint8_t *)buf, 56 ); + imupacket->hz = sg_swap_double( (uint8_t *)buf, 64 ); + imupacket->Ps = sg_swap_double( (uint8_t *)buf, 72 ); + imupacket->Pt = sg_swap_double( (uint8_t *)buf, 80 ); + imupacket->phi = sg_swap_double( (uint8_t *)buf, 88 ); + imupacket->the = sg_swap_double( (uint8_t *)buf, 96 ); + imupacket->psi = sg_swap_double( (uint8_t *)buf, 104 ); + imupacket->time = sg_swap_double( (uint8_t *)buf, 116 ); + // printf("imu.time = %.4f\n", imupacket->time); + } else if ( id == NAV_PACKET ) { + *navpacket = *(struct nav *)buf; + } else if ( id == SERVO_PACKET ) { + *servopacket = *(struct servo *)buf; + servopacket->time = sg_swap_double( (uint8_t *)buf, 20 ); + // printf("servo time = %.3f\n", servopacket->time); + } else { + cout << "unknown id = " << id << endl; + } +} + + +// load the specified file, return the number of records loaded +bool UGEARTrack::load( const string &file ) { + int count = 0; + + gps gpspacket; + imu imupacket; + nav navpacket; + servo servopacket; + + double gps_time = 0; + double imu_time = 0; + double nav_time = 0; + double servo_time = 0; + + gps_data.clear(); + imu_data.clear(); + nav_data.clear(); + servo_data.clear(); + + // open the file + SGFile input( file ); + if ( !input.open( SG_IO_IN ) ) { + cout << "Cannot open file: " << file << endl; + return false; + } + + while ( ! input.eof() ) { + // cout << "looking for next message ..." << endl; + int id = next_message( &input, NULL, &gpspacket, &imupacket, + &navpacket, &servopacket ); + count++; + + if ( id == GPS_PACKET ) { + if ( gpspacket.time > gps_time ) { + gps_data.push_back( gpspacket ); + gps_time = gpspacket.time; + } else { + cout << "oops att back in time" << endl; + } + } else if ( id == IMU_PACKET ) { + if ( imupacket.time > imu_time ) { + imu_data.push_back( imupacket ); + imu_time = imupacket.time; + } else { + cout << "oops pos back in time" << endl; + } + } else if ( id == NAV_PACKET ) { + if ( navpacket.time > nav_time ) { + nav_data.push_back( navpacket ); + nav_time = navpacket.time; + } else { + cout << "oops pos back in time" << endl; + } + } else if ( id == SERVO_PACKET ) { + if ( servopacket.time > servo_time ) { + servo_data.push_back( servopacket ); + servo_time = servopacket.time; + } 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(); + } + } + + 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 UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log, + gps *gpspacket, imu *imupacket, nav *navpacket, + servo *servopacket ) +{ + 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 != 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; + if ( ch->get_type() == sgFileType ) { + myeof = ((SGFile *)ch)->eof(); + } + } + + 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]; + cout << "message = " << (int)id << " size = " << (int)size << endl; + + // 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 + 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, gpspacket, imupacket, navpacket, servopacket ); + return id; + } + + cout << "Check sum failure!" << endl; + return -1; +} + + +// 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 ) +{ + 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 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !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]; + // cout << "cksum0 = " << (int)cksum0 << " cksum1 = " << (int)cksum1 + // << endl; + + if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) { + parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket ); + + // + // 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 ) { + // special handling of rotational data + if ( diff > SGD_PI ) { + diff -= SGD_2PI; + } else if ( diff < -SGD_PI ) { + diff += SGD_2PI; + } + } + return a + diff * p; +} + + +gps UGEARInterpGPS( const gps A, const gps B, const double percent ) +{ + gps p; + p.time = interp(A.time, B.time, percent); + p.lat = interp(A.lat, B.lat, percent); + p.lon = interp(A.lon, B.lon, percent); + p.alt = interp(A.alt, B.alt, percent); + p.ve = interp(A.ve, B.ve, percent); + p.vn = interp(A.vn, B.vn, percent); + p.vd = interp(A.vd, B.vd, percent); + p.ITOW = (int)interp(A.ITOW, B.ITOW, percent); + p.err_type = A.err_type; + + return p; +} + +imu UGEARInterpIMU( const imu A, const imu B, const double percent ) +{ + imu p; + p.time = interp(A.time, B.time, percent); + p.p = interp(A.p, B.p, percent); + p.q = interp(A.q, B.q, percent); + p.r = interp(A.r, B.r, percent); + p.ax = interp(A.ax, B.ax, percent); + p.ay = interp(A.ay, B.ay, percent); + p.az = interp(A.az, B.az, percent); + p.hx = interp(A.hx, B.hx, percent); + p.hy = interp(A.hy, B.hy, percent); + p.hz = interp(A.hz, B.hz, percent); + p.Ps = interp(A.Ps, B.Ps, percent); + p.Pt = interp(A.Pt, B.Pt, percent); + p.phi = interp(A.phi, B.phi, percent); + p.the = interp(A.the, B.the, percent); + p.psi = interp(A.psi, B.psi, percent); + p.err_type = A.err_type; + + return p; +} + +nav UGEARInterpNAV( const nav A, const nav B, const double percent ) +{ + nav p; + p.time = interp(A.time, B.time, percent); + p.lat = interp(A.lat, B.lat, percent); + p.lon = interp(A.lon, B.lon, percent); + p.alt = interp(A.alt, B.alt, percent); + p.ve = interp(A.ve, B.ve, percent); + p.vn = interp(A.vn, B.vn, percent); + p.vd = interp(A.vd, B.vd, percent); + p.err_type = A.err_type; + + return p; +} + + +servo UGEARInterpSERVO( const servo A, const servo B, const double percent ) +{ + servo p; + for ( int i = 0; i < 8; ++i ) { + p.chn[i] = (uint16_t)interp(A.chn[i], B.chn[i], percent); + } + p.status = A.status; + + return p; +} + diff --git a/utils/GPSsmooth/UGear.hxx b/utils/GPSsmooth/UGear.hxx new file mode 100644 index 000000000..1014f57b4 --- /dev/null +++ b/utils/GPSsmooth/UGear.hxx @@ -0,0 +1,147 @@ +#ifndef _FG_UGEAR_II_HXX +#define _FG_UGEAR_II_HXX + + +#ifdef HAVE_CONFIG_H +# include +#endif + +#include + +#include +#include +#include + +#include +#include +#include + +SG_USING_STD(cout); +SG_USING_STD(endl); +SG_USING_STD(string); +SG_USING_STD(vector); + + +enum ugPacketType { + GPS_PACKET = 0, + IMU_PACKET = 1, + NAV_PACKET = 2, + SERVO_PACKET = 3 +}; + +struct imu { + double p,q,r; /* angular velocities */ + double ax,ay,az; /* acceleration */ + double hx,hy,hz; /* magnetic field */ + double Ps,Pt; /* static/pitot pressure */ + // double Tx,Ty,Tz; /* temperature */ + double phi,the,psi; /* attitudes */ + short err_type; /* error type */ + double time; +}; + +struct gps { + double lat,lon,alt; /* gps position */ + double ve,vn,vd; /* gps velocity */ + uint16_t ITOW; + short err_type; /* error type */ + double time; +}; + +struct nav { + double lat,lon,alt; + double ve,vn,vd; + // float t; + short err_type; + double time; +}; + +struct servo { + uint16_t chn[8]; + uint8_t status; + double time; +}; + + +// Manage a saved ugear log (track file) +class UGEARTrack { + +private: + + vector gps_data; + vector imu_data; + vector