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