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     if ( (fgps = gzopen( file.c_str(), "r" )) == NULL ) {
 265         printf("Cannot open %s\n", file.c_str());
 266         return false;
 267     }
 268
 269     size = sizeof( struct gps );
 270     printf("gps size = %d\n", size);
 271     while ( gzread( fgps, &gpspacket, size ) == size ) {
 272         gps_data.push_back( gpspacket );
 273     }
 274
 275     // open the imu file
 276     file = path; file.append( "imu.dat.gz" );
 277     if ( (fimu = gzopen( file.c_str(), "r" )) == NULL ) {
 278         printf("Cannot open %s\n", file.c_str());
 279         return false;
 280     }
 281
 282     size = sizeof( struct imu );
 283     printf("imu size = %d\n", size);
 284     while ( gzread( fimu, &imupacket, size ) == size ) {
 285         imu_data.push_back( imupacket );
 286     }
 287
 288     // open the nav file
 289     file = path; file.append( "nav.dat.gz" );
 290     if ( (fnav = gzopen( file.c_str(), "r" )) == NULL ) {
 291         printf("Cannot open %s\n", file.c_str());
 292         return false;
 293     }
 294
 295     size = sizeof( struct nav );
 296     printf("nav size = %d\n", size);
 297     while ( gzread( fnav, &navpacket, size ) == size ) {
 298         // printf("%.4f %.4f\n", navpacket.lat, navpacket.lon);
 299         nav_data.push_back( navpacket );
 300     }
 301
 302     // open the servo file
 303     file = path; file.append( "servo.dat.gz" );
 304     if ( (fservo = gzopen( file.c_str(), "r" )) == NULL ) {
 305         printf("Cannot open %s\n", file.c_str());
 306         return false;
 307     }
 308
 309     size = sizeof( struct servo );
 310     printf("servo size = %d\n", size);
 311     while ( gzread( fservo, &servopacket, size ) == size ) {
 312         servo_data.push_back( servopacket );
 313     }
 314
 315     // open the health file
 316     file = path; file.append( "health.dat.gz" );
 317     if ( (fhealth = gzopen( file.c_str(), "r" )) == NULL ) {
 318         printf("Cannot open %s\n", file.c_str());
 319         return false;
 320     }
 321
 322     size = sizeof( struct health );
 323     printf("health size = %d\n", size);
 324     while ( gzread( fhealth, &healthpacket, size ) == size ) {
 325         health_data.push_back( healthpacket );
 326     }
 327
 328     return true;
 329 }
 330
 331
 332 // attempt to work around some system dependent issues.  Our read can
 333 // return < data than we want.
 334 int myread( SGIOChannel *ch, SGIOChannel *log, char *buf, int length ) {
 335     bool myeof = false;
 336     int result = 0;
 337     if ( !myeof ) {
 338       result = ch->read( buf, length );
 339       // cout << "wanted " << length << " read " << result << " bytes" << endl;
 340       if ( ch->get_type() == sgFileType ) {
 341         myeof = ((SGFile *)ch)->eof();
 342       }
 343     }
 344
 345     if ( result > 0 && log != NULL ) {
 346         log->write( buf, result );
 347     }
 348
 349     return result;
 350 }
 351
 352 // attempt to work around some system dependent issues.  Our read can
 353 // return < data than we want.
 354 int serial_read( SGSerialPort *serial, SGIOChannel *log,
 355                  char *buf, int length )
 356 {
 357     int result = 0;
 358     int bytes_read = 0;
 359     char *tmp = buf;
 360
 361     while ( bytes_read < length ) {
 362       result = serial->read_port( tmp, length - bytes_read );
 363       bytes_read += result;
 364       tmp += result;
 365       // cout << "  read " << bytes_read << " of " << length << endl;
 366     }
 367
 368     if ( bytes_read > 0 && log != NULL ) {
 369       log->write( buf, bytes_read );
 370     }
 371
 372     return bytes_read;
 373 }
 374
 375
 376 // load the next message of a real time data stream
 377 int UGTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
 378                               gps *gpspacket, imu *imupacket, nav *navpacket,
 379                               servo *servopacket, health *healthpacket,
 380                               bool ignore_checksum )
 381 {
 382     char tmpbuf[256];
 383     char savebuf[256];
 384
 385     // cout << "in next_message()" << endl;
 386
 387     bool myeof = false;
 388
 389     // scan for sync characters
 390     uint8_t sync0, sync1;
 391     myread( ch, log, tmpbuf, 2 );
 392     sync0 = (unsigned char)tmpbuf[0];
 393     sync1 = (unsigned char)tmpbuf[1];
 394     while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) {
 395         sync0 = sync1;
 396         myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
 397         cout << "scanning for start of message "
 398              << (unsigned int)sync0 << " " << (unsigned int)sync1
 399              << ", eof = " << ch->eof() << endl;
 400         if ( ch->get_type() == sgFileType ) {
 401             myeof = ((SGFile *)ch)->eof();
 402         }
 403     }
 404
 405     cout << "found start of message ..." << endl;
 406
 407     // read message id and size
 408     myread( ch, log, tmpbuf, 2 );
 409     uint8_t id = (unsigned char)tmpbuf[0];
 410     uint8_t size = (unsigned char)tmpbuf[1];
 411     // cout << "message = " << (int)id << " size = " << (int)size << endl;
 412
 413     // load message
 414     if ( ch->get_type() == sgFileType ) {
 415         int count = myread( ch, log, savebuf, size );
 416         if ( count != size ) {
 417             cout << "ERROR: didn't read enough bytes!" << endl;
 418         }
 419     } else {
 420 #ifdef READ_ONE_BY_ONE
 421         for ( int i = 0; i < size; ++i ) {
 422             myread( ch, log, tmpbuf, 1 ); savebuf[i] = tmpbuf[0];
 423         }
 424 #else
 425         myread( ch, log, savebuf, size );
 426 #endif
 427     }
 428
 429     // read checksum
 430     myread( ch, log, tmpbuf, 2 );
 431     uint8_t cksum0 = (unsigned char)tmpbuf[0];
 432     uint8_t cksum1 = (unsigned char)tmpbuf[1];
 433
 434     if ( validate_cksum( id, size, savebuf, cksum0, cksum1, ignore_checksum ) )
 435     {
 436         parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket,
 437                    healthpacket );
 438         return id;
 439     }
 440
 441     cout << "Check sum failure!" << endl;
 442     return -1;
 443 }
 444
 445
 446 // load the next message of a real time data stream
 447 int UGTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
 448                            gps *gpspacket, imu *imupacket, nav *navpacket,
 449                            servo *servopacket, health *healthpacket,
 450                            bool ignore_checksum )
 451 {
 452     char tmpbuf[256];
 453     char savebuf[256];
 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     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