fly/flightgear
1
0
Fork 0
Code Activity

Add support for a new file format which where packet type are saved out into

Browse source 
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.)
This commit is contained in:
curt 2007-04-27 15:30:26 +00:00
parent 922812a913
commit 1722d26b2f
3 changed files with 274 additions and 101 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

191
utils/GPSsmooth/UGear.cxx
View file

@ -7,6 +7,7 @@
#include <simgear/constants.h>
#include <simgear/io/sg_file.hxx>
#include <simgear/math/sg_geodesy.hxx>
#include <simgear/misc/sg_path.hxx>
#include <simgear/misc/sgstream.hxx>
#include <simgear/misc/strutils.hxx>
#include <simgear/misc/stdint.hxx>
@ -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

44
utils/GPSsmooth/UGear.hxx
View file

@ -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;
}
};

80
utils/GPSsmooth/UGear_main.cxx
View file

@ -16,7 +16,7 @@
#include <simgear/constants.h>
#include <simgear/io/lowlevel.hxx> // endian tests
#include <simgear/io/sg_file.hxx>
#include <simgear/io/sg_serial.hxx>
#include <simgear/serial/serial.hxx>
#include <simgear/math/sg_geodesy.hxx>
#include <simgear/timing/timestamp.hxx>
@ -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 <infile_name>" << endl;
cout << "\t[ --flight <flight_dir>" << endl;
cout << "\t[ --serial <dev_name>" << endl;
cout << "\t[ --outfile <outfile_name> (capture the data to a file)" << endl;
cout << "\t[ --hertz <hertz> ]" << endl;
@ -318,7 +323,9 @@ void usage( const string &argv0 ) {
cout << "\t[ --ctrls-port <ctrls output port #> ]" << endl;
cout << "\t[ --altitude-offset <meters> ]" << endl;
cout << "\t[ --skip-seconds <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;
} 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 ( sg_swap ) {
track.set_stargate_swap_mode();
}
if ( infile.length() || flight_dir.length() ) {
if ( infile.length() ) {
// Load data from a track data
track.load( infile, ignore_checksum );
// 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;
@ -501,13 +529,15 @@ 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,15 +660,26 @@ 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(" <trkpt lat=\"%.8f\" lon=\"%.8f\"></trkpt>\n",
navpacket.lat, navpacket.lon );
// printf(" </wpt>\n");
last_lat = navpacket.lat;
last_lon = navpacket.lon;
}
}
send_data( &gpspacket, &imupacket, &navpacket, &servopacket,
&healthpacket );
if ( run_real_time ) {
// Update the elapsed time.
static bool first_time = true;
if ( first_time ) {
@ -657,6 +698,7 @@ int main( int argc, char **argv ) {
while ( current_time_stamp - last_time_stamp < frame_us ) {
current_time_stamp.stamp();
}
}
current_time += (frame_us / 1000000.0);
last_time_stamp = current_time_stamp;
@ -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,31 +765,32 @@ 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 ( current_time >= last_time + (1/hertz) ) {
// if ( gpspacket.lat > -500 ) {
printf( "%.2f %.6f %.6f %.1f %.2f %.2f %.2f %.2f %d\n",
current_time,
@ -758,10 +802,12 @@ int main( int argc, char **argv ) {
healthpacket.est_seconds);
// }
last_time = current_time;
send_data( &gpspacket, &imupacket, &navpacket, &servopacket,
&healthpacket );
}
}
}
return 0;
}