#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>
#define START_OF_MSG1 224
-UGEARTrack::UGEARTrack() {};
+UGEARTrack::UGEARTrack():
+ sg_swap(false)
+{
+};
+
UGEARTrack::~UGEARTrack() {};
{
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;
}
+// 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 ) {
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,
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
};
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)
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();
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(); }
}
+ // 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;
+ }
};
#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>
// Default path
static string infile = "";
+static string flight_dir = "";
static string serialdev = "";
static string outfile = "";
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
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;
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;
}
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 ) {
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 );
}
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;
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;
// Advance imu pointer
while ( current_time > imu1.time
- && imu_count < track.imu_size() )
+ && imu_count < track.imu_size() - 1 )
{
imu0 = imu1;
++imu_count;
// 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()
// Advance servo pointer
while ( current_time > servo1.time
- && servo_count < track.servo_size() )
+ && servo_count < track.servo_size() - 1 )
{
servo0 = servo1;
++servo_count;
// Advance health pointer
while ( current_time > health1.time
- && health_count < track.health_size() )
+ && health_count < track.health_size() - 1 )
{
health0 = health1;
++health_count;
// 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);
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) );
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);
- // }
-
- send_data( &gpspacket, &imupacket, &navpacket, &servopacket,
- &healthpacket );
+ 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);
+ // }
+
+ last_time = current_time;
+ send_data( &gpspacket, &imupacket, &navpacket, &servopacket,
+ &healthpacket );
+ }
}
}
projects / flightgear.git / commitdiff