utils/GPSsmooth/MIDG-II.cxx

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