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/constants.h>
#include <simgear/io/sg_file.hxx> #include <simgear/io/sg_file.hxx>
#include <simgear/math/sg_geodesy.hxx> #include <simgear/math/sg_geodesy.hxx>
#include <simgear/misc/sg_path.hxx>
#include <simgear/misc/sgstream.hxx> #include <simgear/misc/sgstream.hxx>
#include <simgear/misc/strutils.hxx> #include <simgear/misc/strutils.hxx>
#include <simgear/misc/stdint.hxx> #include <simgear/misc/stdint.hxx>
@ -21,7 +22,11 @@ SG_USING_STD(endl);
#define START_OF_MSG1 224 #define START_OF_MSG1 224
UGEARTrack::UGEARTrack() {}; UGEARTrack::UGEARTrack():
sg_swap(false)
{
};
UGEARTrack::~UGEARTrack() {}; UGEARTrack::~UGEARTrack() {};
@ -93,55 +98,65 @@ void UGEARTrack::parse_msg( const int id, char *buf,
{ {
if ( id == GPS_PACKET ) { if ( id == GPS_PACKET ) {
*gpspacket = *(struct gps *)buf; *gpspacket = *(struct gps *)buf;
gpspacket->lat = sg_swap_double( (uint8_t *)buf, 0 ); if ( sg_swap ) {
gpspacket->lon = sg_swap_double( (uint8_t *)buf, 8 ); gpspacket->time = sg_swap_double( (uint8_t *)buf, 0 );
gpspacket->alt = sg_swap_double( (uint8_t *)buf, 16 ); gpspacket->lat = sg_swap_double( (uint8_t *)buf, 8 );
gpspacket->vn = sg_swap_double( (uint8_t *)buf, 24 ); gpspacket->lon = sg_swap_double( (uint8_t *)buf, 16 );
gpspacket->ve = sg_swap_double( (uint8_t *)buf, 32 ); gpspacket->alt = sg_swap_double( (uint8_t *)buf, 24 );
gpspacket->vd = sg_swap_double( (uint8_t *)buf, 40 ); gpspacket->vn = sg_swap_double( (uint8_t *)buf, 32 );
gpspacket->time = sg_swap_double( (uint8_t *)buf, 52 ); gpspacket->ve = sg_swap_double( (uint8_t *)buf, 40 );
gpspacket->vd = sg_swap_double( (uint8_t *)buf, 48 );
}
} else if ( id == IMU_PACKET ) { } else if ( id == IMU_PACKET ) {
*imupacket = *(struct imu *)buf; *imupacket = *(struct imu *)buf;
imupacket->p = sg_swap_double( (uint8_t *)buf, 0 ); if ( sg_swap ) {
imupacket->q = sg_swap_double( (uint8_t *)buf, 8 ); imupacket->time = sg_swap_double( (uint8_t *)buf, 0 );
imupacket->r = sg_swap_double( (uint8_t *)buf, 16 ); imupacket->p = sg_swap_double( (uint8_t *)buf, 8 );
imupacket->ax = sg_swap_double( (uint8_t *)buf, 24 ); imupacket->q = sg_swap_double( (uint8_t *)buf, 16 );
imupacket->ay = sg_swap_double( (uint8_t *)buf, 32 ); imupacket->r = sg_swap_double( (uint8_t *)buf, 24 );
imupacket->az = sg_swap_double( (uint8_t *)buf, 40 ); imupacket->ax = sg_swap_double( (uint8_t *)buf, 32 );
imupacket->hx = sg_swap_double( (uint8_t *)buf, 48 ); imupacket->ay = sg_swap_double( (uint8_t *)buf, 40 );
imupacket->hy = sg_swap_double( (uint8_t *)buf, 56 ); imupacket->az = sg_swap_double( (uint8_t *)buf, 48 );
imupacket->hz = sg_swap_double( (uint8_t *)buf, 64 ); imupacket->hx = sg_swap_double( (uint8_t *)buf, 56 );
imupacket->Ps = sg_swap_double( (uint8_t *)buf, 72 ); imupacket->hy = sg_swap_double( (uint8_t *)buf, 64 );
imupacket->Pt = sg_swap_double( (uint8_t *)buf, 80 ); imupacket->hz = sg_swap_double( (uint8_t *)buf, 72 );
imupacket->phi = sg_swap_double( (uint8_t *)buf, 88 ); imupacket->Ps = sg_swap_double( (uint8_t *)buf, 80 );
imupacket->the = sg_swap_double( (uint8_t *)buf, 96 ); imupacket->Pt = sg_swap_double( (uint8_t *)buf, 88 );
imupacket->psi = sg_swap_double( (uint8_t *)buf, 104 ); imupacket->phi = sg_swap_double( (uint8_t *)buf, 96 );
imupacket->time = sg_swap_double( (uint8_t *)buf, 116 ); imupacket->the = sg_swap_double( (uint8_t *)buf, 104 );
// printf("imu.time = %.4f\n", imupacket->time); 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 ) { } else if ( id == NAV_PACKET ) {
*navpacket = *(struct nav *)buf; *navpacket = *(struct nav *)buf;
navpacket->lat = sg_swap_double( (uint8_t *)buf, 0 ); if ( sg_swap ) {
navpacket->lon = sg_swap_double( (uint8_t *)buf, 8 ); navpacket->time = sg_swap_double( (uint8_t *)buf, 0 );
navpacket->alt = sg_swap_double( (uint8_t *)buf, 16 ); navpacket->lat = sg_swap_double( (uint8_t *)buf, 8 );
navpacket->vn = sg_swap_double( (uint8_t *)buf, 24 ); navpacket->lon = sg_swap_double( (uint8_t *)buf, 16 );
navpacket->ve = sg_swap_double( (uint8_t *)buf, 32 ); navpacket->alt = sg_swap_double( (uint8_t *)buf, 24 );
navpacket->vd = sg_swap_double( (uint8_t *)buf, 40 ); navpacket->vn = sg_swap_double( (uint8_t *)buf, 32 );
navpacket->time = sg_swap_double( (uint8_t *)buf, 52 ); navpacket->ve = sg_swap_double( (uint8_t *)buf, 40 );
navpacket->vd = sg_swap_double( (uint8_t *)buf, 48 );
}
} else if ( id == SERVO_PACKET ) { } else if ( id == SERVO_PACKET ) {
*servopacket = *(struct servo *)buf; *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); // printf("servo time = %.3f\n", servopacket->time);
} else if ( id == HEALTH_PACKET ) { } else if ( id == HEALTH_PACKET ) {
*healthpacket = *(struct health *)buf; *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 { } else {
cout << "unknown id = " << id << endl; cout << "unknown id = " << id << endl;
} }
} }
// load the specified file, return the number of records loaded // load the named stream log file into internal buffers
bool UGEARTrack::load( const string &file, bool ignore_checksum ) { bool UGEARTrack::load_stream( const string &file, bool ignore_checksum ) {
int count = 0; int count = 0;
gps gpspacket; 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 // attempt to work around some system dependent issues. Our read can
// return < data than we want. // return < data than we want.
int myread( SGIOChannel *ch, SGIOChannel *log, char *buf, int length ) { int myread( SGIOChannel *ch, SGIOChannel *log, char *buf, int length ) {
@ -262,6 +370,7 @@ int serial_read( SGSerialPort *serial, SGIOChannel *log,
return bytes_read; return bytes_read;
} }
// load the next message of a real time data stream // load the next message of a real time data stream
int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log, int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
gps *gpspacket, imu *imupacket, nav *navpacket, gps *gpspacket, imu *imupacket, nav *navpacket,
@ -347,18 +456,24 @@ int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
bool myeof = false; bool myeof = false;
// scan for sync characters // scan for sync characters
int scan_count = 0;
uint8_t sync0, sync1; uint8_t sync0, sync1;
result = serial_read( serial, log, tmpbuf, 2 ); result = serial_read( serial, log, tmpbuf, 2 );
sync0 = (unsigned char)tmpbuf[0]; sync0 = (unsigned char)tmpbuf[0];
sync1 = (unsigned char)tmpbuf[1]; sync1 = (unsigned char)tmpbuf[1];
while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) { while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) {
scan_count++;
sync0 = sync1; sync0 = sync1;
serial_read( serial, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0]; serial_read( serial, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
cout << "scanning for start of message " // cout << "scanning for start of message "
<< (unsigned int)sync0 << " " << (unsigned int)sync1 // << (unsigned int)sync0 << " " << (unsigned int)sync1
<< endl; // << endl;
} }
if ( scan_count > 0 ) {
cout << "found start of message after discarding " << scan_count
<< " bytes" << endl;
}
// cout << "found start of message ..." << endl; // cout << "found start of message ..." << endl;
// read message id and size // read message id and size

44
utils/GPSsmooth/UGear.hxx
View file

@ -31,46 +31,46 @@ enum ugPacketType {
}; };
struct imu { struct imu {
double time;
double p,q,r; /* angular velocities */ double p,q,r; /* angular velocities */
double ax,ay,az; /* acceleration */ double ax,ay,az; /* acceleration */
double hx,hy,hz; /* magnetic field */ double hx,hy,hz; /* magnetic field */
double Ps,Pt; /* static/pitot pressure */ double Ps,Pt; /* static/pitot pressure */
// double Tx,Ty,Tz; /* temperature */ // double Tx,Ty,Tz; /* temperature */
double phi,the,psi; /* attitudes */ double phi,the,psi; /* attitudes */
short err_type; /* error type */ uint64_t err_type; /* error type */
double time;
}; };
struct gps { struct gps {
double time;
double lat,lon,alt; /* gps position */ double lat,lon,alt; /* gps position */
double ve,vn,vd; /* gps velocity */ double ve,vn,vd; /* gps velocity */
uint16_t ITOW; double ITOW;
short err_type; /* error type */ uint64_t err_type; /* error type */
double time;
}; };
struct nav { struct nav {
double time;
double lat,lon,alt; double lat,lon,alt;
double ve,vn,vd; double ve,vn,vd;
// float t; // float t;
short err_type; uint64_t err_type;
double time;
}; };
struct servo { struct servo {
uint16_t chn[8];
uint8_t status;
double time; double time;
uint64_t chn[8];
uint64_t status;
}; };
struct health { struct health {
double time;
float volts_raw; /* raw volt reading */ float volts_raw; /* raw volt reading */
float volts; /* filtered volts */ float volts; /* filtered volts */
uint16_t est_seconds; /* estimated useful seconds remaining */ uint32_t est_seconds; /* estimated useful seconds remaining */
uint8_t loadavg; /* system "1 minute" load average */ uint32_t loadavg; /* system "1 minute" load average */
uint8_t ahrs_hz; /* actual ahrs loop hz */ uint32_t ahrs_hz; /* actual ahrs loop hz */
uint8_t nav_hz; /* actual nav loop hz */ uint32_t nav_hz; /* actual nav loop hz */
double time;
}; };
// Manage a saved ugear log (track file) // Manage a saved ugear log (track file)
@ -90,6 +90,10 @@ private:
gps *gpspacket, imu *imupacket, nav *navpacket, gps *gpspacket, imu *imupacket, nav *navpacket,
servo *servopacket, health *healthpacket ); servo *servopacket, health *healthpacket );
// activate special double swap logic for non-standard stargate
// double format
bool sg_swap;
public: public:
UGEARTrack(); UGEARTrack();
@ -106,8 +110,11 @@ public:
servo *servopacket, health *healthpacket, servo *servopacket, health *healthpacket,
bool ignore_checksum ); bool ignore_checksum );
// load the named file into internal buffers // load the named stream log file into internal buffers
bool load( const string &file, bool ignore_checksum ); 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 gps_size() const { return gps_data.size(); }
inline int imu_size() const { return imu_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;
}
}; };

