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