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