utils/GPSsmooth/UGear_main.cxx

   1 #ifdef HAVE_CONFIG_H
   2 #  include <config.h>
   3 #endif
   4
   5 #ifndef _MSC_VER
   6 #  include <strings.h>          // for bzero()
   7 #else
   8 #  define bzero(a,b) memset(a,0,b)
   9 #endif
  10 #include <iostream>
  11 #include <string>
  12
  13 #include <plib/net.h>
  14 #include <plib/sg.h>
  15
  16 #include <simgear/constants.h>
  17 #include <simgear/io/lowlevel.hxx> // endian tests
  18 #include <simgear/io/sg_file.hxx>
  19 #include <simgear/serial/serial.hxx>
  20 #include <simgear/math/sg_geodesy.hxx>
  21 #include <simgear/timing/timestamp.hxx>
  22
  23 #include <Network/net_ctrls.hxx>
  24 #include <Network/net_fdm.hxx>
  25
  26 #include "UGear.hxx"
  27
  28
  29 SG_USING_STD(cout);
  30 SG_USING_STD(endl);
  31 SG_USING_STD(string);
  32
  33
  34 // Network channels
  35 static netSocket fdm_sock, ctrls_sock;
  36
  37 // ugear data
  38 UGEARTrack track;
  39
  40 // Default ports
  41 static int fdm_port = 5505;
  42 static int ctrls_port = 5506;
  43
  44 // Default path
  45 static string infile = "";
  46 static string flight_dir = "";
  47 static string serialdev = "";
  48 static string outfile = "";
  49
  50 // Master time counter
  51 float sim_time = 0.0f;
  52 double frame_us = 0.0f;
  53
  54 // sim control
  55 SGTimeStamp last_time_stamp;
  56 SGTimeStamp current_time_stamp;
  57
  58 // altitude offset
  59 double alt_offset = 0.0;
  60
  61 // skip initial seconds
  62 double skip = 0.0;
  63
  64 // for speed estimate
  65 double last_lat = 0.0, last_lon = 0.0;
  66 double kts_filter = 0.0;
  67
  68 bool inited = false;
  69
  70 bool run_real_time = true;
  71
  72 bool ignore_checksum = false;
  73
  74 bool sg_swap = false;
  75
  76 bool use_ground_track_hdg = false;
  77 bool use_ground_speed = false;
  78
  79 bool est_controls = false;
  80
  81 // The function htond is defined this way due to the way some
  82 // processors and OSes treat floating point values.  Some will raise
  83 // an exception whenever a "bad" floating point value is loaded into a
  84 // floating point register.  Solaris is notorious for this, but then
  85 // so is LynxOS on the PowerPC.  By translating the data in place,
  86 // there is no need to load a FP register with the "corruped" floating
  87 // point value.  By doing the BIG_ENDIAN test, I can optimize the
  88 // routine for big-endian processors so it can be as efficient as
  89 // possible
  90 static void htond (double &x)
  91 {
  92     if ( sgIsLittleEndian() ) {
  93         int    *Double_Overlay;
  94         int     Holding_Buffer;
  95
  96         Double_Overlay = (int *) &x;
  97         Holding_Buffer = Double_Overlay [0];
  98
  99         Double_Overlay [0] = htonl (Double_Overlay [1]);
 100         Double_Overlay [1] = htonl (Holding_Buffer);
 101     } else {
 102         return;
 103     }
 104 }
 105
 106 // Float version
 107 static void htonf (float &x)
 108 {
 109     if ( sgIsLittleEndian() ) {
 110         int    *Float_Overlay;
 111         int     Holding_Buffer;
 112
 113         Float_Overlay = (int *) &x;
 114         Holding_Buffer = Float_Overlay [0];
 115
 116         Float_Overlay [0] = htonl (Holding_Buffer);
 117     } else {
 118         return;
 119     }
 120 }
 121
 122
 123 static void ugear2fg( gps *gpspacket, imu *imupacket, nav *navpacket,
 124                       servo *servopacket, health *healthpacket,
 125                       FGNetFDM *fdm, FGNetCtrls *ctrls )
 126 {
 127     unsigned int i;
 128
 129     // Version sanity checking
 130     fdm->version = FG_NET_FDM_VERSION;
 131
 132     // Aero parameters
 133     fdm->longitude = navpacket->lon * SG_DEGREES_TO_RADIANS;
 134     fdm->latitude = navpacket->lat * SG_DEGREES_TO_RADIANS;
 135     fdm->altitude = navpacket->alt + alt_offset;
 136     fdm->agl = -9999.0;
 137     fdm->psi = imupacket->psi; // heading
 138     fdm->phi = imupacket->phi; // roll
 139     fdm->theta = imupacket->the; // pitch;
 140
 141     fdm->phidot = 0.0;
 142     fdm->thetadot = 0.0;
 143     fdm->psidot = 0.0;
 144
 145     // estimate speed
 146     // double az1, az2, dist;
 147     // geo_inverse_wgs_84( fdm->altitude, last_lat, last_lon,
 148     //                     fdm->latitude, fdm->longitude, &az1, &az2, &dist );
 149     // last_lat = fdm->latitude;
 150     // last_lon = fdm->longitude;
 151     // double v_ms = dist / (frame_us / 1000000);
 152     // double v_kts = v_ms * SG_METER_TO_NM * 3600;
 153     // kts_filter = (0.9 * kts_filter) + (0.1 * v_kts);
 154     // printf("dist = %.5f  kts est = %.2f\n", dist, kts_filter);
 155
 156     double vn = navpacket->vn;
 157     double ve = navpacket->ve;
 158     double vd = navpacket->vd;
 159
 160     if ( use_ground_track_hdg ) {
 161         fdm->psi = SGD_PI * 0.5 - atan2(vn, ve); // heading
 162     }
 163
 164     double mps = 0.0;
 165     if ( use_ground_speed ) {
 166         mps = sqrt( vn*vn + ve*ve + vd*vd );
 167     } else {
 168         mps = imupacket->Pt;
 169     }
 170     // double mph = mps * 3600 / 1609.3440;
 171     double kts = mps * SG_MPS_TO_KT;
 172     fdm->vcas = kts;
 173     // printf("speed = %.2f mph  %.2f kts\n", mph, kts );
 174     static double Ps = 0.0, Ps_last = 0.0, t_last = 0.0;
 175     Ps_last = Ps;
 176     Ps = 0.92 * Ps + 0.08 * imupacket->Ps;
 177     double climb = (Ps - Ps_last) / (imupacket->time - t_last);
 178     t_last = imupacket->time;
 179     static double climbf = 0.0;
 180     climbf = 0.994 * climbf + 0.006 * climb;
 181     fdm->climb_rate = climbf; // fps
 182
 183     static double Ps_error = 0.0;
 184     static double Ps_count = 0;
 185     const double span = 10000.0;
 186     Ps_count += 1.0; if (Ps_count > (span-1.0)) { Ps_count = (span-1.0); }
 187     double error = navpacket->alt - Ps;
 188     Ps_error = (Ps_count/span) * Ps_error + ((span-Ps_count)/span) * error;
 189     fdm->altitude = Ps +  Ps_error;
 190
 191     printf("%.3f, %.3f, %.3f, %.3f, %.8f, %.8f, %.3f, %.3f, %.3f, %.3f, %.3f\n",
 192            imupacket->time, imupacket->the, -navpacket->vd, climbf,
 193            navpacket->lat, navpacket->lon, gpspacket->alt, navpacket->alt,
 194            imupacket->Ps, Ps, Ps + Ps_error);
 195
 196     // cout << "climb rate = " << aero->hdota << endl;
 197     fdm->v_north = 0.0;
 198     fdm->v_east = 0.0;
 199     fdm->v_down = 0.0;
 200     fdm->v_wind_body_north = 0.0;
 201     fdm->v_wind_body_east = 0.0;
 202     fdm->v_wind_body_down = 0.0;
 203     fdm->stall_warning = 0.0;
 204
 205     fdm->A_X_pilot = 0.0;
 206     fdm->A_Y_pilot = 0.0;
 207     fdm->A_Z_pilot = 0.0 /* (should be -G) */;
 208
 209     // Engine parameters
 210     fdm->num_engines = 1;
 211     fdm->eng_state[0] = 2;
 212     // cout << "state = " << fdm->eng_state[0] << endl;
 213     double rpm = 5000.0 - ((double)servopacket->chn[2] / 65536.0)*3500.0;
 214     if ( rpm < 0.0 ) { rpm = 0.0; }
 215     if ( rpm > 5000.0 ) { rpm = 5000.0; }
 216     fdm->rpm[0] = rpm;
 217
 218     fdm->fuel_flow[0] = 0.0;
 219     fdm->egt[0] = 0.0;
 220     // cout << "egt = " << aero->EGT << endl;
 221     fdm->oil_temp[0] = 0.0;
 222     fdm->oil_px[0] = 0.0;
 223
 224     // Consumables
 225     fdm->num_tanks = 2;
 226     fdm->fuel_quantity[0] = 0.0;
 227     fdm->fuel_quantity[1] = 0.0;
 228
 229     // Gear and flaps
 230     fdm->num_wheels = 3;
 231     fdm->wow[0] = 0;
 232     fdm->wow[1] = 0;
 233     fdm->wow[2] = 0;
 234
 235     // the following really aren't used in this context
 236     fdm->cur_time = 0;
 237     fdm->warp = 0;
 238     fdm->visibility = 0;
 239
 240     // cout << "Flap deflection = " << aero->dflap << endl;
 241     fdm->left_flap = 0.0;
 242     fdm->right_flap = 0.0;
 243
 244     if ( est_controls ) {
 245         static float est_elev = 0.0;
 246         static float est_aileron = 0.0;
 247         static float est_rudder = 0.0;
 248         est_elev = 0.99 * est_elev + 0.01 * (imupacket->q * 4);
 249         est_aileron = 0.95 * est_aileron + 0.05 * (imupacket->p * 5);
 250         est_rudder = 0.95 * est_rudder + 0.05 * (imupacket->r * 2);
 251         ctrls->elevator = fdm->elevator = -est_elev;
 252         ctrls->aileron = fdm->left_aileron = est_aileron;
 253         fdm->right_aileron = -est_aileron;
 254         ctrls->rudder = fdm->rudder = est_rudder;
 255     } else {
 256         ctrls->elevator = fdm->elevator = 1.0 - ((double)servopacket->chn[1] / 32768.0);
 257         ctrls->aileron = fdm->left_aileron = 1.0 - ((double)servopacket->chn[0] / 32768.0);
 258         fdm->right_aileron = ((double)servopacket->chn[0] / 32768.0) - 1.0;
 259         ctrls->rudder = fdm->rudder = 1.0 - ((double)servopacket->chn[3] / 32768.0);
 260         ctrls->elevator *= 3.0;
 261         ctrls->aileron *= 3.0;
 262     }
 263     fdm->elevator_trim_tab = 0.0;
 264     fdm->left_flap = 0.0;
 265     fdm->right_flap = 0.0;
 266     fdm->nose_wheel = 0.0;
 267     fdm->speedbrake = 0.0;
 268     fdm->spoilers = 0.0;
 269
 270     // Convert the net buffer to network format
 271     fdm->version = htonl(fdm->version);
 272
 273     htond(fdm->longitude);
 274     htond(fdm->latitude);
 275     htond(fdm->altitude);
 276     htonf(fdm->agl);
 277     htonf(fdm->phi);
 278     htonf(fdm->theta);
 279     htonf(fdm->psi);
 280     htonf(fdm->alpha);
 281     htonf(fdm->beta);
 282
 283     htonf(fdm->phidot);
 284     htonf(fdm->thetadot);
 285     htonf(fdm->psidot);
 286     htonf(fdm->vcas);
 287     htonf(fdm->climb_rate);
 288     htonf(fdm->v_north);
 289     htonf(fdm->v_east);
 290     htonf(fdm->v_down);
 291     htonf(fdm->v_wind_body_north);
 292     htonf(fdm->v_wind_body_east);
 293     htonf(fdm->v_wind_body_down);
 294
 295     htonf(fdm->A_X_pilot);
 296     htonf(fdm->A_Y_pilot);
 297     htonf(fdm->A_Z_pilot);
 298
 299     htonf(fdm->stall_warning);
 300     htonf(fdm->slip_deg);
 301
 302     for ( i = 0; i < fdm->num_engines; ++i ) {
 303         fdm->eng_state[i] = htonl(fdm->eng_state[i]);
 304         htonf(fdm->rpm[i]);
 305         htonf(fdm->fuel_flow[i]);
 306         htonf(fdm->egt[i]);
 307         htonf(fdm->cht[i]);
 308         htonf(fdm->mp_osi[i]);
 309         htonf(fdm->tit[i]);
 310         htonf(fdm->oil_temp[i]);
 311         htonf(fdm->oil_px[i]);
 312     }
 313     fdm->num_engines = htonl(fdm->num_engines);
 314
 315     for ( i = 0; i < fdm->num_tanks; ++i ) {
 316         htonf(fdm->fuel_quantity[i]);
 317     }
 318     fdm->num_tanks = htonl(fdm->num_tanks);
 319
 320     for ( i = 0; i < fdm->num_wheels; ++i ) {
 321         fdm->wow[i] = htonl(fdm->wow[i]);
 322         htonf(fdm->gear_pos[i]);
 323         htonf(fdm->gear_steer[i]);
 324         htonf(fdm->gear_compression[i]);
 325     }
 326     fdm->num_wheels = htonl(fdm->num_wheels);
 327
 328     fdm->cur_time = htonl( fdm->cur_time );
 329     fdm->warp = htonl( fdm->warp );
 330     htonf(fdm->visibility);
 331
 332     htonf(fdm->elevator);
 333     htonf(fdm->elevator_trim_tab);
 334     htonf(fdm->left_flap);
 335     htonf(fdm->right_flap);
 336     htonf(fdm->left_aileron);
 337     htonf(fdm->right_aileron);
 338     htonf(fdm->rudder);
 339     htonf(fdm->nose_wheel);
 340     htonf(fdm->speedbrake);
 341     htonf(fdm->spoilers);
 342
 343 #if 0
 344     ctrls->version = FG_NET_CTRLS_VERSION;
 345     ctrls->elevator_trim = 0.0;
 346     ctrls->flaps = 0.0;
 347
 348     htonl(ctrls->version);
 349     htond(ctrls->aileron);
 350     htond(ctrls->rudder);
 351     htond(ctrls->elevator);
 352     htond(ctrls->elevator_trim);
 353     htond(ctrls->flaps);
 354 #endif
 355 }
 356
 357
 358 static void send_data( gps *gpspacket, imu *imupacket, nav *navpacket,
 359                        servo *servopacket, health *healthpacket ) {
 360     int len;
 361     int fdmsize = sizeof( FGNetFDM );
 362     int ctrlsize = sizeof( FGNetCtrls );
 363
 364     // cout << "Running main loop" << endl;
 365
 366     FGNetFDM fgfdm;
 367     FGNetCtrls fgctrls;
 368
 369     ugear2fg( gpspacket, imupacket, navpacket, servopacket, healthpacket,
 370               &fgfdm, &fgctrls );
 371     len = fdm_sock.send(&fgfdm, fdmsize, 0);
 372     // len = ctrls_sock.send(&fgctrls, ctrlsize, 0);
 373 }
 374
 375
 376 void usage( const string &argv0 ) {
 377     cout << "Usage: " << argv0 << endl;
 378     cout << "\t[ --help ]" << endl;
 379     cout << "\t[ --infile <infile_name>" << endl;
 380     cout << "\t[ --flight <flight_dir>" << endl;
 381     cout << "\t[ --serial <dev_name>" << endl;
 382     cout << "\t[ --outfile <outfile_name> (capture the data to a file)" << endl;
 383     cout << "\t[ --hertz <hertz> ]" << endl;
 384     cout << "\t[ --host <hostname> ]" << endl;
 385     cout << "\t[ --broadcast ]" << endl;
 386     cout << "\t[ --fdm-port <fdm output port #> ]" << endl;
 387     cout << "\t[ --ctrls-port <ctrls output port #> ]" << endl;
 388     cout << "\t[ --groundtrack-heading ]" << endl;
 389     cout << "\t[ --ground-speed ]" << endl;
 390     cout << "\t[ --estimate-control-deflections ]" << endl;
 391     cout << "\t[ --altitude-offset <meters> ]" << endl;
 392     cout << "\t[ --skip-seconds <seconds> ]" << endl;
 393     cout << "\t[ --no-real-time ]" << endl;
 394     cout << "\t[ --ignore-checksum ]" << endl;
 395     cout << "\t[ --sg-swap ]" << endl;
 396 }
 397
 398
 399 int main( int argc, char **argv ) {
 400     double hertz = 60.0;
 401     string out_host = "localhost";
 402     bool do_broadcast = false;
 403
 404     // process command line arguments
 405     for ( int i = 1; i < argc; ++i ) {
 406         if ( strcmp( argv[i], "--help" ) == 0 ) {
 407             usage( argv[0] );
 408             exit( 0 );
 409         } else if ( strcmp( argv[i], "--hertz" ) == 0 ) {
 410             ++i;
 411             if ( i < argc ) {
 412                 hertz = atof( argv[i] );
 413             } else {
 414                 usage( argv[0] );
 415                 exit( -1 );
 416             }
 417         } else if ( strcmp( argv[i], "--infile" ) == 0 ) {
 418             ++i;
 419             if ( i < argc ) {
 420                 infile = argv[i];
 421             } else {
 422                 usage( argv[0] );
 423                 exit( -1 );
 424             }
 425         } else if ( strcmp( argv[i], "--flight" ) == 0 ) {
 426             ++i;
 427             if ( i < argc ) {
 428                 flight_dir = argv[i];
 429             } else {
 430                 usage( argv[0] );
 431                 exit( -1 );
 432             }
 433         } else if ( strcmp( argv[i], "--outfile" ) == 0 ) {
 434             ++i;
 435             if ( i < argc ) {
 436                 outfile = argv[i];
 437             } else {
 438                 usage( argv[0] );
 439                 exit( -1 );
 440             }
 441         } else if ( strcmp( argv[i], "--serial" ) == 0 ) {
 442             ++i;
 443             if ( i < argc ) {
 444                 serialdev = argv[i];
 445             } else {
 446                 usage( argv[0] );
 447                 exit( -1 );
 448             }
 449         } else if ( strcmp( argv[i], "--host" ) == 0 ) {
 450             ++i;
 451             if ( i < argc ) {
 452                 out_host = argv[i];
 453             } else {
 454                 usage( argv[0] );
 455                 exit( -1 );
 456             }
 457         } else if ( strcmp( argv[i], "--broadcast" ) == 0 ) {
 458           do_broadcast = true;
 459         } else if ( strcmp( argv[i], "--fdm-port" ) == 0 ) {
 460             ++i;
 461             if ( i < argc ) {
 462                 fdm_port = atoi( argv[i] );
 463             } else {
 464                 usage( argv[0] );
 465                 exit( -1 );
 466             }
 467         } else if ( strcmp( argv[i], "--ctrls-port" ) == 0 ) {
 468             ++i;
 469             if ( i < argc ) {
 470                 ctrls_port = atoi( argv[i] );
 471             } else {
 472                 usage( argv[0] );
 473                 exit( -1 );
 474             }
 475         } else if ( strcmp (argv[i], "--groundtrack-heading" ) == 0 ) {
 476             use_ground_track_hdg = true;
 477         } else if ( strcmp (argv[i], "--ground-speed" ) == 0 ) {
 478             use_ground_speed = true;
 479         } else if (strcmp (argv[i], "--estimate-control-deflections" ) == 0) {
 480             est_controls = true;
 481         } else if ( strcmp( argv[i], "--altitude-offset" ) == 0 ) {
 482             ++i;
 483             if ( i < argc ) {
 484                 alt_offset = atof( argv[i] );
 485             } else {
 486                 usage( argv[0] );
 487                 exit( -1 );
 488             }
 489         } else if ( strcmp( argv[i], "--skip-seconds" ) == 0 ) {
 490             ++i;
 491             if ( i < argc ) {
 492                 skip = atof( argv[i] );
 493             } else {
 494                 usage( argv[0] );
 495                 exit( -1 );
 496             }
 497         } else if ( strcmp( argv[i], "--no-real-time" ) == 0 ) {
 498             run_real_time = false;
 499         } else if ( strcmp( argv[i], "--ignore-checksum" ) == 0 ) {
 500             ignore_checksum = true;
 501         } else if ( strcmp( argv[i], "--sg-swap" ) == 0 ) {
 502             sg_swap = true;
 503         } else {
 504             usage( argv[0] );
 505             exit( -1 );
 506         }
 507     }
 508
 509     // Setup up outgoing network connections
 510
 511     netInit( &argc,argv ); // We must call this before any other net stuff
 512
 513     if ( ! fdm_sock.open( false ) ) {  // open a UDP socket
 514         cout << "error opening fdm output socket" << endl;
 515         return -1;
 516     }
 517     if ( ! ctrls_sock.open( false ) ) {  // open a UDP socket
 518         cout << "error opening ctrls output socket" << endl;
 519         return -1;
 520     }
 521     cout << "open net channels" << endl;
 522
 523     fdm_sock.setBlocking( false );
 524     ctrls_sock.setBlocking( false );
 525     cout << "blocking false" << endl;
 526
 527     if ( do_broadcast ) {
 528         fdm_sock.setBroadcast( true );
 529         ctrls_sock.setBroadcast( true );
 530     }
 531
 532     if ( fdm_sock.connect( out_host.c_str(), fdm_port ) == -1 ) {
 533         perror("connect");
 534         cout << "error connecting to outgoing fdm port: " << out_host
 535              << ":" << fdm_port << endl;
 536         return -1;
 537     }
 538     cout << "connected outgoing fdm socket" << endl;
 539
 540     if ( ctrls_sock.connect( out_host.c_str(), ctrls_port ) == -1 ) {
 541         perror("connect");
 542         cout << "error connecting to outgoing ctrls port: " << out_host
 543              << ":" << ctrls_port << endl;
 544         return -1;
 545     }
 546     cout << "connected outgoing ctrls socket" << endl;
 547
 548     if ( sg_swap ) {
 549         track.set_stargate_swap_mode();
 550     }
 551
 552     if ( infile.length() || flight_dir.length() ) {
 553         if ( infile.length() ) {
 554             // Load data from a stream log data file
 555             track.load_stream( infile, ignore_checksum );
 556         } else if ( flight_dir.length() ) {
 557             // Load data from a flight directory
 558             track.load_flight( flight_dir );
 559         }
 560         cout << "Loaded " << track.gps_size() << " gps records." << endl;
 561         cout << "Loaded " << track.imu_size() << " imu records." << endl;
 562         cout << "Loaded " << track.nav_size() << " nav records." << endl;
 563         cout << "Loaded " << track.servo_size() << " servo records." << endl;
 564         cout << "Loaded " << track.health_size() << " health records." << endl;
 565
 566         int size = track.imu_size();
 567
 568         double current_time = track.get_imupt(0).time;
 569         cout << "Track begin time is " << current_time << endl;
 570         double end_time = track.get_imupt(size-1).time;
 571         cout << "Track end time is " << end_time << endl;
 572         cout << "Duration = " << end_time - current_time << endl;
 573
 574         if ( track.gps_size() > 0 ) {
 575             double tmp = track.get_gpspt(track.gps_size()-1).ITOW;
 576             int days = tmp / (24 * 60 * 60);
 577             tmp -= days * 24 * 60 * 60;
 578             int hours = tmp / (60 * 60);
 579             tmp -= hours * 60 * 60;
 580             int min = tmp / 60;
 581             tmp -= min * 60;
 582             double sec = tmp;
 583             printf("[GPS  ]:ITOW= %.3f[sec]  %dd %02d:%02d:%06.3f\n",
 584                    tmp, days, hours, min, sec);
 585         }
 586
 587
 588         // advance skip seconds forward
 589         current_time += skip;
 590
 591         frame_us = 1000000.0 / hertz;
 592         if ( frame_us < 0.0 ) {
 593             frame_us = 0.0;
 594         }
 595
 596         SGTimeStamp start_time;
 597         start_time.stamp();
 598         int gps_count = 0;
 599         int imu_count = 0;
 600         int nav_count = 0;
 601         int servo_count = 0;
 602         int health_count = 0;
 603
 604         gps gps0, gps1;
 605         gps0 = gps1 = track.get_gpspt( 0 );
 606
 607         imu imu0, imu1;
 608         imu0 = imu1 = track.get_imupt( 0 );
 609
 610         nav nav0, nav1;
 611         nav0 = nav1 = track.get_navpt( 0 );
 612
 613         servo servo0, servo1;
 614         servo0 = servo1 = track.get_servopt( 0 );
 615
 616         health health0, health1;
 617         health0 = health1 = track.get_healthpt( 0 );
 618
 619         double last_lat = -999.9, last_lon = -999.9;
 620
 621         printf("<gpx>\n");
 622         printf(" <trk>\n");
 623         printf("  <trkseg>\n");
 624         while ( current_time < end_time ) {
 625             // cout << "current_time = " << current_time << " end_time = "
 626             //      << end_time << endl;
 627
 628             // Advance gps pointer
 629             while ( current_time > gps1.time
 630                     && gps_count < track.gps_size() - 1 )
 631             {
 632                 gps0 = gps1;
 633                 ++gps_count;
 634                 // cout << "count = " << count << endl;
 635                 gps1 = track.get_gpspt( gps_count );
 636             }
 637             // cout << "p0 = " << p0.get_time() << " p1 = " << p1.get_time()
 638             //      << endl;
 639
 640             // Advance imu pointer
 641             while ( current_time > imu1.time
 642                     && imu_count < track.imu_size() - 1 )
 643             {
 644                 imu0 = imu1;
 645                 ++imu_count;
 646                 // cout << "count = " << count << endl;
 647                 imu1 = track.get_imupt( imu_count );
 648             }
 649             //  cout << "pos0 = " << pos0.get_seconds()
 650             // << " pos1 = " << pos1.get_seconds() << endl;
 651
 652             // Advance nav pointer
 653             while ( current_time > nav1.time
 654                     && nav_count < track.nav_size() - 1 )
 655             {
 656                 nav0 = nav1;
 657                 ++nav_count;
 658                 // cout << "nav count = " << nav_count << endl;
 659                 nav1 = track.get_navpt( nav_count );
 660             }
 661             //  cout << "pos0 = " << pos0.get_seconds()
 662             // << " pos1 = " << pos1.get_seconds() << endl;
 663
 664             // Advance servo pointer
 665             while ( current_time > servo1.time
 666                     && servo_count < track.servo_size() - 1 )
 667             {
 668                 servo0 = servo1;
 669                 ++servo_count;
 670                 // cout << "count = " << count << endl;
 671                 servo1 = track.get_servopt( servo_count );
 672             }
 673             //  cout << "pos0 = " << pos0.get_seconds()
 674             // << " pos1 = " << pos1.get_seconds() << endl;
 675
 676             // Advance health pointer
 677             while ( current_time > health1.time
 678                     && health_count < track.health_size() - 1 )
 679             {
 680                 health0 = health1;
 681                 ++health_count;
 682                 // cout << "count = " << count << endl;
 683                 health1 = track.get_healthpt( health_count );
 684             }
 685             //  cout << "pos0 = " << pos0.get_seconds()
 686             // << " pos1 = " << pos1.get_seconds() << endl;
 687
 688             double gps_percent;
 689             if ( fabs(gps1.time - gps0.time) < 0.00001 ) {
 690                 gps_percent = 0.0;
 691             } else {
 692                 gps_percent =
 693                     (current_time - gps0.time) /
 694                     (gps1.time - gps0.time);
 695             }
 696             // cout << "Percent = " << percent << endl;
 697
 698             double imu_percent;
 699             if ( fabs(imu1.time - imu0.time) < 0.00001 ) {
 700                 imu_percent = 0.0;
 701             } else {
 702                 imu_percent =
 703                     (current_time - imu0.time) /
 704                     (imu1.time - imu0.time);
 705             }
 706             // cout << "Percent = " << percent << endl;
 707
 708             double nav_percent;
 709             if ( fabs(nav1.time - nav0.time) < 0.00001 ) {
 710                 nav_percent = 0.0;
 711             } else {
 712                 nav_percent =
 713                     (current_time - nav0.time) /
 714                     (nav1.time - nav0.time);
 715             }
 716             // cout << "Percent = " << percent << endl;
 717
 718             double servo_percent;
 719             if ( fabs(servo1.time - servo0.time) < 0.00001 ) {
 720                 servo_percent = 0.0;
 721             } else {
 722                 servo_percent =
 723                     (current_time - servo0.time) /
 724                     (servo1.time - servo0.time);
 725             }
 726             // cout << "Percent = " << percent << endl;
 727
 728             double health_percent;
 729             if ( fabs(health1.time - health0.time) < 0.00001 ) {
 730                 health_percent = 0.0;
 731             } else {
 732                 health_percent =
 733                     (current_time - health0.time) /
 734                     (health1.time - health0.time);
 735             }
 736             // cout << "Percent = " << percent << endl;
 737
 738             gps gpspacket = UGEARInterpGPS( gps0, gps1, gps_percent );
 739             imu imupacket = UGEARInterpIMU( imu0, imu1, imu_percent );
 740             nav navpacket = UGEARInterpNAV( nav0, nav1, nav_percent );
 741             servo servopacket = UGEARInterpSERVO( servo0, servo1,
 742                                                   servo_percent );
 743             health healthpacket = UGEARInterpHEALTH( health0, health1,
 744                                                   health_percent );
 745
 746             // cout << current_time << " " << p0.lat_deg << ", " << p0.lon_deg
 747             //      << endl;
 748             // cout << current_time << " " << p1.lat_deg << ", " << p1.lon_deg
 749             //      << endl;
 750             // cout << (double)current_time << " " << pos.lat_deg << ", "
 751             //      << pos.lon_deg << " " << att.yaw_deg << endl;
 752             if ( gpspacket.lat > -500 ) {
 753                 // printf( "%.3f  %.4f %.4f %.1f  %.2f %.2f %.2f\n",
 754                 //         current_time,
 755                 //         navpacket.lat, navpacket.lon, navpacket.alt,
 756                 //         imupacket.psi, imupacket.the, imupacket.phi );
 757                 double dlat = last_lat - navpacket.lat;
 758                 double dlon = last_lon - navpacket.lon;
 759                 double dist = sqrt( dlat*dlat + dlon*dlon );
 760                 if ( dist > 0.01 ) {
 761                     printf("   <trkpt lat=\"%.8f\" lon=\"%.8f\"></trkpt>\n",
 762                            navpacket.lat, navpacket.lon );
 763                     // printf(" </wpt>\n");
 764                     last_lat = navpacket.lat;
 765                     last_lon = navpacket.lon;
 766                 }
 767             }
 768
 769             send_data( &gpspacket, &imupacket, &navpacket, &servopacket,
 770                        &healthpacket );
 771
 772             if ( run_real_time ) {
 773                 // Update the elapsed time.
 774                 static bool first_time = true;
 775                 if ( first_time ) {
 776                     last_time_stamp.stamp();
 777                     first_time = false;
 778                 }
 779
 780                 current_time_stamp.stamp();
 781                 /* Convert to ms */
 782                 double elapsed_us = current_time_stamp - last_time_stamp;
 783                 if ( elapsed_us < (frame_us - 2000) ) {
 784                     double requested_us = (frame_us - elapsed_us) - 2000 ;
 785                     ulMilliSecondSleep ( (int)(requested_us / 1000.0) ) ;
 786                 }
 787                 current_time_stamp.stamp();
 788                 while ( current_time_stamp - last_time_stamp < frame_us ) {
 789                     current_time_stamp.stamp();
 790                 }
 791             }
 792
 793             current_time += (frame_us / 1000000.0);
 794             last_time_stamp = current_time_stamp;
 795         }
 796
 797         printf("   <trkpt lat=\"%.8f\" lon=\"%.8f\"></trkpt>\n",
 798                nav1.lat, nav1.lon );
 799
 800         printf("  </trkseg>\n");
 801         printf(" </trk>\n");
 802         nav0 = track.get_navpt( 0 );
 803         nav1 = track.get_navpt( track.nav_size() - 1 );
 804         printf(" <wpt lat=\"%.8f\" lon=\"%.8f\"></wpt>\n",
 805                nav0.lat, nav0.lon );
 806         printf(" <wpt lat=\"%.8f\" lon=\"%.8f\"></wpt>\n",
 807                nav1.lat, nav1.lon );
 808         printf("<gpx>\n");
 809
 810         cout << "Processed " << imu_count << " entries in "
 811              << (current_time_stamp - start_time) / 1000000 << " seconds."
 812              << endl;
 813     } else if ( serialdev.length() ) {
 814         // process incoming data from the serial port
 815
 816         int count = 0;
 817         double current_time = 0.0;
 818         double last_time = 0.0;
 819
 820         gps gpspacket; bzero( &gpspacket, sizeof(gpspacket) );
 821         imu imupacket; bzero( &imupacket, sizeof(imupacket) );
 822         nav navpacket; bzero( &navpacket, sizeof(navpacket) );
 823         servo servopacket; bzero( &servopacket, sizeof(servopacket) );
 824         health healthpacket; bzero( &healthpacket, sizeof(healthpacket) );
 825
 826         double gps_time = 0;
 827         double imu_time = 0;
 828         double nav_time = 0;
 829         double servo_time = 0;
 830         double health_time = 0;
 831
 832         // open the serial port device
 833         SGSerialPort input( serialdev, 115200 );
 834         if ( !input.is_enabled() ) {
 835             cout << "Cannot open: " << serialdev << endl;
 836             return false;
 837         }
 838
 839         // open up the data log file if requested
 840         if ( !outfile.length() ) {
 841             cout << "no --outfile <name> specified, cannot capture data!"
 842                  << endl;
 843             return false;
 844         }
 845         SGFile output( outfile );
 846         if ( !output.open( SG_IO_OUT ) ) {
 847             cout << "Cannot open: " << outfile << endl;
 848             return false;
 849         }
 850
 851         while ( input.is_enabled() ) {
 852             // cout << "looking for next message ..." << endl;
 853             int id = track.next_message( &input, &output, &gpspacket,
 854                                          &imupacket, &navpacket, &servopacket,
 855                                          &healthpacket, ignore_checksum );
 856             // cout << "message id = " << id << endl;
 857             count++;
 858
 859             if ( id == GPS_PACKET ) {
 860                 if ( gpspacket.time > gps_time ) {
 861                     gps_time = gpspacket.time;
 862                     current_time = gps_time;
 863                 } else {
 864                   cout << "oops gps back in time: " << gpspacket.time << " " << gps_time << endl;
 865                 }
 866             } else if ( id == IMU_PACKET ) {
 867                 if ( imupacket.time > imu_time ) {
 868                     imu_time = imupacket.time;
 869                     current_time = imu_time;
 870                 } else {
 871                     cout << "oops imu back in time: " << imupacket.time << " " << imu_time << endl;
 872                 }
 873             } else if ( id == NAV_PACKET ) {
 874                 if ( navpacket.time > nav_time ) {
 875                     nav_time = navpacket.time;
 876                     current_time = nav_time;
 877                 } else {
 878                     cout << "oops nav back in time: " << navpacket.time << " " << nav_time << endl;
 879                 }
 880             } else if ( id == SERVO_PACKET ) {
 881                 if ( servopacket.time > servo_time ) {
 882                     servo_time = servopacket.time;
 883                     current_time = servo_time;
 884                 } else {
 885                     cout << "oops servo back in time: " << servopacket.time << " " << servo_time << endl;
 886                 }
 887             } else if ( id == HEALTH_PACKET ) {
 888                 if ( healthpacket.time > health_time ) {
 889                     health_time = healthpacket.time;
 890                     current_time = health_time;
 891                 } else {
 892                     cout << "oops health back in time: " << healthpacket.time << " " << health_time << endl;
 893                 }
 894             }
 895
 896             if ( current_time >= last_time + (1/hertz) ) {
 897                 // if ( gpspacket.lat > -500 ) {
 898                 printf( "%.2f  %.6f %.6f %.1f  %.2f %.2f %.2f  %.2f %d\n",
 899                         current_time,
 900                         navpacket.lat, navpacket.lon, navpacket.alt,
 901                         imupacket.phi*SGD_RADIANS_TO_DEGREES,
 902                         imupacket.the*SGD_RADIANS_TO_DEGREES,
 903                         imupacket.psi*SGD_RADIANS_TO_DEGREES,
 904                         healthpacket.volts,
 905                         healthpacket.est_seconds);
 906                 // }
 907
 908                 last_time = current_time;
 909                 send_data( &gpspacket, &imupacket, &navpacket, &servopacket,
 910                            &healthpacket );
 911             }
 912         }
 913     }
 914
 915     return 0;
 916 }