first commit

2022-10-20 20:29:11 +08:00
commit 4d531f8044
3238 changed files with 1387862 additions and 0 deletions
@@ -0,0 +1,173 @@
+#pragma once
+
+#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>
+
+
+enum ugPacketType {
+    GPS_PACKET = 0,
+    IMU_PACKET = 1,
+    NAV_PACKET = 2,
+    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;
+   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 UGTrack {
+
+private:
+
+    std::vector <gps> gps_data;
+    std::vector <imu> imu_data;
+    std::vector <nav> nav_data;
+    std::vector <servo> servo_data;
+    std::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, health *healthpacket );
+
+    // activate special double swap logic for non-standard stargate
+    // double format
+    bool sg_swap;
+
+public:
+
+    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, 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 std::string &file, bool ignore_checksum );
+
+    // load the named flight files into internal buffers
+    bool load_flight( const std::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 )
+    {
+        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();
+        }
+    }
+    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 );
+health UGEARInterpHEALTH( const health A, const health B,
+			  const double percent );