utils/GPSsmooth/MIDG-II.cxx

   1 #ifdef HAVE_CONFIG_H
   2 #  include <config.h>
   3 #endif
   4
   5 #include <simgear/compiler.h>
   6
   7 #include <iostream>
   8
   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>
  15
  16 #include "MIDG-II.hxx"
  17
  18 using std::cout;
  19 using std::endl;
  20
  21
  22 MIDGTrack::MIDGTrack() {};
  23 MIDGTrack::~MIDGTrack() {};
  24
  25
  26
  27
  28 static uint32_t read_swab( char *buf, size_t offset, size_t size ) {
  29     uint32_t result = 0;
  30
  31     char *ptr = buf + offset;
  32
  33     // MIDG data is big endian so swap if needed.
  34     if ( sgIsLittleEndian() ) {
  35         if ( size == 4 ) {
  36             sgEndianSwap( (uint32_t *)ptr );
  37         } else if ( size == 2 ) {
  38             sgEndianSwap( (uint16_t *)ptr );
  39         }
  40     }
  41
  42     if ( size == 4 ) {
  43         result = *(uint32_t *)ptr;
  44     } else if ( size == 2 ) {
  45         result = *(uint16_t *)ptr;
  46     } else if ( size == 1 ) {
  47         result = *(uint8_t *)ptr;
  48     } else {
  49         cout << "unknown size in read_swab()" << endl;
  50     }
  51
  52     return result;
  53 }
  54
  55
  56 static bool validate_cksum( uint8_t id, uint8_t size, char *buf,
  57                             uint8_t cksum0, uint8_t cksum1 )
  58 {
  59     uint8_t c0 = 0;
  60     uint8_t c1 = 0;
  61
  62     c0 += id;
  63     c1 += c0;
  64     // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 << endl;
  65
  66     c0 += size;
  67     c1 += c0;
  68     // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 << endl;
  69
  70     for ( uint8_t i = 0; i < size; i++ ) {
  71         c0 += (uint8_t)buf[i];
  72         c1 += c0;
  73         // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1
  74         //      << " [" << (unsigned int)buf[i] << "]" << endl;
  75     }
  76
  77     // cout << "c0 = " << (unsigned int)c0 << " (" << (unsigned int)cksum0
  78     //      << ") c1 = " << (unsigned int)c1 << " (" << (unsigned int)cksum1
  79     //      << ")" << endl;
  80
  81     if ( c0 == cksum0 && c1 == cksum1 ) {
  82         return true;
  83     } else {
  84         return false;
  85     }
  86 }
  87
  88
  89 void MIDGTrack::parse_msg( const int id, char *buf, MIDGpos *pos, MIDGatt *att )
  90 {
  91     if ( id == 1 ) {
  92         uint32_t ts;
  93         uint16_t status;
  94         int16_t temp;
  95
  96         // cout << "message 1 =" << endl;
  97
  98         // timestamp
  99         ts = (uint32_t)read_swab( buf, 0, 4 );
 100         // cout << "  time stamp = " << ts << endl;
 101
 102         // status
 103         status = (uint16_t)read_swab( buf, 4, 2 );
 104         // cout << "  status = " << status << endl;
 105
 106         // temp
 107         temp = (int16_t)read_swab( buf, 6, 2 );
 108         // cout << "  temp = " << temp << endl;
 109
 110     } else if ( id == 2 ) {
 111         uint32_t ts;
 112         int16_t p, q, r;
 113         int16_t ax, ay, az;
 114         int16_t mx, my, mz;
 115         uint8_t flags;
 116
 117         // cout << "message 2 =" << endl;
 118
 119         // timestamp
 120         ts = (uint32_t)read_swab( buf, 0, 4 );
 121         // cout << "  time stamp = " << ts << endl;
 122
 123         // p, q, r
 124         p = (int16_t)read_swab( buf, 4, 2 );
 125         q = (int16_t)read_swab( buf, 6, 2 );
 126         r = (int16_t)read_swab( buf, 8, 2 );
 127         // cout << "  pqr = " << p << "," << q << "," << r << endl;
 128
 129         // ax, ay, az
 130         ax = (int16_t)read_swab( buf, 10, 2 );
 131         ay = (int16_t)read_swab( buf, 12, 2 );
 132         az = (int16_t)read_swab( buf, 14, 2 );
 133         // cout << "  ax ay az = " << ax << "," << ay << "," << az << endl;
 134
 135         // mx, my, mz
 136         mx = (int16_t)read_swab( buf, 16, 2 );
 137         my = (int16_t)read_swab( buf, 18, 2 );
 138         mz = (int16_t)read_swab( buf, 20, 2 );
 139         // cout << "  mx my mz = " << mx << "," << my << "," << mz << endl;
 140
 141         // flags
 142         flags = (uint8_t)read_swab( buf, 22, 1 );
 143         // cout << "  GPS 1PPS flag = " << (int)(flags & (1 << 6))
 144         //      << " Timestamp is gps = " << (int)(flags & (1 << 7)) << endl;
 145
 146     } else if ( id == 3 ) {
 147         uint32_t ts;
 148         int16_t mx, my, mz;
 149         uint8_t flags;
 150
 151         // cout << "message 3 =" << endl;
 152
 153         // timestamp
 154         ts = (uint32_t)read_swab( buf, 0, 4 );
 155         // cout << "  time stamp = " << ts << endl;
 156
 157         // mx, my, mz
 158         mx = (int16_t)read_swab( buf, 4, 2 );
 159         my = (int16_t)read_swab( buf, 6, 2 );
 160         mz = (int16_t)read_swab( buf, 8, 2 );
 161         // cout << "  mx my mz = " << mx << "," << my << "," << mz << endl;
 162
 163         // flags
 164         flags = (uint8_t)read_swab( buf, 10, 1 );
 165         // cout << "  GPS 1PPS flag = " << (int)(flags & (1 << 6)) << endl;
 166
 167     } else if ( id == 10 ) {
 168         uint32_t ts;
 169         int16_t p, q, r;
 170         int16_t ax, ay, az;
 171         int16_t yaw, pitch, roll;
 172         int32_t Qw, Qx, Qy, Qz;
 173         uint8_t flags;
 174
 175         // cout << "message 10 =" << endl;
 176
 177         // timestamp
 178         ts = (uint32_t)read_swab( buf, 0, 4 );
 179         // cout << "  att time stamp = " << ts << endl;
 180         att->midg_time = MIDGTime( ts );
 181
 182         // p, q, r
 183         p = (int16_t)read_swab( buf, 4, 2 );
 184         q = (int16_t)read_swab( buf, 6, 2 );
 185         r = (int16_t)read_swab( buf, 8, 2 );
 186         // cout << "  pqr = " << p << "," << q << "," << r << endl;
 187
 188         // ax, ay, az
 189         ax = (int16_t)read_swab( buf, 10, 2 );
 190         ay = (int16_t)read_swab( buf, 12, 2 );
 191         az = (int16_t)read_swab( buf, 14, 2 );
 192         // cout << "  ax ay az = " << ax << "," << ay << "," << az << endl;
 193
 194         // yaw, pitch, roll
 195         yaw =   (int16_t)read_swab( buf, 16, 2 );
 196         pitch = (int16_t)read_swab( buf, 18, 2 );
 197         roll =  (int16_t)read_swab( buf, 20, 2 );
 198         // cout << "  yaw, pitch, roll = " << yaw << "," << pitch << ","
 199         //      << roll << endl;
 200         att->yaw_rad = ( (double)yaw / 100.0 ) * SG_PI / 180.0;
 201         att->pitch_rad = ( (double)pitch / 100.0 ) * SG_PI / 180.0;
 202         att->roll_rad = ( (double)roll / 100.0 ) * SG_PI / 180.0;
 203
 204         // Qw, Qx, Qy, Qz
 205         Qw = (int32_t)read_swab( buf, 22, 4 );
 206         Qx = (int32_t)read_swab( buf, 26, 4 );
 207         Qy = (int32_t)read_swab( buf, 30, 4 );
 208         Qz = (int32_t)read_swab( buf, 34, 4 );
 209         // cout << "  Qw,Qx,Qy,Qz = " << Qw << "," << Qx << "," << Qy << ","
 210         //      << Qz << endl;
 211
 212         // flags
 213         flags = (uint8_t)read_swab( buf, 38, 1 );
 214         // cout << "  External hdg measurement applied = "
 215         //      << (int)(flags & (1 << 3)) << endl
 216         //      << "  Magnatometer measurement applied = "
 217         //      << (int)(flags & (1 << 4)) << endl
 218         //      << "  DGPS = " << (int)(flags & (1 << 5)) << endl
 219         //      << "  Timestamp is gps = " << (int)(flags & (1 << 6)) << endl
 220         //      << "  INS mode = " << (int)(flags & (1 << 7))
 221         //      << endl;
 222
 223     } else if ( id == 12 ) {
 224         uint32_t ts;
 225         int32_t posx, posy, posz;
 226         int32_t velx, vely, velz;
 227         uint8_t flags;
 228
 229         // cout << "message 12 =" << endl;
 230
 231         // timestamp
 232         ts = (uint32_t)read_swab( buf, 0, 4 );
 233         // cout << "  pos time stamp = " << ts << endl;
 234         pos->midg_time = MIDGTime( ts );
 235
 236         // posx, posy, posz
 237         posx = (int32_t)read_swab( buf, 4, 4 );
 238         posy = (int32_t)read_swab( buf, 8, 4 );
 239         posz = (int32_t)read_swab( buf, 12, 4 );
 240         // cout << "  pos = " << posx << "," << posy << "," << posz << endl;
 241
 242         double xyz[3];
 243         xyz[0] = (double)posx/100; xyz[1] = (double)posy/100; xyz[2] = (double)posz/100;
 244         double lat, lon, alt;
 245         sgCartToGeod(xyz, &lat, &lon, &alt);
 246         pos->lat_deg = lat * 180.0 / SG_PI;
 247         pos->lon_deg = lon * 180.0 / SG_PI;
 248         pos->altitude_msl = alt;
 249         // cout << "  lon = " << pos->lon_deg << " lat = " << pos->lat_deg
 250         //      << " alt = " << pos->altitude_msl << endl;
 251
 252         // velx, vely, velz
 253         velx = (int32_t)read_swab( buf, 16, 4 );
 254         vely = (int32_t)read_swab( buf, 20, 4 );
 255         velz = (int32_t)read_swab( buf, 24, 4 );
 256         // cout << "  vel = " << velx << "," << vely << "," << velz << endl;
 257         double tmp1 = velx*velx + vely*vely + velz*velz;
 258         double vel_cms = sqrt( tmp1 );
 259         double vel_ms = vel_cms / 100.0;
 260         pos->speed_kts = vel_ms * SG_METER_TO_NM * 3600;
 261
 262         // flags
 263         flags = (uint8_t)read_swab( buf, 28, 1 );
 264         // cout << "  ENU pos rel to 1st fix = " << (int)(flags & (1 << 0)) << endl
 265         //      << "  Velocity format = " << (int)(flags & (1 << 1)) << endl
 266         //      << "  bit 2 = " << (int)(flags & (1 << 2)) << endl
 267         //      << "  bit 3 = " << (int)(flags & (1 << 3)) << endl
 268         //      << "  GPS pos/vel valid = " << (int)(flags & (1 << 4)) << endl
 269         //      << "  DGPS = " << (int)(flags & (1 << 5)) << endl
 270         //      << "  Timestamp is gps = " << (int)(flags & (1 << 6)) << endl
 271         //      << "  Solution src (0=gps, 1=ins) = " << (int)(flags & (1 << 7))
 272         //      << endl;
 273
 274     } else if ( id == 20 ) {
 275         uint32_t gps_ts, gps_week;
 276         uint16_t details;
 277         int32_t gps_posx, gps_posy, gps_posz;
 278         int32_t gps_velx, gps_vely, gps_velz;
 279         int16_t pdop, pacc, sacc;
 280
 281         // cout << "message 20 =" << endl;
 282
 283         // timestamp -- often slightly off from midg time stamp so
 284         // let's not use gps ts to determine if we need to push the
 285         // previous data or not, just roll it into the current data
 286         // independent of time stamp.
 287         gps_ts = (uint32_t)read_swab( buf, 0, 4 );
 288         // pt->midg_time = MIDGTime( ts );
 289
 290         gps_week = (uint16_t)read_swab( buf, 4, 2 );
 291         // cout << "  gps time stamp = " << gps_ts << " week = " << gps_week
 292         //      <<  endl;
 293
 294         // details
 295         details = (uint16_t)read_swab( buf, 6, 2 );
 296         // cout << "  details = " << details <<  endl;
 297
 298         // gps_posx, gps_posy, gps_posz
 299         gps_posx = (int32_t)read_swab( buf, 8, 4 );
 300         gps_posy = (int32_t)read_swab( buf, 12, 4 );
 301         gps_posz = (int32_t)read_swab( buf, 16, 4 );
 302         // cout << "  gps_pos = " << gps_posx << "," << gps_posy << ","
 303         //      << gps_posz << endl;
 304
 305         // gps_velx, gps_vely, gps_velz
 306         gps_velx = (int32_t)read_swab( buf, 20, 4 );
 307         gps_vely = (int32_t)read_swab( buf, 24, 4 );
 308         gps_velz = (int32_t)read_swab( buf, 28, 4 );
 309         // cout << "  gps_vel = " << gps_velx << "," << gps_vely << ","
 310         //      << gps_velz << endl;
 311
 312         // position dop
 313         pdop = (uint16_t)read_swab( buf, 32, 2 );
 314         // cout << "  pdop = " << pdop <<  endl;
 315
 316         // position accuracy
 317         pacc = (uint16_t)read_swab( buf, 34, 2 );
 318         // cout << "  pacc = " << pacc <<  endl;
 319
 320         // speed accuracy
 321         sacc = (uint16_t)read_swab( buf, 36, 2 );
 322         // cout << "  sacc = " << sacc <<  endl;
 323
 324     } else {
 325         cout << "unknown id = " << id << endl;
 326     }
 327 }
 328
 329
 330 // load the specified file, return the number of records loaded
 331 bool MIDGTrack::load( const string &file ) {
 332     int count = 0;
 333
 334     MIDGpos pos;
 335     MIDGatt att;
 336
 337     uint32_t pos_time = 1;
 338     uint32_t att_time = 1;
 339
 340     pos_data.clear();
 341     att_data.clear();
 342
 343     // open the file
 344     SGFile input( file );
 345     if ( !input.open( SG_IO_IN ) ) {
 346         cout << "Cannot open file: " << file << endl;
 347         return false;
 348     }
 349
 350     while ( ! input.eof() ) {
 351         // cout << "looking for next message ..." << endl;
 352         int id = next_message( &input, NULL, &pos, &att );
 353         count++;
 354
 355         if ( id == 10 ) {
 356             if ( att.get_msec() > att_time ) {
 357                 att_data.push_back( att );
 358                 att_time = att.get_msec();
 359             } else {
 360                 cout << "oops att back in time" << endl;
 361             }
 362         } else if ( id == 12 ) {
 363             if ( pos.get_msec() > pos_time ) {
 364                 pos_data.push_back( pos );
 365                 pos_time = pos.get_msec();
 366             } else {
 367                 cout << "oops pos back in time" << endl;
 368             }
 369         }
 370     }
 371
 372     cout << "processed " << count << " messages" << endl;
 373     return true;
 374 }
 375
 376
 377 // attempt to work around some system dependent issues.  Our read can
 378 // return < data than we want.
 379 int myread( SGIOChannel *ch, SGIOChannel *log, char *buf, int length ) {
 380     bool myeof = false;
 381     int result = 0;
 382     if ( !myeof ) {
 383       result = ch->read( buf, length );
 384       // cout << "wanted " << length << " read " << result << " bytes" << endl;
 385       if ( ch->get_type() == sgFileType ) {
 386         myeof = ((SGFile *)ch)->eof();
 387       }
 388     }
 389
 390     if ( result > 0 && log != NULL ) {
 391         log->write( buf, result );
 392     }
 393
 394     return result;
 395 }
 396
 397 // attempt to work around some system dependent issues.  Our read can
 398 // return < data than we want.
 399 int serial_read( SGSerialPort *serial, char *buf, int length ) {
 400     int result = 0;
 401     int bytes_read = 0;
 402     char *tmp = buf;
 403
 404     while ( bytes_read < length ) {
 405       result = serial->read_port( tmp, length - bytes_read );
 406       bytes_read += result;
 407       tmp += result;
 408       // cout << "  read " << bytes_read << " of " << length << endl;
 409     }
 410
 411     return bytes_read;
 412 }
 413
 414 // load the next message of a real time data stream
 415 int MIDGTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
 416                              MIDGpos *pos, MIDGatt *att )
 417 {
 418     char tmpbuf[256];
 419     char savebuf[256];
 420
 421     // cout << "in next_message()" << endl;
 422
 423     bool myeof = false;
 424
 425     // scan for sync characters
 426     uint8_t sync0, sync1;
 427     myread( ch, log, tmpbuf, 1 ); sync0 = (unsigned char)tmpbuf[0];
 428     myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
 429     while ( (sync0 != 129 || sync1 != 161) && !myeof ) {
 430         sync0 = sync1;
 431         myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
 432         // cout << "scanning for start of message "
 433         //      << (unsigned int)sync0 << " " << (unsigned int)sync1
 434         //      << ", eof = " << ch->eof() << endl;
 435         if ( ch->get_type() == sgFileType ) {
 436             myeof = ((SGFile *)ch)->eof();
 437         }
 438     }
 439
 440     // cout << "found start of message ..." << endl;
 441
 442     // read message id and size
 443     myread( ch, log, tmpbuf, 1 ); uint8_t id = (unsigned char)tmpbuf[0];
 444     myread( ch, log, tmpbuf, 1 ); uint8_t size = (unsigned char)tmpbuf[0];
 445     // cout << "message = " << (int)id << " size = " << (int)size << endl;
 446
 447     // load message
 448     if ( ch->get_type() == sgFileType ) {
 449         int count = myread( ch, log, savebuf, size );
 450         if ( count != size ) {
 451             cout << "ERROR: didn't read enough bytes!" << endl;
 452         }
 453     } else {
 454 #ifdef READ_ONE_BY_ONE
 455         for ( int i = 0; i < size; ++i ) {
 456             myread( ch, log, tmpbuf, 1 ); savebuf[i] = tmpbuf[0];
 457         }
 458 #else
 459         myread( ch, log, savebuf, size );
 460 #endif
 461     }
 462
 463     // read checksum
 464     myread( ch, log, tmpbuf, 1 ); uint8_t cksum0 = (unsigned char)tmpbuf[0];
 465     myread( ch, log, tmpbuf, 1 ); uint8_t cksum1 = (unsigned char)tmpbuf[0];
 466
 467     if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
 468         parse_msg( id, savebuf, pos, att );
 469         return id;
 470     }
 471
 472     cout << "Check sum failure!" << endl;
 473     return -1;
 474 }
 475
 476
 477 // load the next message of a real time data stream
 478 int MIDGTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
 479                              MIDGpos *pos, MIDGatt *att )
 480 {
 481     char tmpbuf[256];
 482     char savebuf[256];
 483     int result = 0;
 484
 485     cout << "in next_message()" << endl;
 486
 487     bool myeof = false;
 488
 489     // scan for sync characters
 490     uint8_t sync0, sync1;
 491     result = serial_read( serial, tmpbuf, 2 );
 492     sync0 = (unsigned char)tmpbuf[0];
 493     sync1 = (unsigned char)tmpbuf[1];
 494     while ( (sync0 != 129 || sync1 != 161) && !myeof ) {
 495         sync0 = sync1;
 496         serial_read( serial, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
 497         cout << "scanning for start of message "
 498              << (unsigned int)sync0 << " " << (unsigned int)sync1
 499              << endl;
 500     }
 501
 502     cout << "found start of message ..." << endl;
 503
 504     // read message id and size
 505     serial_read( serial, tmpbuf, 2 );
 506     uint8_t id = (unsigned char)tmpbuf[0];
 507     uint8_t size = (unsigned char)tmpbuf[1];
 508     // cout << "message = " << (int)id << " size = " << (int)size << endl;
 509
 510     // load message
 511     serial_read( serial, savebuf, size );
 512
 513     // read checksum
 514     serial_read( serial, tmpbuf, 2 );
 515     uint8_t cksum0 = (unsigned char)tmpbuf[0];
 516     uint8_t cksum1 = (unsigned char)tmpbuf[1];
 517
 518     if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
 519         parse_msg( id, savebuf, pos, att );
 520
 521         //
 522         // FIXME
 523         // WRITE DATA TO LOG FILE
 524         //
 525
 526         return id;
 527     }
 528
 529     cout << "Check sum failure!" << endl;
 530     return -1;
 531
 532
 533 }
 534
 535
 536 static double interp( double a, double b, double p, bool rotational = false ) {
 537     double diff = b - a;
 538     if ( rotational ) {
 539         // special handling of rotational data
 540         if ( diff > SGD_PI ) {
 541             diff -= SGD_2PI;
 542         } else if ( diff < -SGD_PI ) {
 543             diff += SGD_2PI;
 544         }
 545     }
 546     return a + diff * p;
 547 }
 548
 549
 550 MIDGpos MIDGInterpPos( const MIDGpos A, const MIDGpos B, const double percent )
 551 {
 552     MIDGpos p;
 553     p.midg_time = MIDGTime((uint32_t)interp(A.midg_time.get_msec(),
 554                                             B.midg_time.get_msec(),
 555                                             percent));
 556     p.lat_deg = interp(A.lat_deg, B.lat_deg, percent);
 557     p.lon_deg = interp(A.lon_deg, B.lon_deg, percent);
 558     p.altitude_msl = interp(A.altitude_msl, B.altitude_msl, percent);
 559     p.fix_quality = (int)interp(A.fix_quality, B.fix_quality, percent);
 560     p.num_satellites = (int)interp(A.num_satellites, B.num_satellites, percent);
 561     p.hdop = interp(A.hdop, B.hdop, percent);
 562     p.speed_kts = interp(A.speed_kts, B.speed_kts, percent);
 563     p.course_true = interp(A.course_true, B.course_true, percent, true);
 564
 565     return p;
 566 }
 567
 568 MIDGatt MIDGInterpAtt( const MIDGatt A, const MIDGatt B, const double percent )
 569 {
 570     MIDGatt p;
 571     p.midg_time = MIDGTime((uint32_t)interp(A.midg_time.get_msec(),
 572                                             B.midg_time.get_msec(),
 573                                             percent));
 574     p.yaw_rad = interp(A.yaw_rad, B.yaw_rad, percent, true);
 575     p.pitch_rad = interp(A.pitch_rad, B.pitch_rad, percent, true);
 576     p.roll_rad = interp(A.roll_rad, B.roll_rad, percent, true);
 577
 578     return p;
 579 }