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