fly/flightgear
1
0
Fork 0
Code Activity
flightgear/utils/GPSsmooth/MIDG-II.cxx

438 lines
14 KiB
C++
Raw Blame History

#include <iostream>
#include <plib/ul.h>
#include <simgear/constants.h>
#include <simgear/math/sg_geodesy.hxx>
#include <simgear/misc/sgstream.hxx>
#include <simgear/misc/strutils.hxx>
#include "MIDG-II.hxx"
SG_USING_STD(cout);
SG_USING_STD(endl);
MIDGTrack::MIDGTrack() {};
MIDGTrack::~MIDGTrack() {};
static uint32_t read_swab( char *buf, size_t offset, size_t size ) {
uint32_t result = 0;
char *ptr = buf + offset;
// MIDG data is big endian so swap if needed.
if ( ulIsLittleEndian ) {
if ( size == 4 ) {
ulEndianSwap( (uint32_t *)ptr );
} else if ( size == 2 ) {
ulEndianSwap( (uint16_t *)ptr );
}
}
if ( size == 4 ) {
result = *(uint32_t *)ptr;
} else if ( size == 2 ) {
result = *(uint16_t *)ptr;
} else if ( size == 1 ) {
result = *(uint8_t *)ptr;
} else {
cout << "unknown size in read_swab()" << endl;
}
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;
c0 += size;
c1 += c0;
for ( uint8_t i = 0; i < size; i++ ) {
c0 += (uint8_t)buf[i];
c1 += c0;
}
// 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 MIDGTrack::parse_msg( const int id, char *buf, MIDGpos *pos, MIDGatt *att )
{
if ( id == 1 ) {
uint32_t ts;
uint16_t status;
int16_t temp;
// cout << "message 1 =" << endl;
// timestamp
ts = (uint32_t)read_swab( buf, 0, 4 );
// cout << " time stamp = " << ts << endl;
// status
status = (uint16_t)read_swab( buf, 4, 2 );
// cout << " status = " << status << endl;
// temp
temp = (int16_t)read_swab( buf, 6, 2 );
// cout << " temp = " << temp << endl;
} else if ( id == 2 ) {
uint32_t ts;
int16_t p, q, r;
int16_t ax, ay, az;
int16_t mx, my, mz;
uint8_t flags;
// cout << "message 2 =" << endl;
// timestamp
ts = (uint32_t)read_swab( buf, 0, 4 );
// cout << " time stamp = " << ts << endl;
// p, q, r
p = (int16_t)read_swab( buf, 4, 2 );
q = (int16_t)read_swab( buf, 6, 2 );
r = (int16_t)read_swab( buf, 8, 2 );
// cout << " pqr = " << p << "," << q << "," << r << endl;
// ax, ay, az
ax = (int16_t)read_swab( buf, 10, 2 );
ay = (int16_t)read_swab( buf, 12, 2 );
az = (int16_t)read_swab( buf, 14, 2 );
// cout << " ax ay az = " << ax << "," << ay << "," << az << endl;
// mx, my, mz
mx = (int16_t)read_swab( buf, 16, 2 );
my = (int16_t)read_swab( buf, 18, 2 );
mz = (int16_t)read_swab( buf, 20, 2 );
// cout << " mx my mz = " << mx << "," << my << "," << mz << endl;
// flags
flags = (uint8_t)read_swab( buf, 22, 1 );
// cout << " GPS 1PPS flag = " << (int)(flags & (1 << 6))
// << " Timestamp is gps = " << (int)(flags & (1 << 7)) << endl;
} else if ( id == 3 ) {
uint32_t ts;
int16_t mx, my, mz;
uint8_t flags;
// cout << "message 3 =" << endl;
// timestamp
ts = (uint32_t)read_swab( buf, 0, 4 );
// cout << " time stamp = " << ts << endl;
// mx, my, mz
mx = (int16_t)read_swab( buf, 4, 2 );
my = (int16_t)read_swab( buf, 6, 2 );
mz = (int16_t)read_swab( buf, 8, 2 );
// cout << " mx my mz = " << mx << "," << my << "," << mz << endl;
// flags
flags = (uint8_t)read_swab( buf, 10, 1 );
// cout << " GPS 1PPS flag = " << (int)(flags & (1 << 6)) << endl;
} else if ( id == 10 ) {
uint32_t ts;
int16_t p, q, r;
int16_t ax, ay, az;
int16_t yaw, pitch, roll;
int32_t Qw, Qx, Qy, Qz;
uint8_t flags;
// cout << "message 10 =" << endl;
// timestamp
ts = (uint32_t)read_swab( buf, 0, 4 );
// cout << " time stamp = " << ts << endl;
if ( ts > att->get_msec() && att->get_msec() > 1.0 ) {
attdata.push_back( *att );
}
if ( ts < att->get_msec() ) {
cout << "OOOPS moving back in time!!! " << ts << " < " << att->get_msec() << endl;
} else {
att->midg_time = MIDGTime( ts );
}
// p, q, r
p = (int16_t)read_swab( buf, 4, 2 );
q = (int16_t)read_swab( buf, 6, 2 );
r = (int16_t)read_swab( buf, 8, 2 );
// cout << " pqr = " << p << "," << q << "," << r << endl;
// ax, ay, az
ax = (int16_t)read_swab( buf, 10, 2 );
ay = (int16_t)read_swab( buf, 12, 2 );
az = (int16_t)read_swab( buf, 14, 2 );
// cout << " ax ay az = " << ax << "," << ay << "," << az << endl;
// yaw, pitch, roll
yaw = (int16_t)read_swab( buf, 16, 2 );
pitch = (int16_t)read_swab( buf, 18, 2 );
roll = (int16_t)read_swab( buf, 20, 2 );
// cout << " yaw, pitch, roll = " << yaw << "," << pitch << ","
// << roll << endl;
att->yaw_rad = ( (double)yaw / 100.0 ) * SG_PI / 180.0;
att->pitch_rad = ( (double)pitch / 100.0 ) * SG_PI / 180.0;
att->roll_rad = ( (double)roll / 100.0 ) * SG_PI / 180.0;
// Qw, Qx, Qy, Qz
Qw = (int32_t)read_swab( buf, 22, 4 );
Qx = (int32_t)read_swab( buf, 26, 4 );
Qy = (int32_t)read_swab( buf, 30, 4 );
Qz = (int32_t)read_swab( buf, 34, 4 );
// cout << " Qw,Qx,Qy,Qz = " << Qw << "," << Qx << "," << Qy << ","
// << Qz << endl;
// flags
flags = (uint8_t)read_swab( buf, 38, 1 );
// cout << " External hdg measurement applied = "
// << (int)(flags & (1 << 3)) << endl
// << " Magnatometer measurement applied = "
// << (int)(flags & (1 << 4)) << endl
// << " DGPS = " << (int)(flags & (1 << 5)) << endl
// << " Timestamp is gps = " << (int)(flags & (1 << 6)) << endl
// << " INS mode = " << (int)(flags & (1 << 7))
// << endl;
} else if ( id == 12 ) {
uint32_t ts;
int32_t posx, posy, posz;
int32_t velx, vely, velz;
uint8_t flags;
// cout << "message 12 =" << endl;
// timestamp
ts = (uint32_t)read_swab( buf, 0, 4 );
// cout << " time stamp = " << ts << endl;
if ( ts > pos->get_msec() && pos->get_msec() > 1.0 ) {
posdata.push_back( *pos );
}
if ( ts < pos->get_msec() ) {
cout << "OOOPS moving back in time!!! " << ts << " < " << pos->get_msec() << endl;
} else {
pos->midg_time = MIDGTime( ts );
}
// posx, posy, posz
posx = (int32_t)read_swab( buf, 4, 4 );
posy = (int32_t)read_swab( buf, 8, 4 );
posz = (int32_t)read_swab( buf, 12, 4 );
// cout << " pos = " << posx << "," << posy << "," << posz << endl;
double xyz[3];
xyz[0] = posx/100; xyz[1] = posy/100; xyz[2] = posz/100;
double lat, lon, alt;
sgCartToGeod(xyz, &lat, &lon, &alt);
pos->lat_deg = lat * 180.0 / SG_PI;
pos->lon_deg = lon * 180.0 / SG_PI;
pos->altitude_msl = alt;
// cout << " lon = " << pos->lon_deg << " lat = " << pos->lat_deg
// << " alt = " << pos->altitude_msl << endl;
// velx, vely, velz
velx = (int32_t)read_swab( buf, 16, 4 );
vely = (int32_t)read_swab( buf, 20, 4 );
velz = (int32_t)read_swab( buf, 24, 4 );
// cout << " vel = " << velx << "," << vely << "," << velz << endl;
double vel_cms = sqrt( velx*velx + vely*vely + velz*velz );
double vel_ms = vel_cms / 100.0;
pos->speed_kts = vel_ms * SG_METER_TO_NM * 3600;
// flags
flags = (uint8_t)read_swab( buf, 28, 1 );
// cout << " ENU pos rel to 1st fix = " << (int)(flags & (1 << 0)) << endl
// << " Velocity format = " << (int)(flags & (1 << 1)) << endl
// << " bit 2 = " << (int)(flags & (1 << 2)) << endl
// << " bit 3 = " << (int)(flags & (1 << 3)) << endl
// << " GPS pos/vel valid = " << (int)(flags & (1 << 4)) << endl
// << " DGPS = " << (int)(flags & (1 << 5)) << endl
// << " Timestamp is gps = " << (int)(flags & (1 << 6)) << endl
// << " Solution src (0=gps, 1=ins) = " << (int)(flags & (1 << 7))
// << endl;
} else if ( id == 20 ) {
uint32_t gps_ts, gps_week;
uint16_t details;
int32_t gps_posx, gps_posy, gps_posz;
int32_t gps_velx, gps_vely, gps_velz;
int16_t pdop, pacc, sacc;
// cout << "message 20 =" << endl;
// timestamp -- often slightly off from midg time stamp so
// let's not use gps ts to determine if we need to push the
// previous data or not, just roll it into the current data
// independent of time stamp.
gps_ts = (uint32_t)read_swab( buf, 0, 4 );
// if ( ts > pt->get_msec() && pt->get_msec() > 1.0 ) {
// data.push_back( *pt );
// }
// if ( ts < pt->get_msec() ) {
// cout << "OOOPS moving back in time!!! " << ts << " < " << pt->get_msec() << endl;
// } else {
// pt->midg_time = MIDGTime( ts );
// }
gps_week = (uint16_t)read_swab( buf, 4, 2 );
// cout << " gps time stamp = " << gps_ts << " week = " << gps_week
// << endl;
// details
details = (uint16_t)read_swab( buf, 6, 2 );
// cout << " details = " << details << endl;
// gps_posx, gps_posy, gps_posz
gps_posx = (int32_t)read_swab( buf, 8, 4 );
gps_posy = (int32_t)read_swab( buf, 12, 4 );
gps_posz = (int32_t)read_swab( buf, 16, 4 );
// cout << " gps_pos = " << gps_posx << "," << gps_posy << ","
// << gps_posz << endl;
// gps_velx, gps_vely, gps_velz
gps_velx = (int32_t)read_swab( buf, 20, 4 );
gps_vely = (int32_t)read_swab( buf, 24, 4 );
gps_velz = (int32_t)read_swab( buf, 28, 4 );
// cout << " gps_vel = " << gps_velx << "," << gps_vely << ","
// << gps_velz << endl;
// position dop
pdop = (uint16_t)read_swab( buf, 32, 2 );
// cout << " pdop = " << pdop << endl;
// position accuracy
pacc = (uint16_t)read_swab( buf, 34, 2 );
// cout << " pacc = " << pacc << endl;
// speed accuracy
sacc = (uint16_t)read_swab( buf, 36, 2 );
// cout << " sacc = " << sacc << endl;
} else {
cout << "unknown id = " << id << endl;
}
}
// load the specified file, return the number of records loaded
int MIDGTrack::load( const string &file ) {
int count = 0;
posdata.clear();
attdata.clear();
// openg the file
fd = fopen( file.c_str(), "r" );
if ( fd == NULL ) {
cout << "Cannot open file: " << file << endl;
return 0;
}
vector <string> tokens;
MIDGpos pos;
MIDGatt att;
while ( ! feof( fd ) ) {
// scan for sync characters
int sync0, sync1;
sync0 = fgetc( fd );
sync1 = fgetc( fd );
while ( (sync0 != 129 || sync1 != 161) && !feof(fd) ) {
sync0 = sync1;
sync1 = fgetc( fd );
}
// cout << "start of message ..." << endl;
// read message id and size
int id = fgetc( fd );
int size = fgetc( fd );
// cout << "message = " << id << " size = " << size << endl;
// load message
char buf[256];
fread( buf, size, 1, fd );
// read checksum
int cksum0 = fgetc( fd );
int cksum1 = fgetc( fd );
if ( validate_cksum( id, size, buf, cksum0, cksum1 ) ) {
parse_msg( id, buf, &pos, &att );
} else {
cout << "Check sum failure!" << endl;
}
}
return count;
}
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;
}
MIDGpos MIDGInterpPos( const MIDGpos A, const MIDGpos B, const double percent )
{
MIDGpos p;
p.midg_time = MIDGTime((uint32_t)interp(A.midg_time.get_msec(),
B.midg_time.get_msec(),
percent));
p.lat_deg = interp(A.lat_deg, B.lat_deg, percent);
p.lon_deg = interp(A.lon_deg, B.lon_deg, percent);
p.altitude_msl = interp(A.altitude_msl, B.altitude_msl, percent);
p.fix_quality = (int)interp(A.fix_quality, B.fix_quality, percent);
p.num_satellites = (int)interp(A.num_satellites, B.num_satellites, percent);
p.hdop = interp(A.hdop, B.hdop, percent);
p.speed_kts = interp(A.speed_kts, B.speed_kts, percent);
p.course_true = interp(A.course_true, B.course_true, percent, true);
return p;
}
MIDGatt MIDGInterpAtt( const MIDGatt A, const MIDGatt B, const double percent )
{
MIDGatt p;
p.midg_time = MIDGTime((uint32_t)interp(A.midg_time.get_msec(),
B.midg_time.get_msec(),
percent));
p.yaw_rad = interp(A.yaw_rad, B.yaw_rad, percent, true);
p.pitch_rad = interp(A.pitch_rad, B.pitch_rad, percent, true);
p.roll_rad = interp(A.roll_rad, B.roll_rad, percent, true);
return p;
}
View git blame Copy permalink