utils/GPSsmooth/UGear.cxx

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