X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=utils%2FGPSsmooth%2FUGear.cxx;h=604926c19dbd9b917a4a2214d7ab714dac53d205;hb=46ec7a6ff78b0718c6eb09e5abb45c67c153d2f5;hp=52b3c34e5e39646bc70eef878bbeb9aaa5d885c6;hpb=06dc1d6afafea0caf5138f70b3ad1961aae692f9;p=flightgear.git diff --git a/utils/GPSsmooth/UGear.cxx b/utils/GPSsmooth/UGear.cxx index 52b3c34e5..604926c19 100644 --- a/utils/GPSsmooth/UGear.cxx +++ b/utils/GPSsmooth/UGear.cxx @@ -1,28 +1,34 @@ #ifdef HAVE_CONFIG_H # include -#endif +#endif #include +#include #include #include #include +#include #include #include #include #include "UGear.hxx" -SG_USING_STD(cout); -SG_USING_STD(endl); +using std::cout; +using std::endl; #define START_OF_MSG0 147 #define START_OF_MSG1 224 -UGEARTrack::UGEARTrack() {}; -UGEARTrack::~UGEARTrack() {}; +UGTrack::UGTrack(): + sg_swap(false) +{ +}; + +UGTrack::~UGTrack() {}; // swap the 1st 4 bytes with the last 4 bytes of a stargate double so @@ -49,8 +55,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; @@ -66,12 +77,12 @@ static bool validate_cksum( uint8_t id, uint8_t size, char *buf, c0 += (uint8_t)buf[i]; c1 += c0; // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 - // << " [" << (unsigned int)buf[i] << "]" << endl; + // << " [" << (unsigned int)(uint8_t)buf[i] << "]" << endl; } - cout << "c0 = " << (unsigned int)c0 << " (" << (unsigned int)cksum0 - << ") c1 = " << (unsigned int)c1 << " (" << (unsigned int)cksum1 - << ")" << endl; + // cout << "c0 = " << (unsigned int)c0 << " (" << (unsigned int)cksum0 + // << ") c1 = " << (unsigned int)c1 << " (" << (unsigned int)cksum1 + // << ")" << endl; if ( c0 == cksum0 && c1 == cksum1 ) { return true; @@ -81,74 +92,92 @@ static bool validate_cksum( uint8_t id, uint8_t size, char *buf, } -void UGEARTrack::parse_msg( const int id, char *buf, +void UGTrack::parse_msg( const int id, char *buf, struct gps *gpspacket, imu *imupacket, - nav *navpacket, servo *servopacket ) + nav *navpacket, servo *servopacket, + health *healthpacket ) { if ( id == GPS_PACKET ) { *gpspacket = *(struct gps *)buf; - gpspacket->lat = sg_swap_double( (uint8_t *)buf, 0 ); - gpspacket->lon = 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 ); + if ( sg_swap ) { + gpspacket->time = sg_swap_double( (uint8_t *)buf, 0 ); + gpspacket->lat = sg_swap_double( (uint8_t *)buf, 8 ); + gpspacket->lon = sg_swap_double( (uint8_t *)buf, 16 ); + gpspacket->alt = sg_swap_double( (uint8_t *)buf, 24 ); + gpspacket->vn = sg_swap_double( (uint8_t *)buf, 32 ); + gpspacket->ve = sg_swap_double( (uint8_t *)buf, 40 ); + gpspacket->vd = sg_swap_double( (uint8_t *)buf, 48 ); + gpspacket->ITOW = 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); + if ( sg_swap ) { + imupacket->time = sg_swap_double( (uint8_t *)buf, 0 ); + imupacket->p = sg_swap_double( (uint8_t *)buf, 8 ); + imupacket->q = sg_swap_double( (uint8_t *)buf, 16 ); + imupacket->r = sg_swap_double( (uint8_t *)buf, 24 ); + imupacket->ax = sg_swap_double( (uint8_t *)buf, 32 ); + imupacket->ay = sg_swap_double( (uint8_t *)buf, 40 ); + imupacket->az = sg_swap_double( (uint8_t *)buf, 48 ); + imupacket->hx = sg_swap_double( (uint8_t *)buf, 56 ); + imupacket->hy = sg_swap_double( (uint8_t *)buf, 64 ); + imupacket->hz = sg_swap_double( (uint8_t *)buf, 72 ); + imupacket->Ps = sg_swap_double( (uint8_t *)buf, 80 ); + imupacket->Pt = sg_swap_double( (uint8_t *)buf, 88 ); + imupacket->phi = sg_swap_double( (uint8_t *)buf, 96 ); + imupacket->the = sg_swap_double( (uint8_t *)buf, 104 ); + imupacket->psi = sg_swap_double( (uint8_t *)buf, 112 ); + } + // printf("imu.time = %.4f size = %d\n", imupacket->time, sizeof(struct imu)); } else if ( id == NAV_PACKET ) { *navpacket = *(struct nav *)buf; - navpacket->lon = sg_swap_double( (uint8_t *)buf, 0 ); - navpacket->lat = sg_swap_double( (uint8_t *)buf, 8 ); - navpacket->alt = sg_swap_double( (uint8_t *)buf, 16 ); - navpacket->vn = sg_swap_double( (uint8_t *)buf, 24 ); - navpacket->ve = sg_swap_double( (uint8_t *)buf, 32 ); - navpacket->vd = sg_swap_double( (uint8_t *)buf, 40 ); - navpacket->time = sg_swap_double( (uint8_t *)buf, 52 ); + if ( sg_swap ) { + navpacket->time = sg_swap_double( (uint8_t *)buf, 0 ); + navpacket->lat = sg_swap_double( (uint8_t *)buf, 8 ); + navpacket->lon = sg_swap_double( (uint8_t *)buf, 16 ); + navpacket->alt = sg_swap_double( (uint8_t *)buf, 24 ); + navpacket->vn = sg_swap_double( (uint8_t *)buf, 32 ); + navpacket->ve = sg_swap_double( (uint8_t *)buf, 40 ); + navpacket->vd = sg_swap_double( (uint8_t *)buf, 48 ); + } } 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); + if ( sg_swap ) { + servopacket->time = sg_swap_double( (uint8_t *)buf, 0 ); + } + // printf("servo time = %.3f %d %d\n", servopacket->time, servopacket->chn[0], servopacket->chn[1]); + } else if ( id == HEALTH_PACKET ) { + *healthpacket = *(struct health *)buf; + if ( sg_swap ) { + healthpacket->time = sg_swap_double( (uint8_t *)buf, 0 ); + } } else { cout << "unknown id = " << id << endl; } } -// load the specified file, return the number of records loaded -bool UGEARTrack::load( const string &file ) { +// load the named stream log file into internal buffers +bool UGTrack::load_stream( const string &file, bool ignore_checksum ) { int count = 0; gps gpspacket; imu imupacket; nav navpacket; servo servopacket; + health healthpacket; double gps_time = 0; double imu_time = 0; double nav_time = 0; double servo_time = 0; + double health_time = 0; gps_data.clear(); imu_data.clear(); nav_data.clear(); servo_data.clear(); + health_data.clear(); // open the file SGFile input( file ); @@ -160,7 +189,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 ); + &navpacket, &servopacket, &healthpacket, + ignore_checksum ); count++; if ( id == GPS_PACKET ) { @@ -168,28 +198,35 @@ bool UGEARTrack::load( const string &file ) { gps_data.push_back( gpspacket ); gps_time = gpspacket.time; } else { - cout << "oops att back in time" << endl; + cout << "oops gps back in time: " << gpspacket.time << " " << gps_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; + cout << "oops imu 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; + cout << "oops nav 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 << "oops servo back in time" << endl; + } + } else if ( id == HEALTH_PACKET ) { + if ( healthpacket.time > health_time ) { + health_data.push_back( healthpacket ); + health_time = healthpacket.time; + } else { + cout << "oops health back in time" << endl; } } } @@ -199,6 +236,108 @@ bool UGEARTrack::load( const string &file ) { } +// load the named stream log file into internal buffers +bool UGTrack::load_flight( const string &path ) { + gps gpspacket; + imu imupacket; + nav navpacket; + servo servopacket; + health healthpacket; + + gps_data.clear(); + imu_data.clear(); + nav_data.clear(); + servo_data.clear(); + health_data.clear(); + + gzFile fgps = NULL; + gzFile fimu = NULL; + gzFile fnav = NULL; + gzFile fservo = NULL; + gzFile fhealth = NULL; + + SGPath file; + int size; + + // open the gps file + file = path; file.append( "gps.dat.gz" ); + std::string fdata = file.local8BitStr(); + + if ( (fgps = gzopen( fdata.c_str(), "r" )) == NULL ) { + printf("Cannot open %s\n", fdata.c_str()); + return false; + } + + size = sizeof( struct gps ); + printf("gps size = %d\n", size); + while ( gzread( fgps, &gpspacket, size ) == size ) { + gps_data.push_back( gpspacket ); + } + + // open the imu file + file = path; file.append( "imu.dat.gz" ); + fdata = file.local8BitStr(); + if ( (fimu = gzopen( fdata.c_str(), "r" )) == NULL ) { + printf("Cannot open %s\n", fdata.c_str()); + return false; + } + + size = sizeof( struct imu ); + printf("imu size = %d\n", size); + while ( gzread( fimu, &imupacket, size ) == size ) { + imu_data.push_back( imupacket ); + } + + // open the nav file + file = path; file.append( "nav.dat.gz" ); + fdata = file.local8BitStr(); + + if ( (fnav = gzopen( fdata.c_str(), "r" )) == NULL ) { + printf("Cannot open %s\n", fdata.c_str()); + return false; + } + + size = sizeof( struct nav ); + printf("nav size = %d\n", size); + while ( gzread( fnav, &navpacket, size ) == size ) { + // printf("%.4f %.4f\n", navpacket.lat, navpacket.lon); + nav_data.push_back( navpacket ); + } + + // open the servo file + file = path; file.append( "servo.dat.gz" ); + fdata = file.local8BitStr(); + + if ( (fservo = gzopen( fdata.c_str(), "r" )) == NULL ) { + printf("Cannot open %s\n", fdata.c_str()); + return false; + } + + size = sizeof( struct servo ); + printf("servo size = %d\n", size); + while ( gzread( fservo, &servopacket, size ) == size ) { + servo_data.push_back( servopacket ); + } + + // open the health file + file = path; file.append( "health.dat.gz" ); + fdata = file.local8BitStr(); + + if ( (fhealth = gzopen( fdata.c_str(), "r" )) == NULL ) { + printf("Cannot open %s\n", fdata.c_str()); + return false; + } + + size = sizeof( struct health ); + printf("health size = %d\n", size); + while ( gzread( fhealth, &healthpacket, size ) == size ) { + health_data.push_back( healthpacket ); + } + + 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 ) { @@ -230,21 +369,24 @@ 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, +int UGTrack::next_message( SGIOChannel *ch, SGIOChannel *log, gps *gpspacket, imu *imupacket, nav *navpacket, - servo *servopacket ) + servo *servopacket, health *healthpacket, + bool ignore_checksum ) { char tmpbuf[256]; char savebuf[256]; @@ -255,14 +397,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(); } @@ -271,9 +414,10 @@ 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]; - cout << "message = " << (int)id << " size = " << (int)size << endl; + 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 if ( ch->get_type() == sgFileType ) { @@ -292,11 +436,14 @@ 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]; - - if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) { - parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket ); + 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, ignore_checksum ) ) + { + parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket, + healthpacket ); return id; } @@ -306,38 +453,44 @@ 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 ) +int UGTrack::next_message( SGSerialPort *serial, SGIOChannel *log, + gps *gpspacket, imu *imupacket, nav *navpacket, + servo *servopacket, health *healthpacket, + bool ignore_checksum ) { char tmpbuf[256]; char savebuf[256]; - int result = 0; // cout << "in next_message()" << endl; bool myeof = false; // scan for sync characters + int scan_count = 0; uint8_t sync0, sync1; - result = serial_read( serial, log, tmpbuf, 2 ); + serial_read( serial, log, tmpbuf, 2 ); sync0 = (unsigned char)tmpbuf[0]; sync1 = (unsigned char)tmpbuf[1]; while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) { + scan_count++; sync0 = sync1; serial_read( serial, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0]; - cout << "scanning for start of message " - << (unsigned int)sync0 << " " << (unsigned int)sync1 - << endl; + // cout << "scanning for start of message " + // << (unsigned int)sync0 << " " << (unsigned int)sync1 + // << endl; } + if ( scan_count > 0 ) { + cout << "found start of message after discarding " << scan_count + << " bytes" << endl; + } // cout << "found start of message ..." << endl; // read message id and size serial_read( serial, 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; + // cout << "message = " << (int)id << " size = " << (int)size << endl; // load message serial_read( serial, log, savebuf, size ); @@ -348,9 +501,11 @@ int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log, 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 ); + + if ( validate_cksum( id, size, savebuf, cksum0, cksum1, ignore_checksum ) ) + { + parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket, + healthpacket ); return id; } @@ -358,7 +513,7 @@ int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log, cout << "Check sum failure!" << endl; return -1; - + } @@ -407,9 +562,9 @@ imu UGEARInterpIMU( const imu A, const imu B, const double 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.phi = interp(A.phi, B.phi, percent, true); + p.the = interp(A.the, B.the, percent, true); + p.psi = interp(A.psi, B.psi, percent, true); p.err_type = A.err_type; return p; @@ -442,3 +597,13 @@ servo UGEARInterpSERVO( const servo A, const servo B, const double percent ) return p; } + +health UGEARInterpHEALTH( const health A, const health B, const double percent ) +{ + health p; + p.command_sequence = B.command_sequence; + p.time = interp(A.time, B.time, percent); + + return p; +} +