first commit

2022-10-20 20:29:11 +08:00
commit 4d531f8044
3238 changed files with 1387862 additions and 0 deletions
@@ -0,0 +1,157 @@
+#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>
+
+
+// encapsulate a midg integer time (fixme, assumes all times in a track
+// are from the same day, so we don't handle midnight roll over)
+class MIDGTime {
+
+public:
+
+    uint32_t msec;
+    double seconds;
+
+    inline MIDGTime( const int dd, const int hh, const int mm,
+                     const double ss )
+    {
+        seconds = dd*86400.0 + hh*3600.0 + mm*60.0 + ss;
+        msec = (uint32_t)(seconds * 1000);
+    }
+    inline MIDGTime( const uint32_t midgtime_msec ) {
+        msec = midgtime_msec;
+        seconds = (double)midgtime_msec / 1000.0;
+        // std::cout << midgtime << " = " << seconds << std::endl;
+    }
+    inline ~MIDGTime() {}
+
+    inline double get_seconds() const { return seconds; }
+    inline uint32_t get_msec() const { return msec; }
+    inline double diff_seconds( const MIDGTime t ) const {
+        return seconds - t.seconds;        
+    }
+};
+
+
+
+// base class for MIDG data types
+class MIDGpoint {
+
+public:
+
+    MIDGTime midg_time;
+
+    MIDGpoint() :
+        midg_time(MIDGTime(0))
+    { }
+
+    inline double get_seconds() const { return midg_time.get_seconds(); }
+    inline uint32_t get_msec() const { return midg_time.get_msec(); }
+};
+
+
+// encapsulate the interesting midg data for a moment in time
+class MIDGpos : public MIDGpoint {
+
+public:
+
+    double lat_deg;
+    double lon_deg;
+    double altitude_msl;
+    int fix_quality;
+    int num_satellites;
+    double hdop;
+    double speed_kts;
+    double course_true;
+
+    MIDGpos() :
+        lat_deg(0.0),
+        lon_deg(0.0),
+        altitude_msl(0.0),
+        fix_quality(0),
+        num_satellites(0),
+        hdop(0.0),
+        speed_kts(0.0),
+        course_true(0.0)
+    { }
+};
+
+
+// encapsulate the interesting midg data for a moment in time
+class MIDGatt : public MIDGpoint {
+
+public:
+
+    double yaw_rad;
+    double pitch_rad;
+    double roll_rad;
+
+    MIDGatt() :
+        yaw_rad(0.0),
+        pitch_rad(0.0),
+        roll_rad(0.0)
+    { }
+};
+
+
+// Manage a saved midg log (track file)
+class MIDGTrack {
+
+private:
+
+    std::vector <MIDGpos> pos_data;
+    std::vector <MIDGatt> att_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, MIDGpos *pos, MIDGatt *att );
+
+public:
+
+    MIDGTrack();
+    ~MIDGTrack();
+
+    // read/parse the next message from the specified data stream,
+    // returns id # if a valid message found.
+    int next_message( SGIOChannel *ch, SGIOChannel *log,
+                      MIDGpos *pos, MIDGatt *att );
+    int next_message( SGSerialPort *serial, SGIOChannel *log,
+                      MIDGpos *pos, MIDGatt *att );
+
+    // load the named file into internal buffers
+    bool load( const std::string &file );
+
+    inline int pos_size() const { return pos_data.size(); }
+    inline int att_size() const { return att_data.size(); }
+
+    inline MIDGpos get_pospt( const unsigned int i )
+    {
+        if ( i < pos_data.size() ) {
+            return pos_data[i];
+        } else {
+            return MIDGpos();
+        }
+    }
+    inline MIDGatt get_attpt( const unsigned int i )
+    {
+        if ( i < att_data.size() ) {
+            return att_data[i];
+        } else {
+            return MIDGatt();
+        }
+    }
+        
+
+};
+
+
+MIDGpos MIDGInterpPos( const MIDGpos A, const MIDGpos B, const double percent );
+MIDGatt MIDGInterpAtt( const MIDGatt A, const MIDGatt B, const double percent );