utils/GPSsmooth/UGear.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/sg_path.hxx>
  11 #include <simgear/misc/sgstream.hxx>
  12 #include <simgear/misc/strutils.hxx>
  13 #include <simgear/misc/stdint.hxx>
  14
  15 #include "UGear.hxx"
  16
  17 SG_USING_STD(cout);
  18 SG_USING_STD(endl);
  19
  20
  21 #define START_OF_MSG0 147
  22 #define START_OF_MSG1 224
  23
  24
  25 UGEARTrack::UGEARTrack():
  26     sg_swap(false)
  27 {
  28 };
  29
  30 UGEARTrack::~UGEARTrack() {};
  31
  32
  33 // swap the 1st 4 bytes with the last 4 bytes of a stargate double so
  34 // it matches the PC representation
  35 static double sg_swap_double( uint8_t *buf, size_t offset ) {
  36     double *result;
  37     uint8_t tmpbuf[10];
  38
  39     for ( size_t i = 0; i < 4; ++i ) {
  40       tmpbuf[i] = buf[offset + i + 4];
  41     }
  42     for ( size_t i = 0; i < 4; ++i ) {
  43       tmpbuf[i + 4] = buf[offset + i];
  44     }
  45
  46     // for ( size_t i = 0; i < 8; ++i ) {
  47     //   printf("%d ", tmpbuf[i]);
  48     // }
  49     // printf("\n");
  50
  51     result = (double *)tmpbuf;
  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                             bool ignore_checksum )
  59 {
  60     if ( ignore_checksum ) {
  61         return true;
  62     }
  63
  64     uint8_t c0 = 0;
  65     uint8_t c1 = 0;
  66
  67     c0 += id;
  68     c1 += c0;
  69     // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 << endl;
  70
  71     c0 += size;
  72     c1 += c0;
  73     // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 << endl;
  74
  75     for ( uint8_t i = 0; i < size; i++ ) {
  76         c0 += (uint8_t)buf[i];
  77         c1 += c0;
  78         // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1
  79         //      << " [" << (unsigned int)(uint8_t)buf[i] << "]" << endl;
  80     }
  81
  82     // cout << "c0 = " << (unsigned int)c0 << " (" << (unsigned int)cksum0
  83     //      << ") c1 = " << (unsigned int)c1 << " (" << (unsigned int)cksum1
  84     //      << ")" << endl;
  85
  86     if ( c0 == cksum0 && c1 == cksum1 ) {
  87         return true;
  88     } else {
  89         return false;
  90     }
  91 }
  92
  93
  94 void UGEARTrack::parse_msg( const int id, char *buf,
  95                             struct gps *gpspacket, imu *imupacket,
  96                             nav *navpacket, servo *servopacket,
  97                             health *healthpacket )
  98 {
  99     if ( id == GPS_PACKET ) {
 100       *gpspacket = *(struct gps *)buf;
 101       if ( sg_swap ) {
 102           gpspacket->time = sg_swap_double( (uint8_t *)buf, 0 );
 103           gpspacket->lat  = sg_swap_double( (uint8_t *)buf, 8 );
 104           gpspacket->lon  = sg_swap_double( (uint8_t *)buf, 16 );
 105           gpspacket->alt  = sg_swap_double( (uint8_t *)buf, 24 );
 106           gpspacket->vn   = sg_swap_double( (uint8_t *)buf, 32 );
 107           gpspacket->ve   = sg_swap_double( (uint8_t *)buf, 40 );
 108           gpspacket->vd   = sg_swap_double( (uint8_t *)buf, 48 );
 109       }
 110     } else if ( id == IMU_PACKET ) {
 111       *imupacket = *(struct imu *)buf;
 112       if ( sg_swap ) {
 113           imupacket->time = sg_swap_double( (uint8_t *)buf, 0 );
 114           imupacket->p    = sg_swap_double( (uint8_t *)buf, 8 );
 115           imupacket->q    = sg_swap_double( (uint8_t *)buf, 16 );
 116           imupacket->r    = sg_swap_double( (uint8_t *)buf, 24 );
 117           imupacket->ax   = sg_swap_double( (uint8_t *)buf, 32 );
 118           imupacket->ay   = sg_swap_double( (uint8_t *)buf, 40 );
 119           imupacket->az   = sg_swap_double( (uint8_t *)buf, 48 );
 120           imupacket->hx   = sg_swap_double( (uint8_t *)buf, 56 );
 121           imupacket->hy   = sg_swap_double( (uint8_t *)buf, 64 );
 122           imupacket->hz   = sg_swap_double( (uint8_t *)buf, 72 );
 123           imupacket->Ps   = sg_swap_double( (uint8_t *)buf, 80 );
 124           imupacket->Pt   = sg_swap_double( (uint8_t *)buf, 88 );
 125           imupacket->phi  = sg_swap_double( (uint8_t *)buf, 96 );
 126           imupacket->the  = sg_swap_double( (uint8_t *)buf, 104 );
 127           imupacket->psi  = sg_swap_double( (uint8_t *)buf, 112 );
 128       }
 129       // printf("imu.time = %.4f  size = %d\n", imupacket->time, sizeof(struct imu));
 130     } else if ( id == NAV_PACKET ) {
 131       *navpacket = *(struct nav *)buf;
 132       if ( sg_swap ) {
 133           navpacket->time = sg_swap_double( (uint8_t *)buf, 0 );
 134           navpacket->lat  = sg_swap_double( (uint8_t *)buf, 8 );
 135           navpacket->lon  = sg_swap_double( (uint8_t *)buf, 16 );
 136           navpacket->alt  = sg_swap_double( (uint8_t *)buf, 24 );
 137           navpacket->vn   = sg_swap_double( (uint8_t *)buf, 32 );
 138           navpacket->ve   = sg_swap_double( (uint8_t *)buf, 40 );
 139           navpacket->vd   = sg_swap_double( (uint8_t *)buf, 48 );
 140       }
 141     } else if ( id == SERVO_PACKET ) {
 142       *servopacket = *(struct servo *)buf;
 143       if ( sg_swap ) {
 144           servopacket->time = sg_swap_double( (uint8_t *)buf, 0 );
 145       }
 146       // printf("servo time = %.3f\n", servopacket->time);
 147     } else if ( id == HEALTH_PACKET ) {
 148       *healthpacket = *(struct health *)buf;
 149       if ( sg_swap ) {
 150           healthpacket->time = sg_swap_double( (uint8_t *)buf, 0 );
 151       }
 152     } else {
 153         cout << "unknown id = " << id << endl;
 154     }
 155 }
 156
 157
 158 // load the named stream log file into internal buffers
 159 bool UGEARTrack::load_stream( const string &file, bool ignore_checksum ) {
 160     int count = 0;
 161
 162     gps gpspacket;
 163     imu imupacket;
 164     nav navpacket;
 165     servo servopacket;
 166     health healthpacket;
 167
 168     double gps_time = 0;
 169     double imu_time = 0;
 170     double nav_time = 0;
 171     double servo_time = 0;
 172     double health_time = 0;
 173
 174     gps_data.clear();
 175     imu_data.clear();
 176     nav_data.clear();
 177     servo_data.clear();
 178     health_data.clear();
 179
 180     // open the file
 181     SGFile input( file );
 182     if ( !input.open( SG_IO_IN ) ) {
 183         cout << "Cannot open file: " << file << endl;
 184         return false;
 185     }
 186
 187     while ( ! input.eof() ) {
 188         // cout << "looking for next message ..." << endl;
 189         int id = next_message( &input, NULL, &gpspacket, &imupacket,
 190                                &navpacket, &servopacket, &healthpacket,
 191                                ignore_checksum );
 192         count++;
 193
 194         if ( id == GPS_PACKET ) {
 195             if ( gpspacket.time > gps_time ) {
 196                 gps_data.push_back( gpspacket );
 197                 gps_time = gpspacket.time;
 198             } else {
 199               cout << "oops gps back in time: " << gpspacket.time << " " << gps_time << endl;
 200             }
 201         } else if ( id == IMU_PACKET ) {
 202             if ( imupacket.time > imu_time ) {
 203                 imu_data.push_back( imupacket );
 204                 imu_time = imupacket.time;
 205             } else {
 206                 cout << "oops imu back in time" << endl;
 207             }
 208         } else if ( id == NAV_PACKET ) {
 209             if ( navpacket.time > nav_time ) {
 210                 nav_data.push_back( navpacket );
 211                 nav_time = navpacket.time;
 212             } else {
 213                 cout << "oops nav back in time" << endl;
 214             }
 215         } else if ( id == SERVO_PACKET ) {
 216             if ( servopacket.time > servo_time ) {
 217                 servo_data.push_back( servopacket );
 218                 servo_time = servopacket.time;
 219             } else {
 220                 cout << "oops servo back in time" << endl;
 221             }
 222         } else if ( id == HEALTH_PACKET ) {
 223             if ( healthpacket.time > health_time ) {
 224                 health_data.push_back( healthpacket );
 225                 health_time = healthpacket.time;
 226             } else {
 227                 cout << "oops health back in time" << endl;
 228             }
 229         }
 230     }
 231
 232     cout << "processed " << count << " messages" << endl;
 233     return true;
 234 }
 235
 236
 237 // load the named stream log file into internal buffers
 238 bool UGEARTrack::load_flight( const string &path ) {
 239     gps gpspacket;
 240     imu imupacket;
 241     nav navpacket;
 242     servo servopacket;
 243     health healthpacket;
 244
 245     gps_data.clear();
 246     imu_data.clear();
 247     nav_data.clear();
 248     servo_data.clear();
 249     health_data.clear();
 250
 251     gzFile fgps = NULL;
 252     gzFile fimu = NULL;
 253     gzFile fnav = NULL;
 254     gzFile fservo = NULL;
 255     gzFile fhealth = NULL;
 256
 257     SGPath file;
 258     int size;
 259
 260     // open the gps file
 261     file = path; file.append( "gps.dat.gz" );
 262     if ( (fgps = gzopen( file.c_str(), "r" )) == NULL ) {
 263         printf("Cannont open %s\n", file.c_str());
 264         return false;
 265     }
 266
 267     size = sizeof( struct gps );
 268     printf("gps size = %d\n", size);
 269     while ( gzread( fgps, &gpspacket, size ) == size ) {
 270         gps_data.push_back( gpspacket );
 271     }
 272
 273     // open the imu file
 274     file = path; file.append( "imu.dat.gz" );
 275     if ( (fimu = gzopen( file.c_str(), "r" )) == NULL ) {
 276         printf("Cannot open %s\n", file.c_str());
 277         return false;
 278     }
 279
 280     size = sizeof( struct imu );
 281     printf("imu size = %d\n", size);
 282     while ( gzread( fimu, &imupacket, size ) == size ) {
 283         imu_data.push_back( imupacket );
 284     }
 285
 286     // open the nav file
 287     file = path; file.append( "nav.dat.gz" );
 288     if ( (fnav = gzopen( file.c_str(), "r" )) == NULL ) {
 289         printf("Cannot open %s\n", file.c_str());
 290         return false;
 291     }
 292
 293     size = sizeof( struct nav );
 294     printf("nav size = %d\n", size);
 295     while ( gzread( fnav, &navpacket, size ) == size ) {
 296         // printf("%.4f %.4f\n", navpacket.lat, navpacket.lon);
 297         nav_data.push_back( navpacket );
 298     }
 299
 300     // open the servo file
 301     file = path; file.append( "servo.dat.gz" );
 302     if ( (fservo = gzopen( file.c_str(), "r" )) == NULL ) {
 303         printf("Cannot open %s\n", file.c_str());
 304         return false;
 305     }
 306
 307     size = sizeof( struct servo );
 308     printf("servo size = %d\n", size);
 309     while ( gzread( fservo, &servopacket, size ) == size ) {
 310         servo_data.push_back( servopacket );
 311     }
 312
 313     // open the health file
 314     file = path; file.append( "health.dat.gz" );
 315     if ( (fhealth = gzopen( file.c_str(), "r" )) == NULL ) {
 316         printf("Cannot open %s\n", file.c_str());
 317         return false;
 318     }
 319
 320     size = sizeof( struct health );
 321     printf("health size = %d\n", size);
 322     while ( gzread( fhealth, &healthpacket, size ) == size ) {
 323         health_data.push_back( healthpacket );
 324     }
 325
 326     return true;
 327 }
 328
 329
 330 // attempt to work around some system dependent issues.  Our read can
 331 // return < data than we want.
 332 int myread( SGIOChannel *ch, SGIOChannel *log, char *buf, int length ) {
 333     bool myeof = false;
 334     int result = 0;
 335     if ( !myeof ) {
 336       result = ch->read( buf, length );
 337       // cout << "wanted " << length << " read " << result << " bytes" << endl;
 338       if ( ch->get_type() == sgFileType ) {
 339         myeof = ((SGFile *)ch)->eof();
 340       }
 341     }
 342
 343     if ( result > 0 && log != NULL ) {
 344         log->write( buf, result );
 345     }
 346
 347     return result;
 348 }
 349
 350 // attempt to work around some system dependent issues.  Our read can
 351 // return < data than we want.
 352 int serial_read( SGSerialPort *serial, SGIOChannel *log,
 353                  char *buf, int length )
 354 {
 355     int result = 0;
 356     int bytes_read = 0;
 357     char *tmp = buf;
 358
 359     while ( bytes_read < length ) {
 360       result = serial->read_port( tmp, length - bytes_read );
 361       bytes_read += result;
 362       tmp += result;
 363       // cout << "  read " << bytes_read << " of " << length << endl;
 364     }
 365
 366     if ( bytes_read > 0 && log != NULL ) {
 367       log->write( buf, bytes_read );
 368     }
 369
 370     return bytes_read;
 371 }
 372
 373
 374 // load the next message of a real time data stream
 375 int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
 376                               gps *gpspacket, imu *imupacket, nav *navpacket,
 377                               servo *servopacket, health *healthpacket,
 378                               bool ignore_checksum )
 379 {
 380     char tmpbuf[256];
 381     char savebuf[256];
 382
 383     // cout << "in next_message()" << endl;
 384
 385     bool myeof = false;
 386
 387     // scan for sync characters
 388     uint8_t sync0, sync1;
 389     myread( ch, log, tmpbuf, 2 );
 390     sync0 = (unsigned char)tmpbuf[0];
 391     sync1 = (unsigned char)tmpbuf[1];
 392     while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) {
 393         sync0 = sync1;
 394         myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
 395         cout << "scanning for start of message "
 396              << (unsigned int)sync0 << " " << (unsigned int)sync1
 397              << ", eof = " << ch->eof() << endl;
 398         if ( ch->get_type() == sgFileType ) {
 399             myeof = ((SGFile *)ch)->eof();
 400         }
 401     }
 402
 403     cout << "found start of message ..." << endl;
 404
 405     // read message id and size
 406     myread( ch, log, tmpbuf, 2 );
 407     uint8_t id = (unsigned char)tmpbuf[0];
 408     uint8_t size = (unsigned char)tmpbuf[1];
 409     // cout << "message = " << (int)id << " size = " << (int)size << endl;
 410
 411     // load message
 412     if ( ch->get_type() == sgFileType ) {
 413         int count = myread( ch, log, savebuf, size );
 414         if ( count != size ) {
 415             cout << "ERROR: didn't read enough bytes!" << endl;
 416         }
 417     } else {
 418 #ifdef READ_ONE_BY_ONE
 419         for ( int i = 0; i < size; ++i ) {
 420             myread( ch, log, tmpbuf, 1 ); savebuf[i] = tmpbuf[0];
 421         }
 422 #else
 423         myread( ch, log, savebuf, size );
 424 #endif
 425     }
 426
 427     // read checksum
 428     myread( ch, log, tmpbuf, 2 );
 429     uint8_t cksum0 = (unsigned char)tmpbuf[0];
 430     uint8_t cksum1 = (unsigned char)tmpbuf[1];
 431
 432     if ( validate_cksum( id, size, savebuf, cksum0, cksum1, ignore_checksum ) )
 433     {
 434         parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket,
 435                    healthpacket );
 436         return id;
 437     }
 438
 439     cout << "Check sum failure!" << endl;
 440     return -1;
 441 }
 442
 443
 444 // load the next message of a real time data stream
 445 int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
 446                               gps *gpspacket, imu *imupacket, nav *navpacket,
 447                               servo *servopacket, health *healthpacket,
 448                               bool ignore_checksum )
 449 {
 450     char tmpbuf[256];
 451     char savebuf[256];
 452     int result = 0;
 453
 454     // cout << "in next_message()" << endl;
 455
 456     bool myeof = false;
 457
 458     // scan for sync characters
 459     int scan_count = 0;
 460     uint8_t sync0, sync1;
 461     result = serial_read( serial, log, tmpbuf, 2 );
 462     sync0 = (unsigned char)tmpbuf[0];
 463     sync1 = (unsigned char)tmpbuf[1];
 464     while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) {
 465         scan_count++;
 466         sync0 = sync1;
 467         serial_read( serial, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
 468         // cout << "scanning for start of message "
 469         //      << (unsigned int)sync0 << " " << (unsigned int)sync1
 470         //      << endl;
 471     }
 472
 473     if ( scan_count > 0 ) {
 474         cout << "found start of message after discarding " << scan_count
 475              << " bytes" << endl;
 476     }
 477     // cout << "found start of message ..." << endl;
 478
 479     // read message id and size
 480     serial_read( serial, log, tmpbuf, 2 );
 481     uint8_t id = (unsigned char)tmpbuf[0];
 482     uint8_t size = (unsigned char)tmpbuf[1];
 483     // cout << "message = " << (int)id << " size = " << (int)size << endl;
 484
 485     // load message
 486     serial_read( serial, log, savebuf, size );
 487
 488     // read checksum
 489     serial_read( serial, log, tmpbuf, 2 );
 490     uint8_t cksum0 = (unsigned char)tmpbuf[0];
 491     uint8_t cksum1 = (unsigned char)tmpbuf[1];
 492     // cout << "cksum0 = " << (int)cksum0 << " cksum1 = " << (int)cksum1
 493     //      << endl;
 494
 495     if ( validate_cksum( id, size, savebuf, cksum0, cksum1, ignore_checksum ) )
 496     {
 497         parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket,
 498                    healthpacket );
 499
 500         return id;
 501     }
 502
 503     cout << "Check sum failure!" << endl;
 504     return -1;
 505
 506
 507 }
 508
 509
 510 static double interp( double a, double b, double p, bool rotational = false ) {
 511     double diff = b - a;
 512     if ( rotational ) {
 513         // special handling of rotational data
 514         if ( diff > SGD_PI ) {
 515             diff -= SGD_2PI;
 516         } else if ( diff < -SGD_PI ) {
 517             diff += SGD_2PI;
 518         }
 519     }
 520     return a + diff * p;
 521 }
 522
 523
 524 gps UGEARInterpGPS( const gps A, const gps B, const double percent )
 525 {
 526     gps p;
 527     p.time = interp(A.time, B.time, percent);
 528     p.lat = interp(A.lat, B.lat, percent);
 529     p.lon = interp(A.lon, B.lon, percent);
 530     p.alt = interp(A.alt, B.alt, percent);
 531     p.ve = interp(A.ve, B.ve, percent);
 532     p.vn = interp(A.vn, B.vn, percent);
 533     p.vd = interp(A.vd, B.vd, percent);
 534     p.ITOW = (int)interp(A.ITOW, B.ITOW, percent);
 535     p.err_type = A.err_type;
 536
 537     return p;
 538 }
 539
 540 imu UGEARInterpIMU( const imu A, const imu B, const double percent )
 541 {
 542     imu p;
 543     p.time = interp(A.time, B.time, percent);
 544     p.p = interp(A.p, B.p, percent);
 545     p.q = interp(A.q, B.q, percent);
 546     p.r = interp(A.r, B.r, percent);
 547     p.ax = interp(A.ax, B.ax, percent);
 548     p.ay = interp(A.ay, B.ay, percent);
 549     p.az = interp(A.az, B.az, percent);
 550     p.hx = interp(A.hx, B.hx, percent);
 551     p.hy = interp(A.hy, B.hy, percent);
 552     p.hz = interp(A.hz, B.hz, percent);
 553     p.Ps = interp(A.Ps, B.Ps, percent);
 554     p.Pt = interp(A.Pt, B.Pt, percent);
 555     p.phi = interp(A.phi, B.phi, percent);
 556     p.the = interp(A.the, B.the, percent);
 557     p.psi = interp(A.psi, B.psi, percent);
 558     p.err_type = A.err_type;
 559
 560     return p;
 561 }
 562
 563 nav UGEARInterpNAV( const nav A, const nav B, const double percent )
 564 {
 565     nav p;
 566     p.time = interp(A.time, B.time, percent);
 567     p.lat = interp(A.lat, B.lat, percent);
 568     p.lon = interp(A.lon, B.lon, percent);
 569     p.alt = interp(A.alt, B.alt, percent);
 570     p.ve = interp(A.ve, B.ve, percent);
 571     p.vn = interp(A.vn, B.vn, percent);
 572     p.vd = interp(A.vd, B.vd, percent);
 573     p.err_type = A.err_type;
 574
 575     return p;
 576 }
 577
 578
 579 servo UGEARInterpSERVO( const servo A, const servo B, const double percent )
 580 {
 581     servo p;
 582     for ( int i = 0; i < 8; ++i ) {
 583       p.chn[i] = (uint16_t)interp(A.chn[i], B.chn[i], percent);
 584     }
 585     p.status = A.status;
 586
 587     return p;
 588 }
 589
 590
 591 health UGEARInterpHEALTH( const health A, const health B, const double percent )
 592 {
 593     health p;
 594     p.volts_raw = interp(A.volts_raw, B.volts_raw, percent);
 595     p.volts = interp(A.volts, B.volts, percent);
 596     p.est_seconds = (uint16_t)interp(A.est_seconds, B.est_seconds, percent);
 597     p.time = interp(A.time, B.time, percent);
 598
 599     return p;
 600 }
 601