utils/GPSsmooth/MIDG-II.hxx

   1 #ifndef _FG_MIDG_II_HXX
   2 #define _FG_MIDG_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 // encapsulate a midg integer time (fixme, assumes all times in a track
  26 // are from the same day, so we don't handle midnight roll over)
  27 class MIDGTime {
  28
  29 public:
  30
  31     uint32_t msec;
  32     double seconds;
  33
  34     inline MIDGTime( const int dd, const int hh, const int mm,
  35                      const double ss )
  36     {
  37         seconds = dd*86400.0 + hh*3600.0 + mm*60.0 + ss;
  38         msec = (uint32_t)(seconds * 1000);
  39     }
  40     inline MIDGTime( const uint32_t midgtime_msec ) {
  41         msec = midgtime_msec;
  42         seconds = (double)midgtime_msec / 1000.0;
  43         // cout << midgtime << " = " << seconds << endl;
  44     }
  45     inline ~MIDGTime() {}
  46
  47     inline double get_seconds() const { return seconds; }
  48     inline uint32_t get_msec() const { return msec; }
  49     inline double diff_seconds( const MIDGTime t ) const {
  50         return seconds - t.seconds;
  51     }
  52 };
  53
  54
  55
  56 // base class for MIDG data types
  57 class MIDGpoint {
  58
  59 public:
  60
  61     MIDGTime midg_time;
  62
  63     MIDGpoint() :
  64         midg_time(MIDGTime(0))
  65     { }
  66
  67     inline double get_seconds() const { return midg_time.get_seconds(); }
  68     inline uint32_t get_msec() const { return midg_time.get_msec(); }
  69 };
  70
  71
  72 // encapsulate the interesting midg data for a moment in time
  73 class MIDGpos : public MIDGpoint {
  74
  75 public:
  76
  77     double lat_deg;
  78     double lon_deg;
  79     double altitude_msl;
  80     int fix_quality;
  81     int num_satellites;
  82     double hdop;
  83     double speed_kts;
  84     double course_true;
  85
  86     MIDGpos() :
  87         lat_deg(0.0),
  88         lon_deg(0.0),
  89         altitude_msl(0.0),
  90         fix_quality(0),
  91         num_satellites(0),
  92         hdop(0.0),
  93         speed_kts(0.0),
  94         course_true(0.0)
  95     { }
  96 };
  97
  98
  99 // encapsulate the interesting midg data for a moment in time
 100 class MIDGatt : public MIDGpoint {
 101
 102 public:
 103
 104     double yaw_rad;
 105     double pitch_rad;
 106     double roll_rad;
 107
 108     MIDGatt() :
 109         yaw_rad(0.0),
 110         pitch_rad(0.0),
 111         roll_rad(0.0)
 112     { }
 113 };
 114
 115
 116 // Manage a saved midg log (track file)
 117 class MIDGTrack {
 118
 119 private:
 120
 121     vector <MIDGpos> pos_data;
 122     vector <MIDGatt> att_data;
 123
 124     // parse message and put current data into vector if message has a
 125     // newer time stamp than existing data.
 126     void parse_msg( const int id, char *buf, MIDGpos *pos, MIDGatt *att );
 127
 128 public:
 129
 130     MIDGTrack();
 131     ~MIDGTrack();
 132
 133     // read/parse the next message from the specified data stream,
 134     // returns id # if a valid message found.
 135     int next_message( SGIOChannel *ch, SGIOChannel *log,
 136                       MIDGpos *pos, MIDGatt *att );
 137     int next_message( SGSerialPort *serial, SGIOChannel *log,
 138                       MIDGpos *pos, MIDGatt *att );
 139
 140     // load the named file into internal buffers
 141     bool load( const string &file );
 142
 143     inline int pos_size() const { return pos_data.size(); }
 144     inline int att_size() const { return att_data.size(); }
 145
 146     inline MIDGpos get_pospt( const unsigned int i )
 147     {
 148         if ( i < pos_data.size() ) {
 149             return pos_data[i];
 150         } else {
 151             return MIDGpos();
 152         }
 153     }
 154     inline MIDGatt get_attpt( const unsigned int i )
 155     {
 156         if ( i < att_data.size() ) {
 157             return att_data[i];
 158         } else {
 159             return MIDGatt();
 160         }
 161     }
 162
 163
 164 };
 165
 166
 167 MIDGpos MIDGInterpPos( const MIDGpos A, const MIDGpos B, const double percent );
 168 MIDGatt MIDGInterpAtt( const MIDGatt A, const MIDGatt B, const double percent );
 169
 170
 171 #endif // _FG_MIDG_II_HXX