utils/GPSsmooth/MIDG-II.cxx

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