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