#ifdef HAVE_CONFIG_H
# include <config.h>
-#endif
+#endif
#include <iostream>
+#include <cstdio>
#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>
#include "UGear.hxx"
-SG_USING_STD(cout);
-SG_USING_STD(endl);
+using std::cout;
+using std::endl;
#define START_OF_MSG0 147
#define START_OF_MSG1 224
-UGEARTrack::UGEARTrack() {};
-UGEARTrack::~UGEARTrack() {};
+UGTrack::UGTrack():
+ sg_swap(false)
+{
+};
+
+UGTrack::~UGTrack() {};
// swap the 1st 4 bytes with the last 4 bytes of a stargate double so
static bool validate_cksum( uint8_t id, uint8_t size, char *buf,
- uint8_t cksum0, uint8_t cksum1 )
+ uint8_t cksum0, uint8_t cksum1,
+ bool ignore_checksum )
{
+ if ( ignore_checksum ) {
+ return true;
+ }
+
uint8_t c0 = 0;
uint8_t c1 = 0;
c0 += (uint8_t)buf[i];
c1 += c0;
// cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1
- // << " [" << (unsigned int)buf[i] << "]" << endl;
+ // << " [" << (unsigned int)(uint8_t)buf[i] << "]" << endl;
}
- cout << "c0 = " << (unsigned int)c0 << " (" << (unsigned int)cksum0
- << ") c1 = " << (unsigned int)c1 << " (" << (unsigned int)cksum1
- << ")" << endl;
+ // cout << "c0 = " << (unsigned int)c0 << " (" << (unsigned int)cksum0
+ // << ") c1 = " << (unsigned int)c1 << " (" << (unsigned int)cksum1
+ // << ")" << endl;
if ( c0 == cksum0 && c1 == cksum1 ) {
return true;
}
-void UGEARTrack::parse_msg( const int id, char *buf,
+void UGTrack::parse_msg( const int id, char *buf,
struct gps *gpspacket, imu *imupacket,
- nav *navpacket, servo *servopacket )
+ nav *navpacket, servo *servopacket,
+ health *healthpacket )
{
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, 56 );
+ 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 );
+ gpspacket->ITOW = sg_swap_double( (uint8_t *)buf, 56 );
+ }
} 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->lon = sg_swap_double( (uint8_t *)buf, 0 );
- navpacket->lat = 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 );
- // printf("servo time = %.3f\n", servopacket->time);
+ if ( sg_swap ) {
+ servopacket->time = sg_swap_double( (uint8_t *)buf, 0 );
+ }
+ // printf("servo time = %.3f %d %d\n", servopacket->time, servopacket->chn[0], servopacket->chn[1]);
+ } else if ( id == HEALTH_PACKET ) {
+ *healthpacket = *(struct health *)buf;
+ 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 ) {
+// load the named stream log file into internal buffers
+bool UGTrack::load_stream( const string &file, bool ignore_checksum ) {
int count = 0;
gps gpspacket;
imu imupacket;
nav navpacket;
servo servopacket;
+ health healthpacket;
double gps_time = 0;
double imu_time = 0;
double nav_time = 0;
double servo_time = 0;
+ double health_time = 0;
gps_data.clear();
imu_data.clear();
nav_data.clear();
servo_data.clear();
+ health_data.clear();
// open the file
SGFile input( file );
while ( ! input.eof() ) {
// cout << "looking for next message ..." << endl;
int id = next_message( &input, NULL, &gpspacket, &imupacket,
- &navpacket, &servopacket );
+ &navpacket, &servopacket, &healthpacket,
+ ignore_checksum );
count++;
if ( id == GPS_PACKET ) {
gps_data.push_back( gpspacket );
gps_time = gpspacket.time;
} else {
- cout << "oops att back in time" << endl;
+ cout << "oops gps back in time: " << gpspacket.time << " " << gps_time << endl;
}
} else if ( id == IMU_PACKET ) {
if ( imupacket.time > imu_time ) {
imu_data.push_back( imupacket );
imu_time = imupacket.time;
} else {
- cout << "oops pos back in time" << endl;
+ cout << "oops imu back in time" << endl;
}
} else if ( id == NAV_PACKET ) {
if ( navpacket.time > nav_time ) {
nav_data.push_back( navpacket );
nav_time = navpacket.time;
} else {
- cout << "oops pos back in time" << endl;
+ cout << "oops nav back in time" << endl;
}
} else if ( id == SERVO_PACKET ) {
if ( servopacket.time > servo_time ) {
servo_data.push_back( servopacket );
servo_time = servopacket.time;
} else {
- cout << "oops pos back in time" << endl;
+ cout << "oops servo back in time" << endl;
+ }
+ } else if ( id == HEALTH_PACKET ) {
+ if ( healthpacket.time > health_time ) {
+ health_data.push_back( healthpacket );
+ health_time = healthpacket.time;
+ } else {
+ cout << "oops health back in time" << endl;
}
}
}
}
+// load the named stream log file into internal buffers
+bool UGTrack::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" );
+ std::string fdata = file.local8BitStr();
+
+ if ( (fgps = gzopen( fdata.c_str(), "r" )) == NULL ) {
+ printf("Cannot open %s\n", fdata.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" );
+ fdata = file.local8BitStr();
+ if ( (fimu = gzopen( fdata.c_str(), "r" )) == NULL ) {
+ printf("Cannot open %s\n", fdata.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" );
+ fdata = file.local8BitStr();
+
+ if ( (fnav = gzopen( fdata.c_str(), "r" )) == NULL ) {
+ printf("Cannot open %s\n", fdata.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" );
+ fdata = file.local8BitStr();
+
+ if ( (fservo = gzopen( fdata.c_str(), "r" )) == NULL ) {
+ printf("Cannot open %s\n", fdata.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" );
+ fdata = file.local8BitStr();
+
+ if ( (fhealth = gzopen( fdata.c_str(), "r" )) == NULL ) {
+ printf("Cannot open %s\n", fdata.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 ) {
while ( bytes_read < length ) {
result = serial->read_port( tmp, length - bytes_read );
- if ( result > 0 && log != NULL ) {
- log->write( buf, result );
- }
bytes_read += result;
tmp += result;
// cout << " read " << bytes_read << " of " << length << endl;
}
+ if ( bytes_read > 0 && log != NULL ) {
+ log->write( buf, bytes_read );
+ }
+
return bytes_read;
}
+
// load the next message of a real time data stream
-int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
+int UGTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
gps *gpspacket, imu *imupacket, nav *navpacket,
- servo *servopacket )
+ servo *servopacket, health *healthpacket,
+ bool ignore_checksum )
{
char tmpbuf[256];
char savebuf[256];
// scan for sync characters
uint8_t sync0, sync1;
- myread( ch, log, tmpbuf, 1 ); sync0 = (unsigned char)tmpbuf[0];
- myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
+ myread( ch, log, tmpbuf, 2 );
+ sync0 = (unsigned char)tmpbuf[0];
+ sync1 = (unsigned char)tmpbuf[1];
while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) {
sync0 = sync1;
myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
- // cout << "scanning for start of message "
- // << (unsigned int)sync0 << " " << (unsigned int)sync1
- // << ", eof = " << ch->eof() << endl;
+ cout << "scanning for start of message "
+ << (unsigned int)sync0 << " " << (unsigned int)sync1
+ << ", eof = " << ch->eof() << endl;
if ( ch->get_type() == sgFileType ) {
myeof = ((SGFile *)ch)->eof();
}
cout << "found start of message ..." << endl;
// read message id and size
- myread( ch, log, tmpbuf, 1 ); uint8_t id = (unsigned char)tmpbuf[0];
- myread( ch, log, tmpbuf, 1 ); uint8_t size = (unsigned char)tmpbuf[0];
- cout << "message = " << (int)id << " size = " << (int)size << endl;
+ myread( ch, log, tmpbuf, 2 );
+ uint8_t id = (unsigned char)tmpbuf[0];
+ uint8_t size = (unsigned char)tmpbuf[1];
+ // cout << "message = " << (int)id << " size = " << (int)size << endl;
// load message
if ( ch->get_type() == sgFileType ) {
}
// read checksum
- myread( ch, log, tmpbuf, 1 ); uint8_t cksum0 = (unsigned char)tmpbuf[0];
- myread( ch, log, tmpbuf, 1 ); uint8_t cksum1 = (unsigned char)tmpbuf[0];
-
- if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
- parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket );
+ myread( ch, log, tmpbuf, 2 );
+ uint8_t cksum0 = (unsigned char)tmpbuf[0];
+ uint8_t cksum1 = (unsigned char)tmpbuf[1];
+
+ if ( validate_cksum( id, size, savebuf, cksum0, cksum1, ignore_checksum ) )
+ {
+ parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket,
+ healthpacket );
return id;
}
// load the next message of a real time data stream
-int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
- gps *gpspacket, imu *imupacket, nav *navpacket,
- servo *servopacket )
+int UGTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
+ gps *gpspacket, imu *imupacket, nav *navpacket,
+ servo *servopacket, health *healthpacket,
+ bool ignore_checksum )
{
char tmpbuf[256];
char savebuf[256];
- int result = 0;
// cout << "in next_message()" << endl;
bool myeof = false;
// scan for sync characters
+ int scan_count = 0;
uint8_t sync0, sync1;
- result = serial_read( serial, log, tmpbuf, 2 );
+ 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
serial_read( serial, log, tmpbuf, 2 );
uint8_t id = (unsigned char)tmpbuf[0];
uint8_t size = (unsigned char)tmpbuf[1];
- cout << "message = " << (int)id << " size = " << (int)size << endl;
+ // cout << "message = " << (int)id << " size = " << (int)size << endl;
// load message
serial_read( serial, log, savebuf, size );
uint8_t cksum1 = (unsigned char)tmpbuf[1];
// cout << "cksum0 = " << (int)cksum0 << " cksum1 = " << (int)cksum1
// << endl;
-
- if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
- parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket );
+
+ if ( validate_cksum( id, size, savebuf, cksum0, cksum1, ignore_checksum ) )
+ {
+ parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket,
+ healthpacket );
return id;
}
cout << "Check sum failure!" << endl;
return -1;
-
+
}
p.hz = interp(A.hz, B.hz, percent);
p.Ps = interp(A.Ps, B.Ps, percent);
p.Pt = interp(A.Pt, B.Pt, percent);
- p.phi = interp(A.phi, B.phi, percent);
- p.the = interp(A.the, B.the, percent);
- p.psi = interp(A.psi, B.psi, percent);
+ p.phi = interp(A.phi, B.phi, percent, true);
+ p.the = interp(A.the, B.the, percent, true);
+ p.psi = interp(A.psi, B.psi, percent, true);
p.err_type = A.err_type;
return p;
return p;
}
+
+health UGEARInterpHEALTH( const health A, const health B, const double percent )
+{
+ health p;
+ p.command_sequence = B.command_sequence;
+ p.time = interp(A.time, B.time, percent);
+
+ return p;
+}
+
projects / flightgear.git / blobdiff