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