fly/flightgear
1
0
Fork 0
Code Activity

Initial revision of code to read MicroGear serial output and parse,

Browse source 
interpolate, and feed to FlightGear either in real time or replaying the
data file.
This commit is contained in:
curt 2006-11-11 18:43:17 +00:00
parent 33037b2139
commit 651a21f9d5
7 changed files with 1406 additions and 19 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

97
utils/GPSsmooth/MIDG-II.cxx
View file

@ -377,11 +377,12 @@ bool MIDGTrack::load( const string &file ) {
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 ) {
@ -391,6 +392,23 @@ int myread( SGIOChannel *ch, SGIOChannel *log, char *buf, int length ) {
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 )
@ -409,7 +427,9 @@ int MIDGTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
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();
}
@ -429,9 +449,13 @@ int MIDGTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
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
@ -443,11 +467,70 @@ int MIDGTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
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 ) {

3
utils/GPSsmooth/MIDG-II.hxx
View file

@ -14,6 +14,7 @@
#include <simgear/misc/stdint.hxx>
#include <simgear/io/iochannel.hxx>
#include <simgear/serial/serial.hxx>
SG_USING_STD(cout);
SG_USING_STD(endl);
@ -133,6 +134,8 @@ public:
// 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 );

23
utils/GPSsmooth/MIDG_main.cxx
View file

@ -519,7 +519,7 @@ int main( int argc, char **argv ) {
// << 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,
@ -569,8 +569,8 @@ int main( int argc, char **argv ) {
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;
}
@ -587,9 +587,10 @@ int main( int argc, char **argv ) {
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 ) {
@ -608,13 +609,13 @@ int main( int argc, char **argv ) {
}
}
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 );

12
utils/GPSsmooth/Makefile.am
View file

@ -1,4 +1,4 @@
noinst_PROGRAMS = GPSsmooth MIDGsmooth
noinst_PROGRAMS = GPSsmooth MIDGsmooth UGsmooth
GPSsmooth_SOURCES = \
gps.cxx gps.hxx \
@ -17,4 +17,14 @@ MIDGsmooth_LDADD = \
-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

437
utils/GPSsmooth/UGear.cxx Normal file
View file

@ -0,0 +1,437 @@
#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;
}

147
utils/GPSsmooth/UGear.hxx Normal file
View file

@ -0,0 +1,147 @@
#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

706
utils/GPSsmooth/UGear_main.cxx Normal file
View file

@ -0,0 +1,706 @@
#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;
}