utils/GPSsmooth/UGear.hxx

   1 #ifndef _FG_UGEAR_II_HXX
   2 #define _FG_UGEAR_II_HXX
   3
   4
   5 #ifdef HAVE_CONFIG_H
   6 #  include <config.h>
   7 #endif
   8
   9 #include <simgear/compiler.h>
  10
  11 #include <iostream>
  12 #include <string>
  13 #include <vector>
  14
  15 #include <simgear/misc/stdint.hxx>
  16 #include <simgear/io/iochannel.hxx>
  17 #include <simgear/serial/serial.hxx>
  18
  19 using std::cout;
  20 using std::endl;
  21 using std::string;
  22 using std::vector;
  23
  24
  25 enum ugPacketType {
  26     GPS_PACKET = 0,
  27     IMU_PACKET = 1,
  28     NAV_PACKET = 2,
  29     SERVO_PACKET = 3,
  30     HEALTH_PACKET = 4
  31 };
  32
  33 struct imu {
  34    double time;
  35    double p,q,r;                /* angular velocities    */
  36    double ax,ay,az;             /* acceleration          */
  37    double hx,hy,hz;             /* magnetic field        */
  38    double Ps,Pt;                /* static/pitot pressure */
  39    // double Tx,Ty,Tz;          /* temperature           */
  40    double phi,the,psi;          /* attitudes             */
  41    uint64_t err_type;           /* error type            */
  42 };
  43
  44 struct gps {
  45    double time;
  46    double lat,lon,alt;          /* gps position          */
  47    double ve,vn,vd;             /* gps velocity          */
  48    double ITOW;
  49    uint64_t err_type;           /* error type            */
  50 };
  51
  52 struct nav {
  53    double time;
  54    double lat,lon,alt;
  55    double ve,vn,vd;
  56    // float  t;
  57    uint64_t err_type;
  58 };
  59
  60 struct servo {
  61    double time;
  62    uint16_t chn[8];
  63    uint64_t status;
  64 };
  65
  66 struct health {
  67     double time;
  68     double target_roll_deg;     /* AP target roll angle */
  69     double target_heading_deg;  /* AP target heading angle */
  70     double target_pitch_deg;    /* AP target pitch angle */
  71     double target_climb_fps;    /* AP target climb rate */
  72     double target_altitude_ft;  /* AP target altitude */
  73     uint64_t command_sequence;  /* highest received command sequence num */
  74     uint64_t target_waypoint;   /* index of current waypoint target */
  75     uint64_t loadavg;           /* system "1 minute" load average */
  76     uint64_t ahrs_hz;           /* actual ahrs loop hz */
  77     uint64_t nav_hz;            /* actual nav loop hz */
  78 };
  79
  80 // Manage a saved ugear log (track file)
  81 class UGTrack {
  82
  83 private:
  84
  85     vector <gps> gps_data;
  86     vector <imu> imu_data;
  87     vector <nav> nav_data;
  88     vector <servo> servo_data;
  89     vector <health> health_data;
  90
  91     // parse message and put current data into vector if message has a
  92     // newer time stamp than existing data.
  93     void parse_msg( const int id, char *buf,
  94                     gps *gpspacket, imu *imupacket, nav *navpacket,
  95                     servo *servopacket, health *healthpacket );
  96
  97     // activate special double swap logic for non-standard stargate
  98     // double format
  99     bool sg_swap;
 100
 101 public:
 102
 103     UGTrack();
 104     ~UGTrack();
 105
 106     // read/parse the next message from the specified data stream,
 107     // returns id # if a valid message found.
 108     int next_message( SGIOChannel *ch, SGIOChannel *log,
 109                       gps *gpspacket, imu *imupacket, nav *navpacket,
 110                       servo *servopacket, health * healthpacket,
 111                       bool ignore_checksum );
 112     int next_message( SGSerialPort *serial, SGIOChannel *log,
 113                       gps *gpspacket, imu *imupacket, nav *navpacket,
 114                       servo *servopacket, health *healthpacket,
 115                       bool ignore_checksum );
 116
 117     // load the named stream log file into internal buffers
 118     bool load_stream( const string &file, bool ignore_checksum );
 119
 120     // load the named flight files into internal buffers
 121     bool load_flight( const string &path );
 122
 123     inline int gps_size() const { return gps_data.size(); }
 124     inline int imu_size() const { return imu_data.size(); }
 125     inline int nav_size() const { return nav_data.size(); }
 126     inline int servo_size() const { return servo_data.size(); }
 127     inline int health_size() const { return health_data.size(); }
 128
 129     inline gps get_gpspt( const unsigned int i )
 130     {
 131         if ( i < gps_data.size() ) {
 132             return gps_data[i];
 133         } else {
 134             return gps();
 135         }
 136     }
 137     inline imu get_imupt( const unsigned int i )
 138     {
 139         if ( i < imu_data.size() ) {
 140             return imu_data[i];
 141         } else {
 142             return imu();
 143         }
 144     }
 145     inline nav get_navpt( const unsigned int i )
 146     {
 147         if ( i < nav_data.size() ) {
 148             return nav_data[i];
 149         } else {
 150             return nav();
 151         }
 152     }
 153     inline servo get_servopt( const unsigned int i )
 154     {
 155         if ( i < servo_data.size() ) {
 156             return servo_data[i];
 157         } else {
 158             return servo();
 159         }
 160     }
 161     inline health get_healthpt( const unsigned int i )
 162     {
 163         if ( i < health_data.size() ) {
 164             return health_data[i];
 165         } else {
 166             return health();
 167         }
 168     }
 169
 170
 171     // set stargate mode where we have to do an odd swapping of doubles to
 172     // account for their non-standard formate
 173     inline void set_stargate_swap_mode() {
 174         sg_swap = true;
 175     }
 176 };
 177
 178
 179 gps UGEARInterpGPS( const gps A, const gps B, const double percent );
 180 imu UGEARInterpIMU( const imu A, const imu B, const double percent );
 181 nav UGEARInterpNAV( const nav A, const nav B, const double percent );
 182 servo UGEARInterpSERVO( const servo A, const servo B, const double percent );
 183 health UGEARInterpHEALTH( const health A, const health B,
 184                           const double percent );
 185
 186
 187 #endif // _FG_UGEAR_II_HXX