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