utils/GPSsmooth/UGear_main.cxx

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