utils/GPSsmooth/MIDG-II.cxx

   1 #include <iostream>
   2
   3 #include <plib/ul.h>
   4
   5 #include <simgear/constants.h>
   6 #include <simgear/io/sg_file.hxx>
   7 #include <simgear/math/sg_geodesy.hxx>
   8 #include <simgear/misc/sgstream.hxx>
   9 #include <simgear/misc/strutils.hxx>
  10
  11 #include "MIDG-II.hxx"
  12
  13 SG_USING_STD(cout);
  14 SG_USING_STD(endl);
  15
  16
  17 MIDGTrack::MIDGTrack() {};
  18 MIDGTrack::~MIDGTrack() {};
  19
  20
  21
  22
  23 static uint32_t read_swab( char *buf, size_t offset, size_t size ) {
  24     uint32_t result = 0;
  25
  26     char *ptr = buf + offset;
  27
  28     // MIDG data is big endian so swap if needed.
  29     if ( ulIsLittleEndian ) {
  30         if ( size == 4 ) {
  31             ulEndianSwap( (uint32_t *)ptr );
  32         } else if ( size == 2 ) {
  33             ulEndianSwap( (uint16_t *)ptr );
  34         }
  35     }
  36
  37     if ( size == 4 ) {
  38         result = *(uint32_t *)ptr;
  39     } else if ( size == 2 ) {
  40         result = *(uint16_t *)ptr;
  41     } else if ( size == 1 ) {
  42         result = *(uint8_t *)ptr;
  43     } else {
  44         cout << "unknown size in read_swab()" << endl;
  45     }
  46
  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 MIDGTrack::parse_msg( const int id, char *buf, MIDGpos *pos, MIDGatt *att )
  85 {
  86     if ( id == 1 ) {
  87         uint32_t ts;
  88         uint16_t status;
  89         int16_t temp;
  90
  91         // cout << "message 1 =" << endl;
  92
  93         // timestamp
  94         ts = (uint32_t)read_swab( buf, 0, 4 );
  95         // cout << "  time stamp = " << ts << endl;
  96
  97         // status
  98         status = (uint16_t)read_swab( buf, 4, 2 );
  99         // cout << "  status = " << status << endl;
 100
 101         // temp
 102         temp = (int16_t)read_swab( buf, 6, 2 );
 103         // cout << "  temp = " << temp << endl;
 104
 105     } else if ( id == 2 ) {
 106         uint32_t ts;
 107         int16_t p, q, r;
 108         int16_t ax, ay, az;
 109         int16_t mx, my, mz;
 110         uint8_t flags;
 111
 112         // cout << "message 2 =" << endl;
 113
 114         // timestamp
 115         ts = (uint32_t)read_swab( buf, 0, 4 );
 116         // cout << "  time stamp = " << ts << endl;
 117
 118         // p, q, r
 119         p = (int16_t)read_swab( buf, 4, 2 );
 120         q = (int16_t)read_swab( buf, 6, 2 );
 121         r = (int16_t)read_swab( buf, 8, 2 );
 122         // cout << "  pqr = " << p << "," << q << "," << r << endl;
 123
 124         // ax, ay, az
 125         ax = (int16_t)read_swab( buf, 10, 2 );
 126         ay = (int16_t)read_swab( buf, 12, 2 );
 127         az = (int16_t)read_swab( buf, 14, 2 );
 128         // cout << "  ax ay az = " << ax << "," << ay << "," << az << endl;
 129
 130         // mx, my, mz
 131         mx = (int16_t)read_swab( buf, 16, 2 );
 132         my = (int16_t)read_swab( buf, 18, 2 );
 133         mz = (int16_t)read_swab( buf, 20, 2 );
 134         // cout << "  mx my mz = " << mx << "," << my << "," << mz << endl;
 135
 136         // flags
 137         flags = (uint8_t)read_swab( buf, 22, 1 );
 138         // cout << "  GPS 1PPS flag = " << (int)(flags & (1 << 6))
 139         //      << " Timestamp is gps = " << (int)(flags & (1 << 7)) << endl;
 140
 141     } else if ( id == 3 ) {
 142         uint32_t ts;
 143         int16_t mx, my, mz;
 144         uint8_t flags;
 145
 146         // cout << "message 3 =" << endl;
 147
 148         // timestamp
 149         ts = (uint32_t)read_swab( buf, 0, 4 );
 150         // cout << "  time stamp = " << ts << endl;
 151
 152         // mx, my, mz
 153         mx = (int16_t)read_swab( buf, 4, 2 );
 154         my = (int16_t)read_swab( buf, 6, 2 );
 155         mz = (int16_t)read_swab( buf, 8, 2 );
 156         // cout << "  mx my mz = " << mx << "," << my << "," << mz << endl;
 157
 158         // flags
 159         flags = (uint8_t)read_swab( buf, 10, 1 );
 160         // cout << "  GPS 1PPS flag = " << (int)(flags & (1 << 6)) << endl;
 161
 162     } else if ( id == 10 ) {
 163         uint32_t ts;
 164         int16_t p, q, r;
 165         int16_t ax, ay, az;
 166         int16_t yaw, pitch, roll;
 167         int32_t Qw, Qx, Qy, Qz;
 168         uint8_t flags;
 169
 170         // cout << "message 10 =" << endl;
 171
 172         // timestamp
 173         ts = (uint32_t)read_swab( buf, 0, 4 );
 174         // cout << "  att time stamp = " << ts << endl;
 175         att->midg_time = MIDGTime( ts );
 176
 177         // p, q, r
 178         p = (int16_t)read_swab( buf, 4, 2 );
 179         q = (int16_t)read_swab( buf, 6, 2 );
 180         r = (int16_t)read_swab( buf, 8, 2 );
 181         // cout << "  pqr = " << p << "," << q << "," << r << endl;
 182
 183         // ax, ay, az
 184         ax = (int16_t)read_swab( buf, 10, 2 );
 185         ay = (int16_t)read_swab( buf, 12, 2 );
 186         az = (int16_t)read_swab( buf, 14, 2 );
 187         // cout << "  ax ay az = " << ax << "," << ay << "," << az << endl;
 188
 189         // yaw, pitch, roll
 190         yaw =   (int16_t)read_swab( buf, 16, 2 );
 191         pitch = (int16_t)read_swab( buf, 18, 2 );
 192         roll =  (int16_t)read_swab( buf, 20, 2 );
 193         // cout << "  yaw, pitch, roll = " << yaw << "," << pitch << ","
 194         //      << roll << endl;
 195         att->yaw_rad = ( (double)yaw / 100.0 ) * SG_PI / 180.0;
 196         att->pitch_rad = ( (double)pitch / 100.0 ) * SG_PI / 180.0;
 197         att->roll_rad = ( (double)roll / 100.0 ) * SG_PI / 180.0;
 198
 199         // Qw, Qx, Qy, Qz
 200         Qw = (int32_t)read_swab( buf, 22, 4 );
 201         Qx = (int32_t)read_swab( buf, 26, 4 );
 202         Qy = (int32_t)read_swab( buf, 30, 4 );
 203         Qz = (int32_t)read_swab( buf, 34, 4 );
 204         // cout << "  Qw,Qx,Qy,Qz = " << Qw << "," << Qx << "," << Qy << ","
 205         //      << Qz << endl;
 206
 207         // flags
 208         flags = (uint8_t)read_swab( buf, 38, 1 );
 209         // cout << "  External hdg measurement applied = "
 210         //      << (int)(flags & (1 << 3)) << endl
 211         //      << "  Magnatometer measurement applied = "
 212         //      << (int)(flags & (1 << 4)) << endl
 213         //      << "  DGPS = " << (int)(flags & (1 << 5)) << endl
 214         //      << "  Timestamp is gps = " << (int)(flags & (1 << 6)) << endl
 215         //      << "  INS mode = " << (int)(flags & (1 << 7))
 216         //      << endl;
 217
 218     } else if ( id == 12 ) {
 219         uint32_t ts;
 220         int32_t posx, posy, posz;
 221         int32_t velx, vely, velz;
 222         uint8_t flags;
 223
 224         // cout << "message 12 =" << endl;
 225
 226         // timestamp
 227         ts = (uint32_t)read_swab( buf, 0, 4 );
 228         // cout << "  pos time stamp = " << ts << endl;
 229         pos->midg_time = MIDGTime( ts );
 230
 231         // posx, posy, posz
 232         posx = (int32_t)read_swab( buf, 4, 4 );
 233         posy = (int32_t)read_swab( buf, 8, 4 );
 234         posz = (int32_t)read_swab( buf, 12, 4 );
 235         // cout << "  pos = " << posx << "," << posy << "," << posz << endl;
 236
 237         double xyz[3];
 238         xyz[0] = posx/100; xyz[1] = posy/100; xyz[2] = posz/100;
 239         double lat, lon, alt;
 240         sgCartToGeod(xyz, &lat, &lon, &alt);
 241         pos->lat_deg = lat * 180.0 / SG_PI;
 242         pos->lon_deg = lon * 180.0 / SG_PI;
 243         pos->altitude_msl = alt;
 244         // cout << "  lon = " << pos->lon_deg << " lat = " << pos->lat_deg
 245         //      << " alt = " << pos->altitude_msl << endl;
 246
 247         // velx, vely, velz
 248         velx = (int32_t)read_swab( buf, 16, 4 );
 249         vely = (int32_t)read_swab( buf, 20, 4 );
 250         velz = (int32_t)read_swab( buf, 24, 4 );
 251         // cout << "  vel = " << velx << "," << vely << "," << velz << endl;
 252         double tmp1 = velx*velx + vely*vely + velz*velz;
 253         double vel_cms = sqrt( tmp1 );
 254         double vel_ms = vel_cms / 100.0;
 255         pos->speed_kts = vel_ms * SG_METER_TO_NM * 3600;
 256
 257         // flags
 258         flags = (uint8_t)read_swab( buf, 28, 1 );
 259         // cout << "  ENU pos rel to 1st fix = " << (int)(flags & (1 << 0)) << endl
 260         //      << "  Velocity format = " << (int)(flags & (1 << 1)) << endl
 261         //      << "  bit 2 = " << (int)(flags & (1 << 2)) << endl
 262         //      << "  bit 3 = " << (int)(flags & (1 << 3)) << endl
 263         //      << "  GPS pos/vel valid = " << (int)(flags & (1 << 4)) << endl
 264         //      << "  DGPS = " << (int)(flags & (1 << 5)) << endl
 265         //      << "  Timestamp is gps = " << (int)(flags & (1 << 6)) << endl
 266         //      << "  Solution src (0=gps, 1=ins) = " << (int)(flags & (1 << 7))
 267         //      << endl;
 268
 269     } else if ( id == 20 ) {
 270         uint32_t gps_ts, gps_week;
 271         uint16_t details;
 272         int32_t gps_posx, gps_posy, gps_posz;
 273         int32_t gps_velx, gps_vely, gps_velz;
 274         int16_t pdop, pacc, sacc;
 275
 276         // cout << "message 20 =" << endl;
 277
 278         // timestamp -- often slightly off from midg time stamp so
 279         // let's not use gps ts to determine if we need to push the
 280         // previous data or not, just roll it into the current data
 281         // independent of time stamp.
 282         gps_ts = (uint32_t)read_swab( buf, 0, 4 );
 283         // pt->midg_time = MIDGTime( ts );
 284
 285         gps_week = (uint16_t)read_swab( buf, 4, 2 );
 286         // cout << "  gps time stamp = " << gps_ts << " week = " << gps_week
 287         //      <<  endl;
 288
 289         // details
 290         details = (uint16_t)read_swab( buf, 6, 2 );
 291         // cout << "  details = " << details <<  endl;
 292
 293         // gps_posx, gps_posy, gps_posz
 294         gps_posx = (int32_t)read_swab( buf, 8, 4 );
 295         gps_posy = (int32_t)read_swab( buf, 12, 4 );
 296         gps_posz = (int32_t)read_swab( buf, 16, 4 );
 297         // cout << "  gps_pos = " << gps_posx << "," << gps_posy << ","
 298         //      << gps_posz << endl;
 299
 300         // gps_velx, gps_vely, gps_velz
 301         gps_velx = (int32_t)read_swab( buf, 20, 4 );
 302         gps_vely = (int32_t)read_swab( buf, 24, 4 );
 303         gps_velz = (int32_t)read_swab( buf, 28, 4 );
 304         // cout << "  gps_vel = " << gps_velx << "," << gps_vely << ","
 305         //      << gps_velz << endl;
 306
 307         // position dop
 308         pdop = (uint16_t)read_swab( buf, 32, 2 );
 309         // cout << "  pdop = " << pdop <<  endl;
 310
 311         // position accuracy
 312         pacc = (uint16_t)read_swab( buf, 34, 2 );
 313         // cout << "  pacc = " << pacc <<  endl;
 314
 315         // speed accuracy
 316         sacc = (uint16_t)read_swab( buf, 36, 2 );
 317         // cout << "  sacc = " << sacc <<  endl;
 318
 319     } else {
 320         cout << "unknown id = " << id << endl;
 321     }
 322 }
 323
 324
 325 // load the specified file, return the number of records loaded
 326 bool MIDGTrack::load( const string &file ) {
 327     int count = 0;
 328
 329     MIDGpos pos;
 330     MIDGatt att;
 331
 332     uint32_t pos_time = 1;
 333     uint32_t att_time = 1;
 334
 335     pos_data.clear();
 336     att_data.clear();
 337
 338     // open the file
 339     SGFile input( file );
 340     if ( !input.open( SG_IO_IN ) ) {
 341         cout << "Cannot open file: " << file << endl;
 342         return false;
 343     }
 344
 345     while ( ! input.eof() ) {
 346         // cout << "looking for next message ..." << endl;
 347         int id = next_message( &input, NULL, &pos, &att );
 348         count++;
 349
 350         if ( id == 10 ) {
 351             if ( att.get_msec() > att_time ) {
 352                 att_data.push_back( att );
 353                 att_time = att.get_msec();
 354             } else {
 355                 cout << "oops att back in time" << endl;
 356             }
 357         } else if ( id == 12 ) {
 358             if ( pos.get_msec() > pos_time ) {
 359                 pos_data.push_back( pos );
 360                 pos_time = pos.get_msec();
 361             } else {
 362                 cout << "oops pos back in time" << endl;
 363             }
 364         }
 365     }
 366
 367     cout << "processed " << count << " messages" << endl;
 368     return true;
 369 }
 370
 371
 372 // attempt to work around some system dependent issues.  Our read can
 373 // return < data than we want.
 374 int myread( SGIOChannel *ch, SGIOChannel *log, char *buf, int length ) {
 375     bool myeof = false;
 376     int result = 0;
 377     while ( result != length && !myeof ) {
 378         result = ch->read( buf, length );
 379         if ( ch->get_type() == sgFileType ) {
 380             myeof = ((SGFile *)ch)->eof();
 381         }
 382     }
 383
 384     if ( result > 0 && log != NULL ) {
 385         log->write( buf, result );
 386     }
 387
 388     return result;
 389 }
 390
 391 // load the next message of a real time data stream
 392 int MIDGTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
 393                              MIDGpos *pos, MIDGatt *att )
 394 {
 395     char tmpbuf[256];
 396     char savebuf[256];
 397
 398     // cout << "in next_message()" << endl;
 399
 400     bool myeof = false;
 401
 402     // scan for sync characters
 403     uint8_t sync0, sync1;
 404     myread( ch, log, tmpbuf, 1 ); sync0 = (unsigned char)tmpbuf[0];
 405     myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
 406     while ( (sync0 != 129 || sync1 != 161) && !myeof ) {
 407         sync0 = sync1;
 408         myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
 409         // cout << "scanning for start of message, eof = " << ch->eof() << endl;
 410         if ( ch->get_type() == sgFileType ) {
 411             myeof = ((SGFile *)ch)->eof();
 412         }
 413     }
 414
 415     // cout << "found start of message ..." << endl;
 416
 417     // read message id and size
 418     myread( ch, log, tmpbuf, 1 ); uint8_t id = (unsigned char)tmpbuf[0];
 419     myread( ch, log, tmpbuf, 1 ); uint8_t size = (unsigned char)tmpbuf[0];
 420     // cout << "message = " << (int)id << " size = " << (int)size << endl;
 421
 422     // load message
 423     if ( ch->get_type() == sgFileType ) {
 424         int count = myread( ch, log, savebuf, size );
 425         if ( count != size ) {
 426             cout << "ERROR: didn't read enough bytes!" << endl;
 427         }
 428     } else {
 429         for ( int i = 0; i < size; ++i ) {
 430             myread( ch, log, tmpbuf, 1 ); savebuf[i] = tmpbuf[0];
 431         }
 432     }
 433
 434     // read checksum
 435     myread( ch, log, tmpbuf, 1 ); uint8_t cksum0 = (unsigned char)tmpbuf[0];
 436     myread( ch, log, tmpbuf, 1 ); uint8_t cksum1 = (unsigned char)tmpbuf[0];
 437
 438     if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
 439         parse_msg( id, savebuf, pos, att );
 440         return id;
 441     }
 442
 443     cout << "Check sum failure!" << endl;
 444     return -1;
 445 }
 446
 447
 448 static double interp( double a, double b, double p, bool rotational = false ) {
 449     double diff = b - a;
 450     if ( rotational ) {
 451         // special handling of rotational data
 452         if ( diff > SGD_PI ) {
 453             diff -= SGD_2PI;
 454         } else if ( diff < -SGD_PI ) {
 455             diff += SGD_2PI;
 456         }
 457     }
 458     return a + diff * p;
 459 }
 460
 461
 462 MIDGpos MIDGInterpPos( const MIDGpos A, const MIDGpos B, const double percent )
 463 {
 464     MIDGpos p;
 465     p.midg_time = MIDGTime((uint32_t)interp(A.midg_time.get_msec(),
 466                                             B.midg_time.get_msec(),
 467                                             percent));
 468     p.lat_deg = interp(A.lat_deg, B.lat_deg, percent);
 469     p.lon_deg = interp(A.lon_deg, B.lon_deg, percent);
 470     p.altitude_msl = interp(A.altitude_msl, B.altitude_msl, percent);
 471     p.fix_quality = (int)interp(A.fix_quality, B.fix_quality, percent);
 472     p.num_satellites = (int)interp(A.num_satellites, B.num_satellites, percent);
 473     p.hdop = interp(A.hdop, B.hdop, percent);
 474     p.speed_kts = interp(A.speed_kts, B.speed_kts, percent);
 475     p.course_true = interp(A.course_true, B.course_true, percent, true);
 476
 477     return p;
 478 }
 479
 480 MIDGatt MIDGInterpAtt( const MIDGatt A, const MIDGatt B, const double percent )
 481 {
 482     MIDGatt p;
 483     p.midg_time = MIDGTime((uint32_t)interp(A.midg_time.get_msec(),
 484                                             B.midg_time.get_msec(),
 485                                             percent));
 486     p.yaw_rad = interp(A.yaw_rad, B.yaw_rad, percent, true);
 487     p.pitch_rad = interp(A.pitch_rad, B.pitch_rad, percent, true);
 488     p.roll_rad = interp(A.roll_rad, B.roll_rad, percent, true);
 489
 490     return p;
 491 }