From 1722d26b2f8168de07d4b5898796b2d59baa9e84 Mon Sep 17 00:00:00 2001 From: curt Date: Fri, 27 Apr 2007 15:30:26 +0000 Subject: [PATCH] Add support for a new file format which where packet type are saved out into one file per type in a simple binary conglomeration of packets with no headers or checksumming (this format is intended for local storage only, not to be transmitted on the fly over a noisy communication pipe.) --- utils/GPSsmooth/UGear.cxx | 191 ++++++++++++++++++++++++++------- utils/GPSsmooth/UGear.hxx | 44 +++++--- utils/GPSsmooth/UGear_main.cxx | 140 ++++++++++++++++-------- 3 files changed, 274 insertions(+), 101 deletions(-) diff --git a/utils/GPSsmooth/UGear.cxx b/utils/GPSsmooth/UGear.cxx index 7046e8c61..242683498 100644 --- a/utils/GPSsmooth/UGear.cxx +++ b/utils/GPSsmooth/UGear.cxx @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -21,7 +22,11 @@ SG_USING_STD(endl); #define START_OF_MSG1 224 -UGEARTrack::UGEARTrack() {}; +UGEARTrack::UGEARTrack(): + sg_swap(false) +{ +}; + UGEARTrack::~UGEARTrack() {}; @@ -93,55 +98,65 @@ void UGEARTrack::parse_msg( const int id, char *buf, { 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, 52 ); + 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 ); + } } 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->lat = sg_swap_double( (uint8_t *)buf, 0 ); - navpacket->lon = 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 ); + if ( sg_swap ) { + servopacket->time = sg_swap_double( (uint8_t *)buf, 0 ); + } // printf("servo time = %.3f\n", servopacket->time); } else if ( id == HEALTH_PACKET ) { *healthpacket = *(struct health *)buf; - healthpacket->time = sg_swap_double( (uint8_t *)buf, 16 ); + 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, bool ignore_checksum ) { +// load the named stream log file into internal buffers +bool UGEARTrack::load_stream( const string &file, bool ignore_checksum ) { int count = 0; gps gpspacket; @@ -219,6 +234,99 @@ bool UGEARTrack::load( const string &file, bool ignore_checksum ) { } +// load the named stream log file into internal buffers +bool UGEARTrack::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" ); + if ( (fgps = gzopen( file.c_str(), "r" )) == NULL ) { + printf("Cannont open %s\n", file.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" ); + if ( (fimu = gzopen( file.c_str(), "r" )) == NULL ) { + printf("Cannot open %s\n", file.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" ); + if ( (fnav = gzopen( file.c_str(), "r" )) == NULL ) { + printf("Cannot open %s\n", file.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" ); + if ( (fservo = gzopen( file.c_str(), "r" )) == NULL ) { + printf("Cannot open %s\n", file.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" ); + if ( (fhealth = gzopen( file.c_str(), "r" )) == NULL ) { + printf("Cannot open %s\n", file.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 ) { @@ -262,6 +370,7 @@ int serial_read( SGSerialPort *serial, SGIOChannel *log, 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, @@ -347,18 +456,24 @@ int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log, bool myeof = false; // scan for sync characters + int scan_count = 0; uint8_t sync0, sync1; result = 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 diff --git a/utils/GPSsmooth/UGear.hxx b/utils/GPSsmooth/UGear.hxx index 5bdba2ef5..67983c7b9 100644 --- a/utils/GPSsmooth/UGear.hxx +++ b/utils/GPSsmooth/UGear.hxx @@ -31,46 +31,46 @@ enum ugPacketType { }; struct imu { + double time; 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; + uint64_t err_type; /* error type */ }; struct gps { + double time; double lat,lon,alt; /* gps position */ double ve,vn,vd; /* gps velocity */ - uint16_t ITOW; - short err_type; /* error type */ - double time; + double ITOW; + uint64_t err_type; /* error type */ }; struct nav { + double time; double lat,lon,alt; double ve,vn,vd; // float t; - short err_type; - double time; + uint64_t err_type; }; struct servo { - uint16_t chn[8]; - uint8_t status; double time; + uint64_t chn[8]; + uint64_t status; }; struct health { + double time; float volts_raw; /* raw volt reading */ float volts; /* filtered volts */ - uint16_t est_seconds; /* estimated useful seconds remaining */ - uint8_t loadavg; /* system "1 minute" load average */ - uint8_t ahrs_hz; /* actual ahrs loop hz */ - uint8_t nav_hz; /* actual nav loop hz */ - double time; + uint32_t est_seconds; /* estimated useful seconds remaining */ + uint32_t loadavg; /* system "1 minute" load average */ + uint32_t ahrs_hz; /* actual ahrs loop hz */ + uint32_t nav_hz; /* actual nav loop hz */ }; // Manage a saved ugear log (track file) @@ -90,6 +90,10 @@ private: gps *gpspacket, imu *imupacket, nav *navpacket, servo *servopacket, health *healthpacket ); + // activate special double swap logic for non-standard stargate + // double format + bool sg_swap; + public: UGEARTrack(); @@ -106,8 +110,11 @@ public: servo *servopacket, health *healthpacket, bool ignore_checksum ); - // load the named file into internal buffers - bool load( const string &file, bool ignore_checksum ); + // load the named stream log file into internal buffers + bool load_stream( const string &file, bool ignore_checksum ); + + // load the named flight files into internal buffers + bool load_flight( const string &path ); inline int gps_size() const { return gps_data.size(); } inline int imu_size() const { return imu_data.size(); } @@ -157,6 +164,11 @@ public: } + // set stargate mode where we have to do an odd swapping of doubles to + // account for their non-standard formate + inline void set_stargate_swap_mode() { + sg_swap = true; + } }; diff --git a/utils/GPSsmooth/UGear_main.cxx b/utils/GPSsmooth/UGear_main.cxx index 46db91823..1cc2c90b2 100644 --- a/utils/GPSsmooth/UGear_main.cxx +++ b/utils/GPSsmooth/UGear_main.cxx @@ -16,7 +16,7 @@ #include #include // endian tests #include -#include +#include #include #include @@ -43,6 +43,7 @@ static int ctrls_port = 5506; // Default path static string infile = ""; +static string flight_dir = ""; static string serialdev = ""; static string outfile = ""; @@ -66,8 +67,11 @@ double skip = 0.0; bool inited = false; +bool run_real_time = true; + bool ignore_checksum = false; +bool sg_swap = false; // The function htond is defined this way due to the way some // processors and OSes treat floating point values. Some will raise @@ -309,6 +313,7 @@ void usage( const string &argv0 ) { cout << "Usage: " << argv0 << endl; cout << "\t[ --help ]" << endl; cout << "\t[ --infile " << endl; + cout << "\t[ --flight " << endl; cout << "\t[ --serial " << endl; cout << "\t[ --outfile (capture the data to a file)" << endl; cout << "\t[ --hertz ]" << endl; @@ -318,7 +323,9 @@ void usage( const string &argv0 ) { cout << "\t[ --ctrls-port ]" << endl; cout << "\t[ --altitude-offset ]" << endl; cout << "\t[ --skip-seconds ]" << endl; + cout << "\t[ --no-real-time ]" << endl; cout << "\t[ --ignore-checksum ]" << endl; + cout << "\t[ --sg-swap ]" << endl; } @@ -348,6 +355,14 @@ int main( int argc, char **argv ) { usage( argv[0] ); exit( -1 ); } + } else if ( strcmp( argv[i], "--flight" ) == 0 ) { + ++i; + if ( i < argc ) { + flight_dir = argv[i]; + } else { + usage( argv[0] ); + exit( -1 ); + } } else if ( strcmp( argv[i], "--outfile" ) == 0 ) { ++i; if ( i < argc ) { @@ -406,8 +421,12 @@ int main( int argc, char **argv ) { usage( argv[0] ); exit( -1 ); } + } else if ( strcmp( argv[i], "--no-real-time" ) == 0 ) { + run_real_time = false; } else if ( strcmp( argv[i], "--ignore-checksum" ) == 0 ) { - ignore_checksum = true; + ignore_checksum = true; + } else if ( strcmp( argv[i], "--sg-swap" ) == 0 ) { + sg_swap = true; } else { usage( argv[0] ); exit( -1 ); @@ -453,9 +472,18 @@ int main( int argc, char **argv ) { } cout << "connected outgoing ctrls socket" << endl; - if ( infile.length() ) { - // Load data from a track data - track.load( infile, ignore_checksum ); + if ( sg_swap ) { + track.set_stargate_swap_mode(); + } + + if ( infile.length() || flight_dir.length() ) { + if ( infile.length() ) { + // Load data from a stream log data file + track.load_stream( infile, ignore_checksum ); + } else if ( flight_dir.length() ) { + // Load data from a flight directory + track.load_flight( flight_dir ); + } cout << "Loaded " << track.gps_size() << " gps records." << endl; cout << "Loaded " << track.imu_size() << " imu records." << endl; cout << "Loaded " << track.nav_size() << " nav records." << endl; @@ -500,14 +528,16 @@ int main( int argc, char **argv ) { health health0, health1; health0 = health1 = track.get_healthpt( 0 ); - + + double last_lat = -999.9, last_lon = -999.9; + while ( current_time < end_time ) { // cout << "current_time = " << current_time << " end_time = " // << end_time << endl; // Advance gps pointer while ( current_time > gps1.time - && gps_count < track.gps_size() ) + && gps_count < track.gps_size() - 1 ) { gps0 = gps1; ++gps_count; @@ -519,7 +549,7 @@ int main( int argc, char **argv ) { // Advance imu pointer while ( current_time > imu1.time - && imu_count < track.imu_size() ) + && imu_count < track.imu_size() - 1 ) { imu0 = imu1; ++imu_count; @@ -531,11 +561,11 @@ int main( int argc, char **argv ) { // Advance nav pointer while ( current_time > nav1.time - && nav_count < track.nav_size() ) + && nav_count < track.nav_size() - 1 ) { nav0 = nav1; ++nav_count; - // cout << "count = " << count << endl; + // cout << "nav count = " << nav_count << endl; nav1 = track.get_navpt( nav_count ); } // cout << "pos0 = " << pos0.get_seconds() @@ -543,7 +573,7 @@ int main( int argc, char **argv ) { // Advance servo pointer while ( current_time > servo1.time - && servo_count < track.servo_size() ) + && servo_count < track.servo_size() - 1 ) { servo0 = servo1; ++servo_count; @@ -555,7 +585,7 @@ int main( int argc, char **argv ) { // Advance health pointer while ( current_time > health1.time - && health_count < track.health_size() ) + && health_count < track.health_size() - 1 ) { health0 = health1; ++health_count; @@ -630,32 +660,44 @@ int main( int argc, char **argv ) { // cout << (double)current_time << " " << pos.lat_deg << ", " // << pos.lon_deg << " " << att.yaw_deg << endl; if ( gpspacket.lat > -500 ) { - printf( "%.3f %.4f %.4f %.1f %.2f %.2f %.2f\n", - current_time, - gpspacket.lat, gpspacket.lon, gpspacket.alt, - imupacket.psi, imupacket.the, imupacket.phi ); + // printf( "%.3f %.4f %.4f %.1f %.2f %.2f %.2f\n", + // current_time, + // navpacket.lat, navpacket.lon, navpacket.alt, + // imupacket.psi, imupacket.the, imupacket.phi ); + double dlat = last_lat - navpacket.lat; + double dlon = last_lon - navpacket.lon; + double dist = sqrt( dlat*dlat + dlon*dlon ); + if ( dist > 0.0015 ) { + printf(" \n", + navpacket.lat, navpacket.lon ); + // printf(" \n"); + last_lat = navpacket.lat; + last_lon = navpacket.lon; + } } send_data( &gpspacket, &imupacket, &navpacket, &servopacket, &healthpacket ); - // Update the elapsed time. - static bool first_time = true; - if ( first_time ) { - last_time_stamp.stamp(); - first_time = false; - } + if ( run_real_time ) { + // Update the elapsed time. + static bool first_time = true; + if ( first_time ) { + last_time_stamp.stamp(); + first_time = false; + } - current_time_stamp.stamp(); - /* Convert to ms */ - double elapsed_us = current_time_stamp - last_time_stamp; - if ( elapsed_us < (frame_us - 2000) ) { - double requested_us = (frame_us - elapsed_us) - 2000 ; - ulMilliSecondSleep ( (int)(requested_us / 1000.0) ) ; - } - current_time_stamp.stamp(); - while ( current_time_stamp - last_time_stamp < frame_us ) { current_time_stamp.stamp(); + /* Convert to ms */ + double elapsed_us = current_time_stamp - last_time_stamp; + if ( elapsed_us < (frame_us - 2000) ) { + double requested_us = (frame_us - elapsed_us) - 2000 ; + ulMilliSecondSleep ( (int)(requested_us / 1000.0) ) ; + } + current_time_stamp.stamp(); + while ( current_time_stamp - last_time_stamp < frame_us ) { + current_time_stamp.stamp(); + } } current_time += (frame_us / 1000000.0); @@ -670,6 +712,7 @@ int main( int argc, char **argv ) { int count = 0; double current_time = 0.0; + double last_time = 0.0; gps gpspacket; bzero( &gpspacket, sizeof(gpspacket) ); imu imupacket; bzero( &imupacket, sizeof(imupacket) ); @@ -722,44 +765,47 @@ int main( int argc, char **argv ) { imu_time = imupacket.time; current_time = imu_time; } else { - cout << "oops imu back in time" << endl; + cout << "oops imu back in time: " << imupacket.time << " " << imu_time << endl; } } else if ( id == NAV_PACKET ) { if ( navpacket.time > nav_time ) { nav_time = navpacket.time; current_time = nav_time; } else { - cout << "oops nav back in time" << endl; + cout << "oops nav back in time: " << navpacket.time << " " << nav_time << endl; } } else if ( id == SERVO_PACKET ) { if ( servopacket.time > servo_time ) { servo_time = servopacket.time; current_time = servo_time; } else { - cout << "oops servo back in time" << endl; + cout << "oops servo back in time: " << servopacket.time << " " << servo_time << endl; } } else if ( id == HEALTH_PACKET ) { if ( healthpacket.time > health_time ) { health_time = healthpacket.time; current_time = health_time; } else { - cout << "oops health back in time" << endl; + cout << "oops health back in time: " << healthpacket.time << " " << health_time << endl; } } - // if ( gpspacket.lat > -500 ) { - printf( "%.2f %.6f %.6f %.1f %.2f %.2f %.2f %.2f %d\n", - current_time, - navpacket.lat, navpacket.lon, navpacket.alt, - imupacket.phi*SGD_RADIANS_TO_DEGREES, - imupacket.the*SGD_RADIANS_TO_DEGREES, - imupacket.psi*SGD_RADIANS_TO_DEGREES, - healthpacket.volts, - healthpacket.est_seconds); - // } + if ( current_time >= last_time + (1/hertz) ) { + // if ( gpspacket.lat > -500 ) { + printf( "%.2f %.6f %.6f %.1f %.2f %.2f %.2f %.2f %d\n", + current_time, + navpacket.lat, navpacket.lon, navpacket.alt, + imupacket.phi*SGD_RADIANS_TO_DEGREES, + imupacket.the*SGD_RADIANS_TO_DEGREES, + imupacket.psi*SGD_RADIANS_TO_DEGREES, + healthpacket.volts, + healthpacket.est_seconds); + // } - send_data( &gpspacket, &imupacket, &navpacket, &servopacket, - &healthpacket ); + last_time = current_time; + send_data( &gpspacket, &imupacket, &navpacket, &servopacket, + &healthpacket ); + } } }