Allow setting of NED velocities.

[flightgear.git] / utils / GPSsmooth / UGear.hxx

diff --git a/utils/GPSsmooth/UGear.hxx b/utils/GPSsmooth/UGear.hxx
index 1014f57b4c40641389dbadaee6c71618b21d61a8..ef35d0d18026ac446ad6b9100f11111cd972daa6 100644 (file)
--- a/utils/GPSsmooth/UGear.hxx
+++ b/utils/GPSsmooth/UGear.hxx
@@ -16,55 +16,69 @@
 #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);
+using std::cout;
+using std::endl;
+using std::string;
+using std::vector;
 
 
 enum ugPacketType {
     GPS_PACKET = 0,
     IMU_PACKET = 1,
     NAV_PACKET = 2,
-    SERVO_PACKET = 3
+    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             */
-   short  err_type;            /* error type            */
-   double time;
+   uint64_t err_type;          /* error type            */
 };
 
 struct gps {
+   double time;
    double lat,lon,alt;          /* gps position          */
    double ve,vn,vd;             /* gps velocity          */
-   uint16_t ITOW;
-   short  err_type;             /* error type            */
-   double time;
+   double ITOW;
+   uint64_t err_type;           /* error type            */
 };
 
 struct nav {
+   double time;
    double lat,lon,alt;
    double ve,vn,vd;
    // float  t;
-   short  err_type;
-   double time;
+   uint64_t err_type;
 };
 
 struct servo {
-   uint16_t chn[8];
-   uint8_t status;
    double time;
+   uint16_t chn[8];
+   uint64_t status;
 };
 
+struct health {
+    double time;
+    double target_roll_deg;     /* AP target roll angle */
+    double target_heading_deg;  /* AP target heading angle */
+    double target_pitch_deg;    /* AP target pitch angle */
+    double target_climb_fps;    /* AP target climb rate */
+    double target_altitude_ft;  /* AP target altitude */
+    uint64_t command_sequence;  /* highest received command sequence num */
+    uint64_t target_waypoint;   /* index of current waypoint target */
+    uint64_t loadavg;           /* system "1 minute" load average */
+    uint64_t ahrs_hz;           /* actual ahrs loop hz */
+    uint64_t nav_hz;            /* actual nav loop hz */
+};
 
 // Manage a saved ugear log (track file)
-class UGEARTrack {
+class UGTrack {
 
 private:
 
@@ -72,34 +86,45 @@ private:
     vector <imu> imu_data;
     vector <nav> nav_data;
     vector <servo> servo_data;
+    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,
-                   servo *servopacket );
+                   servo *servopacket, health *healthpacket );
+
+    // activate special double swap logic for non-standard stargate
+    // double format
+    bool sg_swap;
 
 public:
 
-    UGEARTrack();
-    ~UGEARTrack();
+    UGTrack();
+    ~UGTrack();
 
     // 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 );
+                     servo *servopacket, health * healthpacket,
+                     bool ignore_checksum );
     int next_message( SGSerialPort *serial, SGIOChannel *log,
                       gps *gpspacket, imu *imupacket, nav *navpacket,
-                     servo *servopacket );
+                     servo *servopacket, health *healthpacket,
+                     bool ignore_checksum );
 
-    // load the named file into internal buffers
-    bool load( const string &file );
+    // 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(); }
+    inline int health_size() const { return health_data.size(); }
 
     inline gps get_gpspt( const unsigned int i )
     {
@@ -133,8 +158,21 @@ public:
             return servo();
         }
     }
-        
+    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;
+    }
 };
 
 
@@ -142,6 +180,8 @@ 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 );
+health UGEARInterpHEALTH( const health A, const health B,
+                         const double percent );
 
 
 #endif // _FG_UGEAR_II_HXX