#endif
#include <iostream>
+#include <cstdio>
#include <simgear/constants.h>
#include <simgear/io/sg_file.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():
+UGTrack::UGTrack():
sg_swap(false)
{
};
-UGEARTrack::~UGEARTrack() {};
+UGTrack::~UGTrack() {};
// swap the 1st 4 bytes with the last 4 bytes of a stargate double so
}
-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,
health *healthpacket )
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;
if ( sg_swap ) {
servopacket->time = sg_swap_double( (uint8_t *)buf, 0 );
}
- // printf("servo time = %.3f\n", servopacket->time);
+ // 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 ) {
// load the named stream log file into internal buffers
-bool UGEARTrack::load_stream( const string &file, bool ignore_checksum ) {
+bool UGTrack::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 ) {
+bool UGTrack::load_flight( const string &path ) {
gps gpspacket;
imu imupacket;
nav navpacket;
// 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());
+ printf("Cannot open %s\n", file.c_str());
return false;
}
// 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, health *healthpacket,
bool ignore_checksum )
// 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, health *healthpacket,
- bool ignore_checksum )
+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];
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;
health UGEARInterpHEALTH( const health A, const health B, const double percent )
{
health p;
- p.volts_raw = interp(A.volts_raw, B.volts_raw, percent);
- p.volts = interp(A.volts, B.volts, percent);
- p.est_seconds = (uint16_t)interp(A.est_seconds, B.est_seconds, percent);
+ p.command_sequence = B.command_sequence;
p.time = interp(A.time, B.time, percent);
return p;