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