140
utils/GPSsmooth/UGear_main.cxx
View file

@ -16,7 +16,7 @@
#include <simgear/constants.h> #include <simgear/constants.h>
#include <simgear/io/lowlevel.hxx> // endian tests #include <simgear/io/lowlevel.hxx> // endian tests
#include <simgear/io/sg_file.hxx> #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/math/sg_geodesy.hxx>
#include <simgear/timing/timestamp.hxx> #include <simgear/timing/timestamp.hxx>
@ -43,6 +43,7 @@ static int ctrls_port = 5506;
// Default path // Default path
static string infile = ""; static string infile = "";
static string flight_dir = "";
static string serialdev = ""; static string serialdev = "";
static string outfile = ""; static string outfile = "";
@ -66,8 +67,11 @@ double skip = 0.0;
bool inited = false; bool inited = false;
bool run_real_time = true;
bool ignore_checksum = false; bool ignore_checksum = false;
bool sg_swap = false;
// The function htond is defined this way due to the way some // The function htond is defined this way due to the way some
// processors and OSes treat floating point values. Some will raise // processors and OSes treat floating point values. Some will raise
@ -309,6 +313,7 @@ void usage( const string &argv0 ) {
cout << "Usage: " << argv0 << endl; cout << "Usage: " << argv0 << endl;
cout << "\t[ --help ]" << endl; cout << "\t[ --help ]" << endl;
cout << "\t[ --infile <infile_name>" << endl; cout << "\t[ --infile <infile_name>" << endl;
cout << "\t[ --flight <flight_dir>" << endl;
cout << "\t[ --serial <dev_name>" << endl; cout << "\t[ --serial <dev_name>" << endl;
cout << "\t[ --outfile <outfile_name> (capture the data to a file)" << endl; cout << "\t[ --outfile <outfile_name> (capture the data to a file)" << endl;
cout << "\t[ --hertz <hertz> ]" << 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[ --ctrls-port <ctrls output port #> ]" << endl;
cout << "\t[ --altitude-offset <meters> ]" << endl; cout << "\t[ --altitude-offset <meters> ]" << endl;
cout << "\t[ --skip-seconds <seconds> ]" << endl; cout << "\t[ --skip-seconds <seconds> ]" << endl;
cout << "\t[ --no-real-time ]" << endl;
cout << "\t[ --ignore-checksum ]" << endl; cout << "\t[ --ignore-checksum ]" << endl;
cout << "\t[ --sg-swap ]" << endl;
} }
@ -348,6 +355,14 @@ int main( int argc, char **argv ) {
usage( argv[0] ); usage( argv[0] );
exit( -1 ); 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 ) { } else if ( strcmp( argv[i], "--outfile" ) == 0 ) {
++i; ++i;
if ( i < argc ) { if ( i < argc ) {
@ -406,8 +421,12 @@ int main( int argc, char **argv ) {
usage( argv[0] ); usage( argv[0] );
exit( -1 ); exit( -1 );
} }
} else if ( strcmp( argv[i], "--no-real-time" ) == 0 ) {
run_real_time = false;
} else if ( strcmp( argv[i], "--ignore-checksum" ) == 0 ) { } 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 { } else {
usage( argv[0] ); usage( argv[0] );
exit( -1 ); exit( -1 );
@ -453,9 +472,18 @@ int main( int argc, char **argv ) {
} }
cout << "connected outgoing ctrls socket" << endl; cout << "connected outgoing ctrls socket" << endl;
if ( infile.length() ) { if ( sg_swap ) {
// Load data from a track data track.set_stargate_swap_mode();
track.load( infile, ignore_checksum ); }
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.gps_size() << " gps records." << endl;
cout << "Loaded " << track.imu_size() << " imu records." << endl; cout << "Loaded " << track.imu_size() << " imu records." << endl;
cout << "Loaded " << track.nav_size() << " nav records." << endl; cout << "Loaded " << track.nav_size() << " nav records." << endl;
@ -500,14 +528,16 @@ int main( int argc, char **argv ) {
health health0, health1; health health0, health1;
health0 = health1 = track.get_healthpt( 0 ); health0 = health1 = track.get_healthpt( 0 );
double last_lat = -999.9, last_lon = -999.9;
while ( current_time < end_time ) { while ( current_time < end_time ) {
// cout << "current_time = " << current_time << " end_time = " // cout << "current_time = " << current_time << " end_time = "
// << end_time << endl; // << end_time << endl;
// Advance gps pointer // Advance gps pointer
while ( current_time > gps1.time while ( current_time > gps1.time
&& gps_count < track.gps_size() ) && gps_count < track.gps_size() - 1 )
{ {
gps0 = gps1; gps0 = gps1;
++gps_count; ++gps_count;
@ -519,7 +549,7 @@ int main( int argc, char **argv ) {
// Advance imu pointer // Advance imu pointer
while ( current_time > imu1.time while ( current_time > imu1.time
&& imu_count < track.imu_size() ) && imu_count < track.imu_size() - 1 )
{ {
imu0 = imu1; imu0 = imu1;
++imu_count; ++imu_count;
@ -531,11 +561,11 @@ int main( int argc, char **argv ) {
// Advance nav pointer // Advance nav pointer
while ( current_time > nav1.time while ( current_time > nav1.time
&& nav_count < track.nav_size() ) && nav_count < track.nav_size() - 1 )
{ {
nav0 = nav1; nav0 = nav1;
++nav_count; ++nav_count;
// cout << "count = " << count << endl; // cout << "nav count = " << nav_count << endl;
nav1 = track.get_navpt( nav_count ); nav1 = track.get_navpt( nav_count );
} }
// cout << "pos0 = " << pos0.get_seconds() // cout << "pos0 = " << pos0.get_seconds()
@ -543,7 +573,7 @@ int main( int argc, char **argv ) {
// Advance servo pointer // Advance servo pointer
while ( current_time > servo1.time while ( current_time > servo1.time
&& servo_count < track.servo_size() ) && servo_count < track.servo_size() - 1 )
{ {
servo0 = servo1; servo0 = servo1;
++servo_count; ++servo_count;
@ -555,7 +585,7 @@ int main( int argc, char **argv ) {
// Advance health pointer // Advance health pointer
while ( current_time > health1.time while ( current_time > health1.time
&& health_count < track.health_size() ) && health_count < track.health_size() - 1 )
{ {
health0 = health1; health0 = health1;
++health_count; ++health_count;
@ -630,32 +660,44 @@ int main( int argc, char **argv ) {
// cout << (double)current_time << " " << pos.lat_deg << ", " // cout << (double)current_time << " " << pos.lat_deg << ", "
// << pos.lon_deg << " " << att.yaw_deg << endl; // << pos.lon_deg << " " << att.yaw_deg << endl;
if ( gpspacket.lat > -500 ) { if ( gpspacket.lat > -500 ) {
printf( "%.3f %.4f %.4f %.1f %.2f %.2f %.2f\n", // printf( "%.3f %.4f %.4f %.1f %.2f %.2f %.2f\n",
current_time, // current_time,
gpspacket.lat, gpspacket.lon, gpspacket.alt, // navpacket.lat, navpacket.lon, navpacket.alt,
imupacket.psi, imupacket.the, imupacket.phi ); // 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, send_data( &gpspacket, &imupacket, &navpacket, &servopacket,
&healthpacket ); &healthpacket );
// Update the elapsed time. if ( run_real_time ) {
static bool first_time = true; // Update the elapsed time.
if ( first_time ) { static bool first_time = true;
last_time_stamp.stamp(); if ( first_time ) {
first_time = false; 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(); 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); current_time += (frame_us / 1000000.0);
@ -670,6 +712,7 @@ int main( int argc, char **argv ) {
int count = 0; int count = 0;
double current_time = 0.0; double current_time = 0.0;
double last_time = 0.0;
gps gpspacket; bzero( &gpspacket, sizeof(gpspacket) ); gps gpspacket; bzero( &gpspacket, sizeof(gpspacket) );
imu imupacket; bzero( &imupacket, sizeof(imupacket) ); imu imupacket; bzero( &imupacket, sizeof(imupacket) );
@ -722,44 +765,47 @@ int main( int argc, char **argv ) {
imu_time = imupacket.time; imu_time = imupacket.time;
current_time = imu_time; current_time = imu_time;
} else { } else {
cout << "oops imu back in time" << endl; cout << "oops imu back in time: " << imupacket.time << " " << imu_time << endl;
} }
} else if ( id == NAV_PACKET ) { } else if ( id == NAV_PACKET ) {
if ( navpacket.time > nav_time ) { if ( navpacket.time > nav_time ) {
nav_time = navpacket.time; nav_time = navpacket.time;
current_time = nav_time; current_time = nav_time;
} else { } else {
cout << "oops nav back in time" << endl; cout << "oops nav back in time: " << navpacket.time << " " << nav_time << endl;
} }
} else if ( id == SERVO_PACKET ) { } else if ( id == SERVO_PACKET ) {
if ( servopacket.time > servo_time ) { if ( servopacket.time > servo_time ) {
servo_time = servopacket.time; servo_time = servopacket.time;
current_time = servo_time; current_time = servo_time;
} else { } else {
cout << "oops servo back in time" << endl; cout << "oops servo back in time: " << servopacket.time << " " << servo_time << endl;
} }
} else if ( id == HEALTH_PACKET ) { } else if ( id == HEALTH_PACKET ) {
if ( healthpacket.time > health_time ) { if ( healthpacket.time > health_time ) {
health_time = healthpacket.time; health_time = healthpacket.time;
current_time = health_time; current_time = health_time;
} else { } else {
cout << "oops health back in time" << endl; cout << "oops health back in time: " << healthpacket.time << " " << health_time << endl;
} }
} }
// if ( gpspacket.lat > -500 ) { if ( current_time >= last_time + (1/hertz) ) {
printf( "%.2f %.6f %.6f %.1f %.2f %.2f %.2f %.2f %d\n", // if ( gpspacket.lat > -500 ) {
current_time, printf( "%.2f %.6f %.6f %.1f %.2f %.2f %.2f %.2f %d\n",
navpacket.lat, navpacket.lon, navpacket.alt, current_time,
imupacket.phi*SGD_RADIANS_TO_DEGREES, navpacket.lat, navpacket.lon, navpacket.alt,
imupacket.the*SGD_RADIANS_TO_DEGREES, imupacket.phi*SGD_RADIANS_TO_DEGREES,
imupacket.psi*SGD_RADIANS_TO_DEGREES, imupacket.the*SGD_RADIANS_TO_DEGREES,
healthpacket.volts, imupacket.psi*SGD_RADIANS_TO_DEGREES,
healthpacket.est_seconds); healthpacket.volts,
// } healthpacket.est_seconds);
// }
send_data( &gpspacket, &imupacket, &navpacket, &servopacket, last_time = current_time;
&healthpacket ); send_data( &gpspacket, &imupacket, &navpacket, &servopacket,
&healthpacket );
}
} }
} }