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