5 #include <simgear/compiler.h>
9 #include <simgear/constants.h>
10 #include <simgear/io/sg_file.hxx>
11 #include <simgear/math/sg_geodesy.hxx>
12 #include <simgear/misc/sgstream.hxx>
13 #include <simgear/misc/strutils.hxx>
14 #include <simgear/misc/stdint.hxx>
16 #include "MIDG-II.hxx"
22 MIDGTrack::MIDGTrack() {};
23 MIDGTrack::~MIDGTrack() {};
30 static uint32_t read_swab( char *buf, size_t offset, size_t size ) {
33 char *ptr = buf + offset;
35 // MIDG data is big endian so swap if needed.
36 if ( sgIsLittleEndian() ) {
38 sgEndianSwap( (uint32_t *)ptr );
39 } else if ( size == 2 ) {
40 sgEndianSwap( (uint16_t *)ptr );
45 result = *(uint32_t *)ptr;
46 } else if ( size == 2 ) {
47 result = *(uint16_t *)ptr;
48 } else if ( size == 1 ) {
49 result = *(uint8_t *)ptr;
51 cout << "unknown size in read_swab()" << endl;
59 static bool validate_cksum( uint8_t id, uint8_t size, char *buf,
60 uint8_t cksum0, uint8_t cksum1 )
67 // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 << endl;
71 // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 << endl;
73 for ( uint8_t i = 0; i < size; i++ ) {
74 c0 += (uint8_t)buf[i];
76 // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1
77 // << " [" << (unsigned int)buf[i] << "]" << endl;
80 // cout << "c0 = " << (unsigned int)c0 << " (" << (unsigned int)cksum0
81 // << ") c1 = " << (unsigned int)c1 << " (" << (unsigned int)cksum1
84 if ( c0 == cksum0 && c1 == cksum1 ) {
92 void MIDGTrack::parse_msg( const int id, char *buf, MIDGpos *pos, MIDGatt *att )
95 * Completely unused parser results. Removed from compiling to remove the warnings
103 // cout << "message 1 =" << endl;
106 ts = (uint32_t)read_swab( buf, 0, 4 );
107 // cout << " time stamp = " << ts << endl;
110 status = (uint16_t)read_swab( buf, 4, 2 );
111 // cout << " status = " << status << endl;
114 temp = (int16_t)read_swab( buf, 6, 2 );
115 // cout << " temp = " << temp << endl;
117 } else if ( id == 2 ) {
124 // cout << "message 2 =" << endl;
127 ts = (uint32_t)read_swab( buf, 0, 4 );
128 // cout << " time stamp = " << ts << endl;
131 p = (int16_t)read_swab( buf, 4, 2 );
132 q = (int16_t)read_swab( buf, 6, 2 );
133 r = (int16_t)read_swab( buf, 8, 2 );
134 // cout << " pqr = " << p << "," << q << "," << r << endl;
137 ax = (int16_t)read_swab( buf, 10, 2 );
138 ay = (int16_t)read_swab( buf, 12, 2 );
139 az = (int16_t)read_swab( buf, 14, 2 );
140 // cout << " ax ay az = " << ax << "," << ay << "," << az << endl;
143 mx = (int16_t)read_swab( buf, 16, 2 );
144 my = (int16_t)read_swab( buf, 18, 2 );
145 mz = (int16_t)read_swab( buf, 20, 2 );
146 // cout << " mx my mz = " << mx << "," << my << "," << mz << endl;
149 flags = (uint8_t)read_swab( buf, 22, 1 );
150 // cout << " GPS 1PPS flag = " << (int)(flags & (1 << 6))
151 // << " Timestamp is gps = " << (int)(flags & (1 << 7)) << endl;
153 } else if ( id == 3 ) {
158 // cout << "message 3 =" << endl;
161 ts = (uint32_t)read_swab( buf, 0, 4 );
162 // cout << " time stamp = " << ts << endl;
165 mx = (int16_t)read_swab( buf, 4, 2 );
166 my = (int16_t)read_swab( buf, 6, 2 );
167 mz = (int16_t)read_swab( buf, 8, 2 );
168 // cout << " mx my mz = " << mx << "," << my << "," << mz << endl;
171 flags = (uint8_t)read_swab( buf, 10, 1 );
172 // cout << " GPS 1PPS flag = " << (int)(flags & (1 << 6)) << endl;
174 } else if ( id == 10 ) {
178 int16_t yaw, pitch, roll;
179 int32_t Qw, Qx, Qy, Qz;
182 // cout << "message 10 =" << endl;
185 ts = (uint32_t)read_swab( buf, 0, 4 );
186 // cout << " att time stamp = " << ts << endl;
187 att->midg_time = MIDGTime( ts );
190 p = (int16_t)read_swab( buf, 4, 2 );
191 q = (int16_t)read_swab( buf, 6, 2 );
192 r = (int16_t)read_swab( buf, 8, 2 );
193 // cout << " pqr = " << p << "," << q << "," << r << endl;
196 ax = (int16_t)read_swab( buf, 10, 2 );
197 ay = (int16_t)read_swab( buf, 12, 2 );
198 az = (int16_t)read_swab( buf, 14, 2 );
199 // cout << " ax ay az = " << ax << "," << ay << "," << az << endl;
202 yaw = (int16_t)read_swab( buf, 16, 2 );
203 pitch = (int16_t)read_swab( buf, 18, 2 );
204 roll = (int16_t)read_swab( buf, 20, 2 );
205 // cout << " yaw, pitch, roll = " << yaw << "," << pitch << ","
207 att->yaw_rad = ( (double)yaw / 100.0 ) * SG_PI / 180.0;
208 att->pitch_rad = ( (double)pitch / 100.0 ) * SG_PI / 180.0;
209 att->roll_rad = ( (double)roll / 100.0 ) * SG_PI / 180.0;
212 Qw = (int32_t)read_swab( buf, 22, 4 );
213 Qx = (int32_t)read_swab( buf, 26, 4 );
214 Qy = (int32_t)read_swab( buf, 30, 4 );
215 Qz = (int32_t)read_swab( buf, 34, 4 );
216 // cout << " Qw,Qx,Qy,Qz = " << Qw << "," << Qx << "," << Qy << ","
220 flags = (uint8_t)read_swab( buf, 38, 1 );
221 // cout << " External hdg measurement applied = "
222 // << (int)(flags & (1 << 3)) << endl
223 // << " Magnatometer measurement applied = "
224 // << (int)(flags & (1 << 4)) << endl
225 // << " DGPS = " << (int)(flags & (1 << 5)) << endl
226 // << " Timestamp is gps = " << (int)(flags & (1 << 6)) << endl
227 // << " INS mode = " << (int)(flags & (1 << 7))
230 } else if ( id == 12 ) {
232 int32_t posx, posy, posz;
233 int32_t velx, vely, velz;
236 // cout << "message 12 =" << endl;
239 ts = (uint32_t)read_swab( buf, 0, 4 );
240 // cout << " pos time stamp = " << ts << endl;
241 pos->midg_time = MIDGTime( ts );
244 posx = (int32_t)read_swab( buf, 4, 4 );
245 posy = (int32_t)read_swab( buf, 8, 4 );
246 posz = (int32_t)read_swab( buf, 12, 4 );
247 // cout << " pos = " << posx << "," << posy << "," << posz << endl;
250 xyz[0] = (double)posx/100; xyz[1] = (double)posy/100; xyz[2] = (double)posz/100;
251 double lat, lon, alt;
252 sgCartToGeod(xyz, &lat, &lon, &alt);
253 pos->lat_deg = lat * 180.0 / SG_PI;
254 pos->lon_deg = lon * 180.0 / SG_PI;
255 pos->altitude_msl = alt;
256 // cout << " lon = " << pos->lon_deg << " lat = " << pos->lat_deg
257 // << " alt = " << pos->altitude_msl << endl;
260 velx = (int32_t)read_swab( buf, 16, 4 );
261 vely = (int32_t)read_swab( buf, 20, 4 );
262 velz = (int32_t)read_swab( buf, 24, 4 );
263 // cout << " vel = " << velx << "," << vely << "," << velz << endl;
264 double tmp1 = velx*velx + vely*vely + velz*velz;
265 double vel_cms = sqrt( tmp1 );
266 double vel_ms = vel_cms / 100.0;
267 pos->speed_kts = vel_ms * SG_METER_TO_NM * 3600;
270 flags = (uint8_t)read_swab( buf, 28, 1 );
271 // cout << " ENU pos rel to 1st fix = " << (int)(flags & (1 << 0)) << endl
272 // << " Velocity format = " << (int)(flags & (1 << 1)) << endl
273 // << " bit 2 = " << (int)(flags & (1 << 2)) << endl
274 // << " bit 3 = " << (int)(flags & (1 << 3)) << endl
275 // << " GPS pos/vel valid = " << (int)(flags & (1 << 4)) << endl
276 // << " DGPS = " << (int)(flags & (1 << 5)) << endl
277 // << " Timestamp is gps = " << (int)(flags & (1 << 6)) << endl
278 // << " Solution src (0=gps, 1=ins) = " << (int)(flags & (1 << 7))
281 } else if ( id == 20 ) {
282 uint32_t gps_ts, gps_week;
284 int32_t gps_posx, gps_posy, gps_posz;
285 int32_t gps_velx, gps_vely, gps_velz;
286 int16_t pdop, pacc, sacc;
288 // cout << "message 20 =" << endl;
290 // timestamp -- often slightly off from midg time stamp so
291 // let's not use gps ts to determine if we need to push the
292 // previous data or not, just roll it into the current data
293 // independent of time stamp.
294 gps_ts = (uint32_t)read_swab( buf, 0, 4 );
295 // pt->midg_time = MIDGTime( ts );
297 gps_week = (uint16_t)read_swab( buf, 4, 2 );
298 // cout << " gps time stamp = " << gps_ts << " week = " << gps_week
302 details = (uint16_t)read_swab( buf, 6, 2 );
303 // cout << " details = " << details << endl;
305 // gps_posx, gps_posy, gps_posz
306 gps_posx = (int32_t)read_swab( buf, 8, 4 );
307 gps_posy = (int32_t)read_swab( buf, 12, 4 );
308 gps_posz = (int32_t)read_swab( buf, 16, 4 );
309 // cout << " gps_pos = " << gps_posx << "," << gps_posy << ","
310 // << gps_posz << endl;
312 // gps_velx, gps_vely, gps_velz
313 gps_velx = (int32_t)read_swab( buf, 20, 4 );
314 gps_vely = (int32_t)read_swab( buf, 24, 4 );
315 gps_velz = (int32_t)read_swab( buf, 28, 4 );
316 // cout << " gps_vel = " << gps_velx << "," << gps_vely << ","
317 // << gps_velz << endl;
320 pdop = (uint16_t)read_swab( buf, 32, 2 );
321 // cout << " pdop = " << pdop << endl;
324 pacc = (uint16_t)read_swab( buf, 34, 2 );
325 // cout << " pacc = " << pacc << endl;
328 sacc = (uint16_t)read_swab( buf, 36, 2 );
329 // cout << " sacc = " << sacc << endl;
332 cout << "unknown id = " << id << endl;
338 // load the specified file, return the number of records loaded
339 bool MIDGTrack::load( const string &file ) {
345 uint32_t pos_time = 1;
346 uint32_t att_time = 1;
352 SGFile input( file );
353 if ( !input.open( SG_IO_IN ) ) {
354 cout << "Cannot open file: " << file << endl;
358 while ( ! input.eof() ) {
359 // cout << "looking for next message ..." << endl;
360 int id = next_message( &input, NULL, &pos, &att );
364 if ( att.get_msec() > att_time ) {
365 att_data.push_back( att );
366 att_time = att.get_msec();
368 cout << "oops att back in time" << endl;
370 } else if ( id == 12 ) {
371 if ( pos.get_msec() > pos_time ) {
372 pos_data.push_back( pos );
373 pos_time = pos.get_msec();
375 cout << "oops pos back in time" << endl;
380 cout << "processed " << count << " messages" << endl;
385 // attempt to work around some system dependent issues. Our read can
386 // return < data than we want.
387 int myread( SGIOChannel *ch, SGIOChannel *log, char *buf, int length ) {
391 result = ch->read( buf, length );
392 // cout << "wanted " << length << " read " << result << " bytes" << endl;
393 if ( ch->get_type() == sgFileType ) {
394 myeof = ((SGFile *)ch)->eof();
398 if ( result > 0 && log != NULL ) {
399 log->write( buf, result );
405 // attempt to work around some system dependent issues. Our read can
406 // return < data than we want.
407 int serial_read( SGSerialPort *serial, char *buf, int length ) {
412 while ( bytes_read < length ) {
413 result = serial->read_port( tmp, length - bytes_read );
414 bytes_read += result;
416 // cout << " read " << bytes_read << " of " << length << endl;
422 // load the next message of a real time data stream
423 int MIDGTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
424 MIDGpos *pos, MIDGatt *att )
429 // cout << "in next_message()" << endl;
433 // scan for sync characters
434 uint8_t sync0, sync1;
435 myread( ch, log, tmpbuf, 1 ); sync0 = (unsigned char)tmpbuf[0];
436 myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
437 while ( (sync0 != 129 || sync1 != 161) && !myeof ) {
439 myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
440 // cout << "scanning for start of message "
441 // << (unsigned int)sync0 << " " << (unsigned int)sync1
442 // << ", eof = " << ch->eof() << endl;
443 if ( ch->get_type() == sgFileType ) {
444 myeof = ((SGFile *)ch)->eof();
448 // cout << "found start of message ..." << endl;
450 // read message id and size
451 myread( ch, log, tmpbuf, 1 ); uint8_t id = (unsigned char)tmpbuf[0];
452 myread( ch, log, tmpbuf, 1 ); uint8_t size = (unsigned char)tmpbuf[0];
453 // cout << "message = " << (int)id << " size = " << (int)size << endl;
456 if ( ch->get_type() == sgFileType ) {
457 int count = myread( ch, log, savebuf, size );
458 if ( count != size ) {
459 cout << "ERROR: didn't read enough bytes!" << endl;
462 #ifdef READ_ONE_BY_ONE
463 for ( int i = 0; i < size; ++i ) {
464 myread( ch, log, tmpbuf, 1 ); savebuf[i] = tmpbuf[0];
467 myread( ch, log, savebuf, size );
472 myread( ch, log, tmpbuf, 1 ); uint8_t cksum0 = (unsigned char)tmpbuf[0];
473 myread( ch, log, tmpbuf, 1 ); uint8_t cksum1 = (unsigned char)tmpbuf[0];
475 if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
476 parse_msg( id, savebuf, pos, att );
480 cout << "Check sum failure!" << endl;
485 // load the next message of a real time data stream
486 int MIDGTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
487 MIDGpos *pos, MIDGatt *att )
492 cout << "in next_message()" << endl;
496 // scan for sync characters
497 uint8_t sync0, sync1;
498 serial_read( serial, tmpbuf, 2 );
499 sync0 = (unsigned char)tmpbuf[0];
500 sync1 = (unsigned char)tmpbuf[1];
501 while ( (sync0 != 129 || sync1 != 161) && !myeof ) {
503 serial_read( serial, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
504 cout << "scanning for start of message "
505 << (unsigned int)sync0 << " " << (unsigned int)sync1
509 cout << "found start of message ..." << endl;
511 // read message id and size
512 serial_read( serial, tmpbuf, 2 );
513 uint8_t id = (unsigned char)tmpbuf[0];
514 uint8_t size = (unsigned char)tmpbuf[1];
515 // cout << "message = " << (int)id << " size = " << (int)size << endl;
518 serial_read( serial, savebuf, size );
521 serial_read( serial, tmpbuf, 2 );
522 uint8_t cksum0 = (unsigned char)tmpbuf[0];
523 uint8_t cksum1 = (unsigned char)tmpbuf[1];
525 if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
526 parse_msg( id, savebuf, pos, att );
530 // WRITE DATA TO LOG FILE
536 cout << "Check sum failure!" << endl;
543 static double interp( double a, double b, double p, bool rotational = false ) {
546 // special handling of rotational data
547 if ( diff > SGD_PI ) {
549 } else if ( diff < -SGD_PI ) {
557 MIDGpos MIDGInterpPos( const MIDGpos A, const MIDGpos B, const double percent )
560 p.midg_time = MIDGTime((uint32_t)interp(A.midg_time.get_msec(),
561 B.midg_time.get_msec(),
563 p.lat_deg = interp(A.lat_deg, B.lat_deg, percent);
564 p.lon_deg = interp(A.lon_deg, B.lon_deg, percent);
565 p.altitude_msl = interp(A.altitude_msl, B.altitude_msl, percent);
566 p.fix_quality = (int)interp(A.fix_quality, B.fix_quality, percent);
567 p.num_satellites = (int)interp(A.num_satellites, B.num_satellites, percent);
568 p.hdop = interp(A.hdop, B.hdop, percent);
569 p.speed_kts = interp(A.speed_kts, B.speed_kts, percent);
570 p.course_true = interp(A.course_true, B.course_true, percent, true);
575 MIDGatt MIDGInterpAtt( const MIDGatt A, const MIDGatt B, const double percent )
578 p.midg_time = MIDGTime((uint32_t)interp(A.midg_time.get_msec(),
579 B.midg_time.get_msec(),
581 p.yaw_rad = interp(A.yaw_rad, B.yaw_rad, percent, true);
582 p.pitch_rad = interp(A.pitch_rad, B.pitch_rad, percent, true);
583 p.roll_rad = interp(A.roll_rad, B.roll_rad, percent, true);