1
0
Fork 0
flightgear/utils/GPSsmooth/UGear.hxx

184 lines
4.9 KiB
C++
Raw Normal View History

#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,
2006-11-22 04:16:07 +00:00
SERVO_PACKET = 3,
HEALTH_PACKET = 4
};
struct imu {
double time;
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 */
uint64_t err_type; /* error type */
};
struct gps {
double time;
double lat,lon,alt; /* gps position */
double ve,vn,vd; /* gps velocity */
double ITOW;
uint64_t err_type; /* error type */
};
struct nav {
double time;
double lat,lon,alt;
double ve,vn,vd;
// float t;
uint64_t err_type;
};
struct servo {
double time;
2007-12-05 19:01:49 +00:00
uint16_t chn[8];
uint64_t status;
};
2006-11-22 04:16:07 +00:00
struct health {
double time;
2006-11-22 04:16:07 +00:00
float volts_raw; /* raw volt reading */
float volts; /* filtered volts */
uint32_t est_seconds; /* estimated useful seconds remaining */
uint32_t loadavg; /* system "1 minute" load average */
uint32_t ahrs_hz; /* actual ahrs loop hz */
uint32_t nav_hz; /* actual nav loop hz */
2006-11-22 04:16:07 +00:00
};
// 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;
2006-11-22 04:16:07 +00:00
vector <health> health_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,
2006-11-22 04:16:07 +00:00
servo *servopacket, health *healthpacket );
// activate special double swap logic for non-standard stargate
// double format
bool sg_swap;
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, health * healthpacket,
bool ignore_checksum );
int next_message( SGSerialPort *serial, SGIOChannel *log,
gps *gpspacket, imu *imupacket, nav *navpacket,
servo *servopacket, health *healthpacket,
bool ignore_checksum );
// load the named stream log file into internal buffers
bool load_stream( const string &file, bool ignore_checksum );
// load the named flight files into internal buffers
bool load_flight( const string &path );
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(); }
2006-11-22 04:16:07 +00:00
inline int health_size() const { return health_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();
}
}
2006-11-22 04:16:07 +00:00
inline health get_healthpt( const unsigned int i )
{
if ( i < health_data.size() ) {
return health_data[i];
} else {
return health();
}
}
// set stargate mode where we have to do an odd swapping of doubles to
// account for their non-standard formate
inline void set_stargate_swap_mode() {
sg_swap = true;
}
};
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 );
2006-11-22 04:16:07 +00:00
health UGEARInterpHEALTH( const health A, const health B,
const double percent );
#endif // _FG_UGEAR_II_HXX