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 SG_USING_STD(cout);
  20 SG_USING_STD(endl);
  21 SG_USING_STD(string);
  22 SG_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     float volts_raw;            /* raw volt reading */
  69     float volts;                /* filtered volts */
  70     uint32_t est_seconds;       /* estimated useful seconds remaining */
  71     uint32_t loadavg;            /* system "1 minute" load average */
  72     uint32_t ahrs_hz;            /* actual ahrs loop hz */
  73     uint32_t nav_hz;             /* actual nav loop hz */
  74 };
  75
  76 // Manage a saved ugear log (track file)
  77 class UGEARTrack {
  78
  79 private:
  80
  81     vector <gps> gps_data;
  82     vector <imu> imu_data;
  83     vector <nav> nav_data;
  84     vector <servo> servo_data;
  85     vector <health> health_data;
  86
  87     // parse message and put current data into vector if message has a
  88     // newer time stamp than existing data.
  89     void parse_msg( const int id, char *buf,
  90                     gps *gpspacket, imu *imupacket, nav *navpacket,
  91                     servo *servopacket, health *healthpacket );
  92
  93     // activate special double swap logic for non-standard stargate
  94     // double format
  95     bool sg_swap;
  96
  97 public:
  98
  99     UGEARTrack();
 100     ~UGEARTrack();
 101
 102     // read/parse the next message from the specified data stream,
 103     // returns id # if a valid message found.
 104     int next_message( SGIOChannel *ch, SGIOChannel *log,
 105                       gps *gpspacket, imu *imupacket, nav *navpacket,
 106                       servo *servopacket, health * healthpacket,
 107                       bool ignore_checksum );
 108     int next_message( SGSerialPort *serial, SGIOChannel *log,
 109                       gps *gpspacket, imu *imupacket, nav *navpacket,
 110                       servo *servopacket, health *healthpacket,
 111                       bool ignore_checksum );
 112
 113     // load the named stream log file into internal buffers
 114     bool load_stream( const string &file, bool ignore_checksum );
 115
 116     // load the named flight files into internal buffers
 117     bool load_flight( const string &path );
 118
 119     inline int gps_size() const { return gps_data.size(); }
 120     inline int imu_size() const { return imu_data.size(); }
 121     inline int nav_size() const { return nav_data.size(); }
 122     inline int servo_size() const { return servo_data.size(); }
 123     inline int health_size() const { return health_data.size(); }
 124
 125     inline gps get_gpspt( const unsigned int i )
 126     {
 127         if ( i < gps_data.size() ) {
 128             return gps_data[i];
 129         } else {
 130             return gps();
 131         }
 132     }
 133     inline imu get_imupt( const unsigned int i )
 134     {
 135         if ( i < imu_data.size() ) {
 136             return imu_data[i];
 137         } else {
 138             return imu();
 139         }
 140     }
 141     inline nav get_navpt( const unsigned int i )
 142     {
 143         if ( i < nav_data.size() ) {
 144             return nav_data[i];
 145         } else {
 146             return nav();
 147         }
 148     }
 149     inline servo get_servopt( const unsigned int i )
 150     {
 151         if ( i < servo_data.size() ) {
 152             return servo_data[i];
 153         } else {
 154             return servo();
 155         }
 156     }
 157     inline health get_healthpt( const unsigned int i )
 158     {
 159         if ( i < health_data.size() ) {
 160             return health_data[i];
 161         } else {
 162             return health();
 163         }
 164     }
 165
 166
 167     // set stargate mode where we have to do an odd swapping of doubles to
 168     // account for their non-standard formate
 169     inline void set_stargate_swap_mode() {
 170         sg_swap = true;
 171     }
 172 };
 173
 174
 175 gps UGEARInterpGPS( const gps A, const gps B, const double percent );
 176 imu UGEARInterpIMU( const imu A, const imu B, const double percent );
 177 nav UGEARInterpNAV( const nav A, const nav B, const double percent );
 178 servo UGEARInterpSERVO( const servo A, const servo B, const double percent );
 179 health UGEARInterpHEALTH( const health A, const health B,
 180                           const double percent );
 181
 182
 183 #endif // _FG_UGEAR_II_HXX