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 MIDGTime(const int dd,const int hh,const int mm,const double ss)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 } MIDGTime(const uint32_t midgtime_msec)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 } ~MIDGTime()45 inline ~MIDGTime() {} 46 get_seconds() const47 inline double get_seconds() const { return seconds; } get_msec() const48 inline uint32_t get_msec() const { return msec; } diff_seconds(const MIDGTime t) const49 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 MIDGpoint()63 MIDGpoint() : 64 midg_time(MIDGTime(0)) 65 { } 66 get_seconds() const67 inline double get_seconds() const { return midg_time.get_seconds(); } get_msec() const68 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 MIDGpos()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 MIDGatt()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 pos_size() const143 inline int pos_size() const { return pos_data.size(); } att_size() const144 inline int att_size() const { return att_data.size(); } 145 get_pospt(const unsigned int i)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 } get_attpt(const unsigned int i)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 172