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