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  * Unused function
  28  */
  29 #if(0)
  30 static uint32_t read_swab( char *buf, size_t offset, size_t size ) {
  31     uint32_t result = 0;
  32
  33     char *ptr = buf + offset;
  34
  35     // MIDG data is big endian so swap if needed.
  36     if ( sgIsLittleEndian() ) {
  37         if ( size == 4 ) {
  38             sgEndianSwap( (uint32_t *)ptr );
  39         } else if ( size == 2 ) {
  40             sgEndianSwap( (uint16_t *)ptr );
  41         }
  42     }
  43
  44     if ( size == 4 ) {
  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;
  50     } else {
  51         cout << "unknown size in read_swab()" << endl;
  52     }
  53
  54     return result;
  55 }
  56 #endif
  57
  58
  59 static bool validate_cksum( uint8_t id, uint8_t size, char *buf,
  60                             uint8_t cksum0, uint8_t cksum1 )
  61 {
  62     uint8_t c0 = 0;
  63     uint8_t c1 = 0;
  64
  65     c0 += id;
  66     c1 += c0;
  67     // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 << endl;
  68
  69     c0 += size;
  70     c1 += c0;
  71     // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 << endl;
  72
  73     for ( uint8_t i = 0; i < size; i++ ) {
  74         c0 += (uint8_t)buf[i];
  75         c1 += c0;
  76         // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1
  77         //      << " [" << (unsigned int)buf[i] << "]" << endl;
  78     }
  79
  80     // cout << "c0 = " << (unsigned int)c0 << " (" << (unsigned int)cksum0
  81     //      << ") c1 = " << (unsigned int)c1 << " (" << (unsigned int)cksum1
  82     //      << ")" << endl;
  83
  84     if ( c0 == cksum0 && c1 == cksum1 ) {
  85         return true;
  86     } else {
  87         return false;
  88     }
  89 }
  90
  91
  92 void MIDGTrack::parse_msg( const int id, char *buf, MIDGpos *pos, MIDGatt *att )
  93 {
  94 /*
  95  * Completely unused parser results. Removed from compiling to remove the warnings
  96  */
  97 #if(0)
  98     if ( id == 1 ) {
  99         uint32_t ts;
 100         uint16_t status;
 101         int16_t temp;
 102
 103         // cout << "message 1 =" << endl;
 104
 105         // timestamp
 106         ts = (uint32_t)read_swab( buf, 0, 4 );
 107         // cout << "  time stamp = " << ts << endl;
 108
 109         // status
 110         status = (uint16_t)read_swab( buf, 4, 2 );
 111         // cout << "  status = " << status << endl;
 112
 113         // temp
 114         temp = (int16_t)read_swab( buf, 6, 2 );
 115         // cout << "  temp = " << temp << endl;
 116
 117     } else if ( id == 2 ) {
 118         uint32_t ts;
 119         int16_t p, q, r;
 120         int16_t ax, ay, az;
 121         int16_t mx, my, mz;
 122         uint8_t flags;
 123
 124         // cout << "message 2 =" << endl;
 125
 126         // timestamp
 127         ts = (uint32_t)read_swab( buf, 0, 4 );
 128         // cout << "  time stamp = " << ts << endl;
 129
 130         // p, q, r
 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;
 135
 136         // ax, ay, az
 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;
 141
 142         // mx, my, mz
 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;
 147
 148         // flags
 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;
 152
 153     } else if ( id == 3 ) {
 154         uint32_t ts;
 155         int16_t mx, my, mz;
 156         uint8_t flags;
 157
 158         // cout << "message 3 =" << endl;
 159
 160         // timestamp
 161         ts = (uint32_t)read_swab( buf, 0, 4 );
 162         // cout << "  time stamp = " << ts << endl;
 163
 164         // mx, my, mz
 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;
 169
 170         // flags
 171         flags = (uint8_t)read_swab( buf, 10, 1 );
 172         // cout << "  GPS 1PPS flag = " << (int)(flags & (1 << 6)) << endl;
 173
 174     } else if ( id == 10 ) {
 175         uint32_t ts;
 176         int16_t p, q, r;
 177         int16_t ax, ay, az;
 178         int16_t yaw, pitch, roll;
 179         int32_t Qw, Qx, Qy, Qz;
 180         uint8_t flags;
 181
 182         // cout << "message 10 =" << endl;
 183
 184         // timestamp
 185         ts = (uint32_t)read_swab( buf, 0, 4 );
 186         // cout << "  att time stamp = " << ts << endl;
 187         att->midg_time = MIDGTime( ts );
 188
 189         // p, q, r
 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;
 194
 195         // ax, ay, az
 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;
 200
 201         // yaw, pitch, roll
 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 << ","
 206         //      << roll << endl;
 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;
 210
 211         // Qw, Qx, Qy, Qz
 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 << ","
 217         //      << Qz << endl;
 218
 219         // flags
 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))
 228         //      << endl;
 229
 230     } else if ( id == 12 ) {
 231         uint32_t ts;
 232         int32_t posx, posy, posz;
 233         int32_t velx, vely, velz;
 234         uint8_t flags;
 235
 236         // cout << "message 12 =" << endl;
 237
 238         // timestamp
 239         ts = (uint32_t)read_swab( buf, 0, 4 );
 240         // cout << "  pos time stamp = " << ts << endl;
 241         pos->midg_time = MIDGTime( ts );
 242
 243         // posx, posy, posz
 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;
 248
 249         double xyz[3];
 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;
 258
 259         // velx, vely, velz
 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;
 268
 269         // flags
 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))
 279         //      << endl;
 280
 281     } else if ( id == 20 ) {
 282         uint32_t gps_ts, gps_week;
 283         uint16_t details;
 284         int32_t gps_posx, gps_posy, gps_posz;
 285         int32_t gps_velx, gps_vely, gps_velz;
 286         int16_t pdop, pacc, sacc;
 287
 288         // cout << "message 20 =" << endl;
 289
 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 );
 296
 297         gps_week = (uint16_t)read_swab( buf, 4, 2 );
 298         // cout << "  gps time stamp = " << gps_ts << " week = " << gps_week
 299         //      <<  endl;
 300
 301         // details
 302         details = (uint16_t)read_swab( buf, 6, 2 );
 303         // cout << "  details = " << details <<  endl;
 304
 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;
 311
 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;
 318
 319         // position dop
 320         pdop = (uint16_t)read_swab( buf, 32, 2 );
 321         // cout << "  pdop = " << pdop <<  endl;
 322
 323         // position accuracy
 324         pacc = (uint16_t)read_swab( buf, 34, 2 );
 325         // cout << "  pacc = " << pacc <<  endl;
 326
 327         // speed accuracy
 328         sacc = (uint16_t)read_swab( buf, 36, 2 );
 329         // cout << "  sacc = " << sacc <<  endl;
 330
 331     } else {
 332         cout << "unknown id = " << id << endl;
 333     }
 334 #endif
 335 }
 336
 337
 338 // load the specified file, return the number of records loaded
 339 bool MIDGTrack::load( const string &file ) {
 340     int count = 0;
 341
 342     MIDGpos pos;
 343     MIDGatt att;
 344
 345     uint32_t pos_time = 1;
 346     uint32_t att_time = 1;
 347
 348     pos_data.clear();
 349     att_data.clear();
 350
 351     // open the file
 352     SGFile input( file );
 353     if ( !input.open( SG_IO_IN ) ) {
 354         cout << "Cannot open file: " << file << endl;
 355         return false;
 356     }
 357
 358     while ( ! input.eof() ) {
 359         // cout << "looking for next message ..." << endl;
 360         int id = next_message( &input, NULL, &pos, &att );
 361         count++;
 362
 363         if ( id == 10 ) {
 364             if ( att.get_msec() > att_time ) {
 365                 att_data.push_back( att );
 366                 att_time = att.get_msec();
 367             } else {
 368                 cout << "oops att back in time" << endl;
 369             }
 370         } else if ( id == 12 ) {
 371             if ( pos.get_msec() > pos_time ) {
 372                 pos_data.push_back( pos );
 373                 pos_time = pos.get_msec();
 374             } else {
 375                 cout << "oops pos back in time" << endl;
 376             }
 377         }
 378     }
 379
 380     cout << "processed " << count << " messages" << endl;
 381     return true;
 382 }
 383
 384
 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 ) {
 388     bool myeof = false;
 389     int result = 0;
 390     if ( !myeof ) {
 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();
 395       }
 396     }
 397
 398     if ( result > 0 && log != NULL ) {
 399         log->write( buf, result );
 400     }
 401
 402     return result;
 403 }
 404
 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 ) {
 408     int result = 0;
 409     int bytes_read = 0;
 410     char *tmp = buf;
 411
 412     while ( bytes_read < length ) {
 413       result = serial->read_port( tmp, length - bytes_read );
 414       bytes_read += result;
 415       tmp += result;
 416       // cout << "  read " << bytes_read << " of " << length << endl;
 417     }
 418
 419     return bytes_read;
 420 }
 421
 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 )
 425 {
 426     char tmpbuf[256];
 427     char savebuf[256];
 428
 429     // cout << "in next_message()" << endl;
 430
 431     bool myeof = false;
 432
 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 ) {
 438         sync0 = sync1;
 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();
 445         }
 446     }
 447
 448     // cout << "found start of message ..." << endl;
 449
 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;
 454
 455     // load message
 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;
 460         }
 461     } else {
 462 #ifdef READ_ONE_BY_ONE
 463         for ( int i = 0; i < size; ++i ) {
 464             myread( ch, log, tmpbuf, 1 ); savebuf[i] = tmpbuf[0];
 465         }
 466 #else
 467         myread( ch, log, savebuf, size );
 468 #endif
 469     }
 470
 471     // read checksum
 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];
 474
 475     if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
 476         parse_msg( id, savebuf, pos, att );
 477         return id;
 478     }
 479
 480     cout << "Check sum failure!" << endl;
 481     return -1;
 482 }
 483
 484
 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 )
 488 {
 489     char tmpbuf[256];
 490     char savebuf[256];
 491
 492     cout << "in next_message()" << endl;
 493
 494     bool myeof = false;
 495
 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 ) {
 502         sync0 = sync1;
 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
 506              << endl;
 507     }
 508
 509     cout << "found start of message ..." << endl;
 510
 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;
 516
 517     // load message
 518     serial_read( serial, savebuf, size );
 519
 520     // read checksum
 521     serial_read( serial, tmpbuf, 2 );
 522     uint8_t cksum0 = (unsigned char)tmpbuf[0];
 523     uint8_t cksum1 = (unsigned char)tmpbuf[1];
 524
 525     if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
 526         parse_msg( id, savebuf, pos, att );
 527
 528         //
 529         // FIXME
 530         // WRITE DATA TO LOG FILE
 531         //
 532
 533         return id;
 534     }
 535
 536     cout << "Check sum failure!" << endl;
 537     return -1;
 538
 539
 540 }
 541
 542
 543 static double interp( double a, double b, double p, bool rotational = false ) {
 544     double diff = b - a;
 545     if ( rotational ) {
 546         // special handling of rotational data
 547         if ( diff > SGD_PI ) {
 548             diff -= SGD_2PI;
 549         } else if ( diff < -SGD_PI ) {
 550             diff += SGD_2PI;
 551         }
 552     }
 553     return a + diff * p;
 554 }
 555
 556
 557 MIDGpos MIDGInterpPos( const MIDGpos A, const MIDGpos B, const double percent )
 558 {
 559     MIDGpos p;
 560     p.midg_time = MIDGTime((uint32_t)interp(A.midg_time.get_msec(),
 561                                             B.midg_time.get_msec(),
 562                                             percent));
 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);
 571
 572     return p;
 573 }
 574
 575 MIDGatt MIDGInterpAtt( const MIDGatt A, const MIDGatt B, const double percent )
 576 {
 577     MIDGatt p;
 578     p.midg_time = MIDGTime((uint32_t)interp(A.midg_time.get_msec(),
 579                                             B.midg_time.get_msec(),
 580                                             percent));
 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);
 584
 585     return p;
 586 }