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.)
This commit is contained in:
curt
parent
922812a913
commit
1722d26b2f
3 changed files with 274 additions and 101 deletions
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
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(" <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 );
|
||||
|
||||
// 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 );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue