int myread( SGIOChannel *ch, SGIOChannel *log, char *buf, int length ) {
bool myeof = false;
int result = 0;
- while ( result != length && !myeof ) {
- result = ch->read( buf, length );
- if ( ch->get_type() == sgFileType ) {
- myeof = ((SGFile *)ch)->eof();
- }
+ if ( !myeof ) {
+ result = ch->read( buf, length );
+ // cout << "wanted " << length << " read " << result << " bytes" << endl;
+ if ( ch->get_type() == sgFileType ) {
+ myeof = ((SGFile *)ch)->eof();
+ }
}
if ( result > 0 && log != NULL ) {
return result;
}
+// attempt to work around some system dependent issues. Our read can
+// return < data than we want.
+int serial_read( SGSerialPort *serial, char *buf, int length ) {
+ int result = 0;
+ int bytes_read = 0;
+ char *tmp = buf;
+
+ while ( bytes_read < length ) {
+ result = serial->read_port( tmp, length - bytes_read );
+ bytes_read += result;
+ tmp += result;
+ // cout << " read " << bytes_read << " of " << length << endl;
+ }
+
+ return bytes_read;
+}
+
// load the next message of a real time data stream
int MIDGTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
MIDGpos *pos, MIDGatt *att )
while ( (sync0 != 129 || sync1 != 161) && !myeof ) {
sync0 = sync1;
myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
- // cout << "scanning for start of message, 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 << "ERROR: didn't read enough bytes!" << endl;
}
} else {
+#ifdef READ_ONE_BY_ONE
for ( int i = 0; i < size; ++i ) {
myread( ch, log, tmpbuf, 1 ); savebuf[i] = tmpbuf[0];
}
+#else
+ myread( ch, log, savebuf, size );
+#endif
}
// read checksum
return id;
}
- // cout << "Check sum failure!" << endl;
+ cout << "Check sum failure!" << endl;
return -1;
}
+// load the next message of a real time data stream
+int MIDGTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
+ MIDGpos *pos, MIDGatt *att )
+{
+ char tmpbuf[256];
+ char savebuf[256];
+ int result = 0;
+
+ cout << "in next_message()" << endl;
+
+ bool myeof = false;
+
+ // scan for sync characters
+ uint8_t sync0, sync1;
+ result = serial_read( serial, tmpbuf, 2 );
+ sync0 = (unsigned char)tmpbuf[0];
+ sync1 = (unsigned char)tmpbuf[1];
+ while ( (sync0 != 129 || sync1 != 161) && !myeof ) {
+ sync0 = sync1;
+ serial_read( serial, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
+ cout << "scanning for start of message "
+ << (unsigned int)sync0 << " " << (unsigned int)sync1
+ << endl;
+ }
+
+ cout << "found start of message ..." << endl;
+
+ // read message id and size
+ serial_read( serial, 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
+ serial_read( serial, savebuf, size );
+
+ // read checksum
+ serial_read( serial, tmpbuf, 2 );
+ uint8_t cksum0 = (unsigned char)tmpbuf[0];
+ uint8_t cksum1 = (unsigned char)tmpbuf[1];
+
+ if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
+ parse_msg( id, savebuf, pos, att );
+
+ //
+ // FIXME
+ // WRITE DATA TO LOG FILE
+ //
+
+ return id;
+ }
+
+ cout << "Check sum failure!" << endl;
+ return -1;
+
+
+}
+
+
static double interp( double a, double b, double p, bool rotational = false ) {
double diff = b - a;
if ( rotational ) {
#include <simgear/misc/stdint.hxx>
#include <simgear/io/iochannel.hxx>
+#include <simgear/serial/serial.hxx>
SG_USING_STD(cout);
SG_USING_STD(endl);
// returns id # if a valid message found.
int next_message( SGIOChannel *ch, SGIOChannel *log,
MIDGpos *pos, MIDGatt *att );
+ int next_message( SGSerialPort *serial, SGIOChannel *log,
+ MIDGpos *pos, MIDGatt *att );
// load the named file into internal buffers
bool load( const string &file );
// << endl;
// cout << (double)current_time << " " << pos.lat_deg << ", "
// << pos.lon_deg << " " << att.yaw_deg << endl;
- if ( pos.lat_deg > 50 ) {
+ if ( pos.lat_deg > -500 ) {
printf( "%.3f %.4f %.4f %.1f %.2f %.2f %.2f\n",
current_time,
pos.lat_deg, pos.lon_deg, pos.altitude_msl,
uint32_t att_time = 1;
// open the serial port device
- SGSerial input( serialdev, "115200" );
- if ( !input.open( SG_IO_IN ) ) {
+ SGSerialPort input( serialdev, 115200 );
+ if ( !input.is_enabled() ) {
cout << "Cannot open: " << serialdev << endl;
return false;
}
return false;
}
- while ( ! input.eof() ) {
+ while ( input.is_enabled() ) {
// cout << "looking for next message ..." << endl;
int id = track.next_message( &input, &output, &pos, &att );
+ cout << "message id = " << id << endl;
count++;
if ( id == 10 ) {
}
}
- if ( pos.lat_deg > 50 ) {
- printf( "%.3f %.4f %.4f %.1f %.2f %.2f %.2f\n",
- current_time,
- pos.lat_deg, pos.lon_deg, pos.altitude_msl,
- att.yaw_rad * 180.0 / SG_PI,
- att.pitch_rad * 180.0 / SG_PI,
- att.roll_rad * 180.0 / SG_PI );
+ if ( pos.lat_deg > -500 ) {
+ // printf( "%.3f %.4f %.4f %.1f %.2f %.2f %.2f\n",
+ // current_time,
+ // pos.lat_deg, pos.lon_deg, pos.altitude_msl,
+ // att.yaw_rad * 180.0 / SG_PI,
+ // att.pitch_rad * 180.0 / SG_PI,
+ // att.roll_rad * 180.0 / SG_PI );
}
send_data( pos, att );
-noinst_PROGRAMS = GPSsmooth MIDGsmooth
+noinst_PROGRAMS = GPSsmooth MIDGsmooth UGsmooth
GPSsmooth_SOURCES = \
gps.cxx gps.hxx \
-lplibnet -lplibul \
$(joystick_LIBS) $(network_LIBS) $(base_LIBS) -lz
+UGsmooth_SOURCES = \
+ UGear.cxx UGear.hxx \
+ UGear_main.cxx
+
+UGsmooth_LDADD = \
+ -lsgio -lsgserial -lsgtiming -lsgmath -lsgbucket -lsgmisc -lsgdebug \
+ -lplibnet -lplibul \
+ $(joystick_LIBS) $(network_LIBS) $(base_LIBS) -lz
+
+
INCLUDES = -I$(top_srcdir)/src
--- /dev/null
+#ifdef HAVE_CONFIG_H
+# include <config.h>
+#endif
+
+#include <iostream>
+
+#include <simgear/constants.h>
+#include <simgear/io/sg_file.hxx>
+#include <simgear/math/sg_geodesy.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);
+
+
+#define START_OF_MSG0 147
+#define START_OF_MSG1 224
+
+
+UGEARTrack::UGEARTrack() {};
+UGEARTrack::~UGEARTrack() {};
+
+
+// swap the 1st 4 bytes with the last 4 bytes of a stargate double so
+// it matches the PC representation
+static double sg_swap_double( uint8_t *buf, size_t offset ) {
+ double *result;
+ uint8_t tmpbuf[10];
+
+ for ( size_t i = 0; i < 4; ++i ) {
+ tmpbuf[i] = buf[offset + i + 4];
+ }
+ for ( size_t i = 0; i < 4; ++i ) {
+ tmpbuf[i + 4] = buf[offset + i];
+ }
+
+ // for ( size_t i = 0; i < 8; ++i ) {
+ // printf("%d ", tmpbuf[i]);
+ // }
+ // printf("\n");
+
+ result = (double *)tmpbuf;
+ return *result;
+}
+
+
+static bool validate_cksum( uint8_t id, uint8_t size, char *buf,
+ uint8_t cksum0, uint8_t cksum1 )
+{
+ uint8_t c0 = 0;
+ uint8_t c1 = 0;
+
+ c0 += id;
+ c1 += c0;
+ // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 << endl;
+
+ c0 += size;
+ c1 += c0;
+ // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 << endl;
+
+ for ( uint8_t i = 0; i < size; i++ ) {
+ c0 += (uint8_t)buf[i];
+ c1 += c0;
+ // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1
+ // << " [" << (unsigned int)buf[i] << "]" << endl;
+ }
+
+ // cout << "c0 = " << (unsigned int)c0 << " (" << (unsigned int)cksum0
+ // << ") c1 = " << (unsigned int)c1 << " (" << (unsigned int)cksum1
+ // << ")" << endl;
+
+ if ( c0 == cksum0 && c1 == cksum1 ) {
+ return true;
+ } else {
+ return false;
+ }
+}
+
+
+void UGEARTrack::parse_msg( const int id, char *buf,
+ struct gps *gpspacket, imu *imupacket,
+ nav *navpacket, servo *servopacket )
+{
+ if ( id == GPS_PACKET ) {
+ *gpspacket = *(struct gps *)buf;
+ gpspacket->lon = sg_swap_double( (uint8_t *)buf, 0 );
+ gpspacket->lat = 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 );
+ } 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);
+ } else if ( id == NAV_PACKET ) {
+ *navpacket = *(struct nav *)buf;
+ } 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);
+ } else {
+ cout << "unknown id = " << id << endl;
+ }
+}
+
+
+// load the specified file, return the number of records loaded
+bool UGEARTrack::load( const string &file ) {
+ int count = 0;
+
+ gps gpspacket;
+ imu imupacket;
+ nav navpacket;
+ servo servopacket;
+
+ double gps_time = 0;
+ double imu_time = 0;
+ double nav_time = 0;
+ double servo_time = 0;
+
+ gps_data.clear();
+ imu_data.clear();
+ nav_data.clear();
+ servo_data.clear();
+
+ // open the file
+ SGFile input( file );
+ if ( !input.open( SG_IO_IN ) ) {
+ cout << "Cannot open file: " << file << endl;
+ return false;
+ }
+
+ while ( ! input.eof() ) {
+ // cout << "looking for next message ..." << endl;
+ int id = next_message( &input, NULL, &gpspacket, &imupacket,
+ &navpacket, &servopacket );
+ count++;
+
+ if ( id == GPS_PACKET ) {
+ if ( gpspacket.time > gps_time ) {
+ gps_data.push_back( gpspacket );
+ gps_time = gpspacket.time;
+ } else {
+ cout << "oops att back in 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;
+ }
+ } 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;
+ }
+ } 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 << "processed " << count << " messages" << endl;
+ 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 ) {
+ bool myeof = false;
+ int result = 0;
+ if ( !myeof ) {
+ result = ch->read( buf, length );
+ // cout << "wanted " << length << " read " << result << " bytes" << endl;
+ if ( ch->get_type() == sgFileType ) {
+ myeof = ((SGFile *)ch)->eof();
+ }
+ }
+
+ if ( result > 0 && log != NULL ) {
+ log->write( buf, result );
+ }
+
+ return result;
+}
+
+// attempt to work around some system dependent issues. Our read can
+// return < data than we want.
+int serial_read( SGSerialPort *serial, char *buf, int length ) {
+ int result = 0;
+ int bytes_read = 0;
+ char *tmp = buf;
+
+ while ( bytes_read < length ) {
+ result = serial->read_port( tmp, length - bytes_read );
+ bytes_read += result;
+ tmp += result;
+ // cout << " read " << bytes_read << " of " << length << endl;
+ }
+
+ 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,
+ servo *servopacket )
+{
+ char tmpbuf[256];
+ char savebuf[256];
+
+ // cout << "in next_message()" << endl;
+
+ bool myeof = false;
+
+ // 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];
+ 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;
+ 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;
+
+ // load message
+ if ( ch->get_type() == sgFileType ) {
+ int count = myread( ch, log, savebuf, size );
+ if ( count != size ) {
+ cout << "ERROR: didn't read enough bytes!" << endl;
+ }
+ } else {
+#ifdef READ_ONE_BY_ONE
+ for ( int i = 0; i < size; ++i ) {
+ myread( ch, log, tmpbuf, 1 ); savebuf[i] = tmpbuf[0];
+ }
+#else
+ myread( ch, log, savebuf, size );
+#endif
+ }
+
+ // 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 );
+ return id;
+ }
+
+ cout << "Check sum failure!" << endl;
+ return -1;
+}
+
+
+// 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 )
+{
+ char tmpbuf[256];
+ char savebuf[256];
+ int result = 0;
+
+ // cout << "in next_message()" << endl;
+
+ bool myeof = false;
+
+ // scan for sync characters
+ uint8_t sync0, sync1;
+ result = serial_read( serial, tmpbuf, 2 );
+ sync0 = (unsigned char)tmpbuf[0];
+ sync1 = (unsigned char)tmpbuf[1];
+ while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) {
+ sync0 = sync1;
+ serial_read( serial, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
+ cout << "scanning for start of message "
+ << (unsigned int)sync0 << " " << (unsigned int)sync1
+ << endl;
+ }
+
+ // cout << "found start of message ..." << endl;
+
+ // read message id and size
+ serial_read( serial, 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
+ serial_read( serial, savebuf, size );
+
+ // read checksum
+ serial_read( serial, tmpbuf, 2 );
+ uint8_t cksum0 = (unsigned char)tmpbuf[0];
+ 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 );
+
+ //
+ // FIXME
+ // WRITE DATA TO LOG FILE
+ //
+
+ return id;
+ }
+
+ cout << "Check sum failure!" << endl;
+ return -1;
+
+
+}
+
+
+static double interp( double a, double b, double p, bool rotational = false ) {
+ double diff = b - a;
+ if ( rotational ) {
+ // special handling of rotational data
+ if ( diff > SGD_PI ) {
+ diff -= SGD_2PI;
+ } else if ( diff < -SGD_PI ) {
+ diff += SGD_2PI;
+ }
+ }
+ return a + diff * p;
+}
+
+
+gps UGEARInterpGPS( const gps A, const gps B, const double percent )
+{
+ gps p;
+ p.time = interp(A.time, B.time, percent);
+ p.lat = interp(A.lat, B.lat, percent);
+ p.lon = interp(A.lon, B.lon, percent);
+ p.alt = interp(A.alt, B.alt, percent);
+ p.ve = interp(A.ve, B.ve, percent);
+ p.vn = interp(A.vn, B.vn, percent);
+ p.vd = interp(A.vd, B.vd, percent);
+ p.ITOW = (int)interp(A.ITOW, B.ITOW, percent);
+ p.err_type = A.err_type;
+
+ return p;
+}
+
+imu UGEARInterpIMU( const imu A, const imu B, const double percent )
+{
+ imu p;
+ p.time = interp(A.time, B.time, percent);
+ p.p = interp(A.p, B.p, percent);
+ p.q = interp(A.q, B.q, percent);
+ p.r = interp(A.r, B.r, percent);
+ p.ax = interp(A.ax, B.ax, percent);
+ p.ay = interp(A.ay, B.ay, percent);
+ p.az = interp(A.az, B.az, percent);
+ p.hx = interp(A.hx, B.hx, percent);
+ p.hy = interp(A.hy, B.hy, percent);
+ 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.err_type = A.err_type;
+
+ return p;
+}
+
+nav UGEARInterpNAV( const nav A, const nav B, const double percent )
+{
+ nav p;
+ p.time = interp(A.time, B.time, percent);
+ p.lat = interp(A.lat, B.lat, percent);
+ p.lon = interp(A.lon, B.lon, percent);
+ p.alt = interp(A.alt, B.alt, percent);
+ p.ve = interp(A.ve, B.ve, percent);
+ p.vn = interp(A.vn, B.vn, percent);
+ p.vd = interp(A.vd, B.vd, percent);
+ p.err_type = A.err_type;
+
+ return p;
+}
+
+
+servo UGEARInterpSERVO( const servo A, const servo B, const double percent )
+{
+ servo p;
+ for ( int i = 0; i < 8; ++i ) {
+ p.chn[i] = (uint16_t)interp(A.chn[i], B.chn[i], percent);
+ }
+ p.status = A.status;
+
+ return p;
+}
+
--- /dev/null
+#ifndef _FG_UGEAR_II_HXX
+#define _FG_UGEAR_II_HXX
+
+
+#ifdef HAVE_CONFIG_H
+# include <config.h>
+#endif
+
+#include <simgear/compiler.h>
+
+#include <iostream>
+#include <string>
+#include <vector>
+
+#include <simgear/misc/stdint.hxx>
+#include <simgear/io/iochannel.hxx>
+#include <simgear/serial/serial.hxx>
+
+SG_USING_STD(cout);
+SG_USING_STD(endl);
+SG_USING_STD(string);
+SG_USING_STD(vector);
+
+
+enum ugPacketType {
+ GPS_PACKET = 0,
+ IMU_PACKET = 1,
+ NAV_PACKET = 2,
+ SERVO_PACKET = 3
+};
+
+struct imu {
+ 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;
+};
+
+struct gps {
+ double lat,lon,alt; /* gps position */
+ double ve,vn,vd; /* gps velocity */
+ uint16_t ITOW;
+ short err_type; /* error type */
+ double time;
+};
+
+struct nav {
+ double lat,lon,alt;
+ double ve,vn,vd;
+ // float t;
+ short err_type;
+ double time;
+};
+
+struct servo {
+ uint16_t chn[8];
+ uint8_t status;
+ double time;
+};
+
+
+// Manage a saved ugear log (track file)
+class UGEARTrack {
+
+private:
+
+ vector <gps> gps_data;
+ vector <imu> imu_data;
+ vector <nav> nav_data;
+ vector <servo> servo_data;
+
+ // parse message and put current data into vector if message has a
+ // newer time stamp than existing data.
+ void parse_msg( const int id, char *buf,
+ gps *gpspacket, imu *imupacket, nav *navpacket,
+ servo *servopacket );
+
+public:
+
+ UGEARTrack();
+ ~UGEARTrack();
+
+ // read/parse the next message from the specified data stream,
+ // returns id # if a valid message found.
+ int next_message( SGIOChannel *ch, SGIOChannel *log,
+ gps *gpspacket, imu *imupacket, nav *navpacket,
+ servo *servopacket );
+ int next_message( SGSerialPort *serial, SGIOChannel *log,
+ gps *gpspacket, imu *imupacket, nav *navpacket,
+ servo *servopacket );
+
+ // load the named file into internal buffers
+ bool load( const string &file );
+
+ inline int gps_size() const { return gps_data.size(); }
+ inline int imu_size() const { return imu_data.size(); }
+ inline int nav_size() const { return nav_data.size(); }
+ inline int servo_size() const { return servo_data.size(); }
+
+ inline gps get_gpspt( const unsigned int i )
+ {
+ if ( i < gps_data.size() ) {
+ return gps_data[i];
+ } else {
+ return gps();
+ }
+ }
+ inline imu get_imupt( const unsigned int i )
+ {
+ if ( i < imu_data.size() ) {
+ return imu_data[i];
+ } else {
+ return imu();
+ }
+ }
+ inline nav get_navpt( const unsigned int i )
+ {
+ if ( i < nav_data.size() ) {
+ return nav_data[i];
+ } else {
+ return nav();
+ }
+ }
+ inline servo get_servopt( const unsigned int i )
+ {
+ if ( i < servo_data.size() ) {
+ return servo_data[i];
+ } else {
+ return servo();
+ }
+ }
+
+
+};
+
+
+gps UGEARInterpGPS( const gps A, const gps B, const double percent );
+imu UGEARInterpIMU( const imu A, const imu B, const double percent );
+nav UGEARInterpNAV( const nav A, const nav B, const double percent );
+servo UGEARInterpSERVO( const servo A, const servo B, const double percent );
+
+
+#endif // _FG_UGEAR_II_HXX
--- /dev/null
+#ifdef HAVE_CONFIG_H
+# include <config.h>
+#endif
+
+#include <iostream>
+#include <string>
+
+#include <plib/net.h>
+#include <plib/sg.h>
+
+#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/math/sg_geodesy.hxx>
+#include <simgear/timing/timestamp.hxx>
+
+#include <Network/net_ctrls.hxx>
+#include <Network/net_fdm.hxx>
+
+#include "UGear.hxx"
+
+
+SG_USING_STD(cout);
+SG_USING_STD(endl);
+SG_USING_STD(string);
+
+
+// Network channels
+static netSocket fdm_sock, ctrls_sock;
+
+// ugear data
+UGEARTrack track;
+
+// Default ports
+static int fdm_port = 5505;
+static int ctrls_port = 5506;
+
+// Default path
+static string infile = "";
+static string serialdev = "";
+static string outfile = "";
+
+// Master time counter
+float sim_time = 0.0f;
+double frame_us = 0.0f;
+
+// sim control
+SGTimeStamp last_time_stamp;
+SGTimeStamp current_time_stamp;
+
+// altitude offset
+double alt_offset = 0.0;
+
+// skip initial seconds
+double skip = 0.0;
+
+// for speed estimate
+// double last_lat = 0.0, last_lon = 0.0;
+// double kts_filter = 0.0;
+
+bool inited = false;
+
+
+// The function htond is defined this way due to the way some
+// processors and OSes treat floating point values. Some will raise
+// an exception whenever a "bad" floating point value is loaded into a
+// floating point register. Solaris is notorious for this, but then
+// so is LynxOS on the PowerPC. By translating the data in place,
+// there is no need to load a FP register with the "corruped" floating
+// point value. By doing the BIG_ENDIAN test, I can optimize the
+// routine for big-endian processors so it can be as efficient as
+// possible
+static void htond (double &x)
+{
+ if ( sgIsLittleEndian() ) {
+ int *Double_Overlay;
+ int Holding_Buffer;
+
+ Double_Overlay = (int *) &x;
+ Holding_Buffer = Double_Overlay [0];
+
+ Double_Overlay [0] = htonl (Double_Overlay [1]);
+ Double_Overlay [1] = htonl (Holding_Buffer);
+ } else {
+ return;
+ }
+}
+
+// Float version
+static void htonf (float &x)
+{
+ if ( sgIsLittleEndian() ) {
+ int *Float_Overlay;
+ int Holding_Buffer;
+
+ Float_Overlay = (int *) &x;
+ Holding_Buffer = Float_Overlay [0];
+
+ Float_Overlay [0] = htonl (Holding_Buffer);
+ } else {
+ return;
+ }
+}
+
+
+static void ugear2fg( gps *gpspacket, imu *imupacket, nav *navpacket,
+ servo *servopacket,
+ FGNetFDM *fdm, FGNetCtrls *ctrls )
+{
+ unsigned int i;
+
+ // Version sanity checking
+ fdm->version = FG_NET_FDM_VERSION;
+
+ // Aero parameters
+ fdm->longitude = gpspacket->lon;
+ fdm->latitude = gpspacket->lat;
+ fdm->altitude = gpspacket->alt;
+ fdm->agl = -9999.0;
+ fdm->psi = imupacket->psi; // heading
+ fdm->phi = imupacket->phi; // roll
+ fdm->theta = imupacket->the; // pitch;
+
+ fdm->phidot = 0.0;
+ fdm->thetadot = 0.0;
+ fdm->psidot = 0.0;
+
+ // estimate speed
+ // double az1, az2, dist;
+ // geo_inverse_wgs_84( pos.altitude_msl, last_lat, last_lon,
+ // pos.lat_deg, pos.lon_deg, &az1, &az2, &dist );
+ // double v_ms = dist / (frame_us / 1000000);
+ // double v_kts = v_ms * SG_METER_TO_NM * 3600;
+ // kts_filter = (0.99 * kts_filter) + (0.01 * v_kts);
+ double vn = gpspacket->vn;
+ double ve = gpspacket->ve;
+ double vd = gpspacket->vd;
+
+ fdm->vcas = sqrt( vn*vn + ve*ve + vd*vd );
+ // last_lat = pos.lat_deg;
+ // last_lon = pos.lon_deg;
+ // cout << "kts_filter = " << kts_filter << " vel = " << pos.speed_kts << endl;
+
+ fdm->climb_rate = 0; // fps
+ // cout << "climb rate = " << aero->hdota << endl;
+ fdm->v_north = 0.0;
+ fdm->v_east = 0.0;
+ fdm->v_down = 0.0;
+ fdm->v_wind_body_north = 0.0;
+ fdm->v_wind_body_east = 0.0;
+ fdm->v_wind_body_down = 0.0;
+ fdm->stall_warning = 0.0;
+
+ fdm->A_X_pilot = 0.0;
+ fdm->A_Y_pilot = 0.0;
+ fdm->A_Z_pilot = 0.0 /* (should be -G) */;
+
+ // Engine parameters
+ fdm->num_engines = 1;
+ fdm->eng_state[0] = 2;
+ // cout << "state = " << fdm->eng_state[0] << endl;
+ double rpm = ((fdm->vcas - 15.0) / 65.0) * 2000.0 + 500.0;
+ if ( rpm < 0.0 ) { rpm = 0.0; }
+ if ( rpm > 3000.0 ) { rpm = 3000.0; }
+ fdm->rpm[0] = rpm;
+
+ fdm->fuel_flow[0] = 0.0;
+ fdm->egt[0] = 0.0;
+ // cout << "egt = " << aero->EGT << endl;
+ fdm->oil_temp[0] = 0.0;
+ fdm->oil_px[0] = 0.0;
+
+ // Consumables
+ fdm->num_tanks = 2;
+ fdm->fuel_quantity[0] = 0.0;
+ fdm->fuel_quantity[1] = 0.0;
+
+ // Gear and flaps
+ fdm->num_wheels = 3;
+ fdm->wow[0] = 0;
+ fdm->wow[1] = 0;
+ fdm->wow[2] = 0;
+
+ // the following really aren't used in this context
+ fdm->cur_time = 0;
+ fdm->warp = 0;
+ fdm->visibility = 0;
+
+ // cout << "Flap deflection = " << aero->dflap << endl;
+ fdm->left_flap = 0.0;
+ fdm->right_flap = 0.0;
+
+ fdm->elevator = -fdm->theta * 1.0;
+ fdm->elevator_trim_tab = 0.0;
+ fdm->left_flap = 0.0;
+ fdm->right_flap = 0.0;
+ fdm->left_aileron = fdm->phi * 1.0;
+ fdm->right_aileron = -fdm->phi * 1.0;
+ fdm->rudder = 0.0;
+ fdm->nose_wheel = 0.0;
+ fdm->speedbrake = 0.0;
+ fdm->spoilers = 0.0;
+
+ // Convert the net buffer to network format
+ fdm->version = htonl(fdm->version);
+
+ htond(fdm->longitude);
+ htond(fdm->latitude);
+ htond(fdm->altitude);
+ htonf(fdm->agl);
+ htonf(fdm->phi);
+ htonf(fdm->theta);
+ htonf(fdm->psi);
+ htonf(fdm->alpha);
+ htonf(fdm->beta);
+
+ htonf(fdm->phidot);
+ htonf(fdm->thetadot);
+ htonf(fdm->psidot);
+ htonf(fdm->vcas);
+ htonf(fdm->climb_rate);
+ htonf(fdm->v_north);
+ htonf(fdm->v_east);
+ htonf(fdm->v_down);
+ htonf(fdm->v_wind_body_north);
+ htonf(fdm->v_wind_body_east);
+ htonf(fdm->v_wind_body_down);
+
+ htonf(fdm->A_X_pilot);
+ htonf(fdm->A_Y_pilot);
+ htonf(fdm->A_Z_pilot);
+
+ htonf(fdm->stall_warning);
+ htonf(fdm->slip_deg);
+
+ for ( i = 0; i < fdm->num_engines; ++i ) {
+ fdm->eng_state[i] = htonl(fdm->eng_state[i]);
+ htonf(fdm->rpm[i]);
+ htonf(fdm->fuel_flow[i]);
+ htonf(fdm->egt[i]);
+ htonf(fdm->cht[i]);
+ htonf(fdm->mp_osi[i]);
+ htonf(fdm->tit[i]);
+ htonf(fdm->oil_temp[i]);
+ htonf(fdm->oil_px[i]);
+ }
+ fdm->num_engines = htonl(fdm->num_engines);
+
+ for ( i = 0; i < fdm->num_tanks; ++i ) {
+ htonf(fdm->fuel_quantity[i]);
+ }
+ fdm->num_tanks = htonl(fdm->num_tanks);
+
+ for ( i = 0; i < fdm->num_wheels; ++i ) {
+ fdm->wow[i] = htonl(fdm->wow[i]);
+ htonf(fdm->gear_pos[i]);
+ htonf(fdm->gear_steer[i]);
+ htonf(fdm->gear_compression[i]);
+ }
+ fdm->num_wheels = htonl(fdm->num_wheels);
+
+ fdm->cur_time = htonl( fdm->cur_time );
+ fdm->warp = htonl( fdm->warp );
+ htonf(fdm->visibility);
+
+ htonf(fdm->elevator);
+ htonf(fdm->elevator_trim_tab);
+ htonf(fdm->left_flap);
+ htonf(fdm->right_flap);
+ htonf(fdm->left_aileron);
+ htonf(fdm->right_aileron);
+ htonf(fdm->rudder);
+ htonf(fdm->nose_wheel);
+ htonf(fdm->speedbrake);
+ htonf(fdm->spoilers);
+}
+
+
+static void send_data( gps *gpspacket, imu *imupacket, nav *navpacket,
+ servo *servopacket ) {
+ int len;
+ int fdmsize = sizeof( FGNetFDM );
+
+ // cout << "Running main loop" << endl;
+
+ FGNetFDM fgfdm;
+ FGNetCtrls fgctrls;
+
+ ugear2fg( gpspacket, imupacket, navpacket, servopacket, &fgfdm, &fgctrls );
+ len = fdm_sock.send(&fgfdm, fdmsize, 0);
+}
+
+
+void usage( const string &argv0 ) {
+ cout << "Usage: " << argv0 << endl;
+ cout << "\t[ --help ]" << endl;
+ cout << "\t[ --infile <infile_name>" << 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[ --host <hostname> ]" << endl;
+ cout << "\t[ --broadcast ]" << endl;
+ cout << "\t[ --fdm-port <fdm output port #> ]" << endl;
+ cout << "\t[ --ctrls-port <ctrls output port #> ]" << endl;
+ cout << "\t[ --altitude-offset <meters> ]" << endl;
+ cout << "\t[ --skip-seconds <seconds> ]" << endl;
+}
+
+
+int main( int argc, char **argv ) {
+ double hertz = 60.0;
+ string out_host = "localhost";
+ bool do_broadcast = false;
+
+ // process command line arguments
+ for ( int i = 1; i < argc; ++i ) {
+ if ( strcmp( argv[i], "--help" ) == 0 ) {
+ usage( argv[0] );
+ exit( 0 );
+ } else if ( strcmp( argv[i], "--hertz" ) == 0 ) {
+ ++i;
+ if ( i < argc ) {
+ hertz = atof( argv[i] );
+ } else {
+ usage( argv[0] );
+ exit( -1 );
+ }
+ } else if ( strcmp( argv[i], "--infile" ) == 0 ) {
+ ++i;
+ if ( i < argc ) {
+ infile = argv[i];
+ } else {
+ usage( argv[0] );
+ exit( -1 );
+ }
+ } else if ( strcmp( argv[i], "--outfile" ) == 0 ) {
+ ++i;
+ if ( i < argc ) {
+ outfile = argv[i];
+ } else {
+ usage( argv[0] );
+ exit( -1 );
+ }
+ } else if ( strcmp( argv[i], "--serial" ) == 0 ) {
+ ++i;
+ if ( i < argc ) {
+ serialdev = argv[i];
+ } else {
+ usage( argv[0] );
+ exit( -1 );
+ }
+ } else if ( strcmp( argv[i], "--host" ) == 0 ) {
+ ++i;
+ if ( i < argc ) {
+ out_host = argv[i];
+ } else {
+ usage( argv[0] );
+ exit( -1 );
+ }
+ } else if ( strcmp( argv[i], "--broadcast" ) == 0 ) {
+ do_broadcast = true;
+ } else if ( strcmp( argv[i], "--fdm-port" ) == 0 ) {
+ ++i;
+ if ( i < argc ) {
+ fdm_port = atoi( argv[i] );
+ } else {
+ usage( argv[0] );
+ exit( -1 );
+ }
+ } else if ( strcmp( argv[i], "--ctrls-port" ) == 0 ) {
+ ++i;
+ if ( i < argc ) {
+ ctrls_port = atoi( argv[i] );
+ } else {
+ usage( argv[0] );
+ exit( -1 );
+ }
+ } else if ( strcmp( argv[i], "--altitude-offset" ) == 0 ) {
+ ++i;
+ if ( i < argc ) {
+ alt_offset = atof( argv[i] );
+ } else {
+ usage( argv[0] );
+ exit( -1 );
+ }
+ } else if ( strcmp( argv[i], "--skip-seconds" ) == 0 ) {
+ ++i;
+ if ( i < argc ) {
+ skip = atof( argv[i] );
+ } else {
+ usage( argv[0] );
+ exit( -1 );
+ }
+ } else {
+ usage( argv[0] );
+ exit( -1 );
+ }
+ }
+
+ // Setup up outgoing network connections
+
+ netInit( &argc,argv ); // We must call this before any other net stuff
+
+ if ( ! fdm_sock.open( false ) ) { // open a UDP socket
+ cout << "error opening fdm output socket" << endl;
+ return -1;
+ }
+ if ( ! ctrls_sock.open( false ) ) { // open a UDP socket
+ cout << "error opening ctrls output socket" << endl;
+ return -1;
+ }
+ cout << "open net channels" << endl;
+
+ fdm_sock.setBlocking( false );
+ ctrls_sock.setBlocking( false );
+ cout << "blocking false" << endl;
+
+ if ( do_broadcast ) {
+ fdm_sock.setBroadcast( true );
+ ctrls_sock.setBroadcast( true );
+ }
+
+ if ( fdm_sock.connect( out_host.c_str(), fdm_port ) == -1 ) {
+ perror("connect");
+ cout << "error connecting to outgoing fdm port: " << out_host
+ << ":" << fdm_port << endl;
+ return -1;
+ }
+ cout << "connected outgoing fdm socket" << endl;
+
+ if ( ctrls_sock.connect( out_host.c_str(), ctrls_port ) == -1 ) {
+ perror("connect");
+ cout << "error connecting to outgoing ctrls port: " << out_host
+ << ":" << ctrls_port << endl;
+ return -1;
+ }
+ cout << "connected outgoing ctrls socket" << endl;
+
+ if ( infile.length() ) {
+ // Load data from a track data
+ track.load( infile );
+ cout << "Loaded " << track.gps_size() << " gps records." << endl;
+ cout << "Loaded " << track.imu_size() << " imu records." << endl;
+ cout << "Loaded " << track.nav_size() << " nav records." << endl;
+ cout << "Loaded " << track.servo_size() << " servo records." << endl;
+
+ int size = track.imu_size();
+
+ double current_time = track.get_imupt(0).time;
+ cout << "Track begin time is " << current_time << endl;
+ double end_time = track.get_imupt(size-1).time;
+ cout << "Track end time is " << end_time << endl;
+ cout << "Duration = " << end_time - current_time << endl;
+
+ // advance skip seconds forward
+ current_time += skip;
+
+ frame_us = 1000000.0 / hertz;
+ if ( frame_us < 0.0 ) {
+ frame_us = 0.0;
+ }
+
+ SGTimeStamp start_time;
+ start_time.stamp();
+ int gps_count = 0;
+ int imu_count = 0;
+ int nav_count = 0;
+ int servo_count = 0;
+
+ gps gps0, gps1;
+ gps0 = gps1 = track.get_gpspt( 0 );
+
+ imu imu0, imu1;
+ imu0 = imu1 = track.get_imupt( 0 );
+
+ nav nav0, nav1;
+ nav0 = nav1 = track.get_navpt( 0 );
+
+ servo servo0, servo1;
+ servo0 = servo1 = track.get_servopt( 0 );
+
+ 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() )
+ {
+ gps0 = gps1;
+ ++gps_count;
+ // cout << "count = " << count << endl;
+ gps1 = track.get_gpspt( gps_count );
+ }
+ // cout << "p0 = " << p0.get_time() << " p1 = " << p1.get_time()
+ // << endl;
+
+ // Advance imu pointer
+ while ( current_time > imu1.time
+ && imu_count < track.imu_size() )
+ {
+ imu0 = imu1;
+ ++imu_count;
+ // cout << "count = " << count << endl;
+ imu1 = track.get_imupt( imu_count );
+ }
+ // cout << "pos0 = " << pos0.get_seconds()
+ // << " pos1 = " << pos1.get_seconds() << endl;
+
+ // Advance nav pointer
+ while ( current_time > nav1.time
+ && nav_count < track.nav_size() )
+ {
+ nav0 = nav1;
+ ++nav_count;
+ // cout << "count = " << count << endl;
+ nav1 = track.get_navpt( nav_count );
+ }
+ // cout << "pos0 = " << pos0.get_seconds()
+ // << " pos1 = " << pos1.get_seconds() << endl;
+
+ // Advance servo pointer
+ while ( current_time > servo1.time
+ && servo_count < track.servo_size() )
+ {
+ servo0 = servo1;
+ ++servo_count;
+ // cout << "count = " << count << endl;
+ servo1 = track.get_servopt( servo_count );
+ }
+ // cout << "pos0 = " << pos0.get_seconds()
+ // << " pos1 = " << pos1.get_seconds() << endl;
+
+ double gps_percent;
+ if ( fabs(gps1.time - gps0.time) < 0.00001 ) {
+ gps_percent = 0.0;
+ } else {
+ gps_percent =
+ (current_time - gps0.time) /
+ (gps1.time - gps0.time);
+ }
+ // cout << "Percent = " << percent << endl;
+
+ double imu_percent;
+ if ( fabs(imu1.time - imu0.time) < 0.00001 ) {
+ imu_percent = 0.0;
+ } else {
+ imu_percent =
+ (current_time - imu0.time) /
+ (imu1.time - imu0.time);
+ }
+ // cout << "Percent = " << percent << endl;
+
+ double nav_percent;
+ if ( fabs(nav1.time - nav0.time) < 0.00001 ) {
+ nav_percent = 0.0;
+ } else {
+ nav_percent =
+ (current_time - nav0.time) /
+ (nav1.time - nav0.time);
+ }
+ // cout << "Percent = " << percent << endl;
+
+ double servo_percent;
+ if ( fabs(servo1.time - servo0.time) < 0.00001 ) {
+ servo_percent = 0.0;
+ } else {
+ servo_percent =
+ (current_time - servo0.time) /
+ (servo1.time - servo0.time);
+ }
+ // cout << "Percent = " << percent << endl;
+
+ gps gpspacket = UGEARInterpGPS( gps0, gps1, gps_percent );
+ imu imupacket = UGEARInterpIMU( imu0, imu1, imu_percent );
+ nav navpacket = UGEARInterpNAV( nav0, nav1, nav_percent );
+ servo servopacket = UGEARInterpSERVO( servo0, servo1,
+ servo_percent );
+
+ // cout << current_time << " " << p0.lat_deg << ", " << p0.lon_deg
+ // << endl;
+ // cout << current_time << " " << p1.lat_deg << ", " << p1.lon_deg
+ // << endl;
+ // 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 );
+ }
+
+ send_data( &gpspacket, &imupacket, &navpacket, &servopacket );
+
+ // 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();
+ }
+
+ current_time += (frame_us / 1000000.0);
+ last_time_stamp = current_time_stamp;
+ }
+
+ cout << "Processed " << imu_count << " entries in "
+ << (current_time_stamp - start_time) / 1000000 << " seconds."
+ << endl;
+ } else if ( serialdev.length() ) {
+ // process incoming data from the serial port
+
+ int count = 0;
+ double current_time = 0.0;
+
+ gps gpspacket;
+ imu imupacket;
+ nav navpacket;
+ servo servopacket;
+
+ double gps_time = 0;
+ double imu_time = 0;
+ double nav_time = 0;
+ double servo_time = 0;
+
+ // open the serial port device
+ SGSerialPort input( serialdev, 115200 );
+ if ( !input.is_enabled() ) {
+ cout << "Cannot open: " << serialdev << endl;
+ return false;
+ }
+
+ // open up the data log file if requested
+ if ( !outfile.length() ) {
+ cout << "no --outfile <name> specified, cannot capture data!"
+ << endl;
+ return false;
+ }
+ SGFile output( outfile );
+ if ( !output.open( SG_IO_OUT ) ) {
+ cout << "Cannot open: " << outfile << endl;
+ return false;
+ }
+
+ while ( input.is_enabled() ) {
+ // cout << "looking for next message ..." << endl;
+ int id = track.next_message( &input, &output, &gpspacket,
+ &imupacket, &navpacket, &servopacket );
+ cout << "message id = " << id << endl;
+ count++;
+
+ if ( id == GPS_PACKET ) {
+ if ( gpspacket.time > gps_time ) {
+ gps_time = gpspacket.time;
+ current_time = gps_time;
+ } else {
+ cout << "oops gps back in time" << endl;
+ }
+ } else if ( id == IMU_PACKET ) {
+ if ( imupacket.time > imu_time ) {
+ imu_time = imupacket.time;
+ current_time = imu_time;
+ } else {
+ cout << "oops imu back in 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;
+ }
+ } 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;
+ }
+ }
+
+ // if ( gpspacket.lat > -500 ) {
+ printf( "%.3f %.4f %.4f %.1f %.2f %.2f %.2f\n",
+ current_time,
+ gpspacket.lat, gpspacket.lon, gpspacket.alt,
+ imupacket.phi, imupacket.the, imupacket.psi );
+ // }
+
+ send_data( &gpspacket, &imupacket, &navpacket, &servopacket );
+ }
+ }
+
+ return 0;
+}
projects / flightgear.git / commitdiff