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 #include "UGear_command.hxx"
  28 #include "UGear_opengc.hxx"
  29
  30
  31 SG_USING_STD(cout);
  32 SG_USING_STD(endl);
  33 SG_USING_STD(string);
  34
  35
  36 // Network channels
  37 static netSocket fdm_sock, ctrls_sock, opengc_sock;
  38
  39 // ugear data
  40 UGTrack track;
  41
  42 // Default ports
  43 static int fdm_port = 5505;
  44 static int ctrls_port = 5506;
  45 static int opengc_port = 6000;
  46
  47 // Default path
  48 static string infile = "";
  49 static string flight_dir = "";
  50 static string serialdev = "";
  51 static string outfile = "";
  52
  53 // Master time counter
  54 float sim_time = 0.0f;
  55 double frame_us = 0.0f;
  56
  57 // sim control
  58 SGTimeStamp last_time_stamp;
  59 SGTimeStamp current_time_stamp;
  60
  61 // altitude offset
  62 double alt_offset = 0.0;
  63
  64 // skip initial seconds
  65 double skip = 0.0;
  66
  67 // for speed estimate
  68 double last_lat = 0.0, last_lon = 0.0;
  69 double kts_filter = 0.0;
  70
  71 bool inited = false;
  72
  73 bool run_real_time = true;
  74
  75 bool ignore_checksum = false;
  76
  77 bool sg_swap = false;
  78
  79 bool use_ground_track_hdg = false;
  80 bool use_ground_speed = false;
  81
  82 bool est_controls = false;
  83
  84 float gps_status = -1.0;
  85
  86
  87 // The function htond is defined this way due to the way some
  88 // processors and OSes treat floating point values.  Some will raise
  89 // an exception whenever a "bad" floating point value is loaded into a
  90 // floating point register.  Solaris is notorious for this, but then
  91 // so is LynxOS on the PowerPC.  By translating the data in place,
  92 // there is no need to load a FP register with the "corruped" floating
  93 // point value.  By doing the BIG_ENDIAN test, I can optimize the
  94 // routine for big-endian processors so it can be as efficient as
  95 // possible
  96 static void htond (double &x)
  97 {
  98     if ( sgIsLittleEndian() ) {
  99         int    *Double_Overlay;
 100         int     Holding_Buffer;
 101
 102         Double_Overlay = (int *) &x;
 103         Holding_Buffer = Double_Overlay [0];
 104
 105         Double_Overlay [0] = htonl (Double_Overlay [1]);
 106         Double_Overlay [1] = htonl (Holding_Buffer);
 107     } else {
 108         return;
 109     }
 110 }
 111
 112 // Float version
 113 static void htonf (float &x)
 114 {
 115     if ( sgIsLittleEndian() ) {
 116         int    *Float_Overlay;
 117         int     Holding_Buffer;
 118
 119         Float_Overlay = (int *) &x;
 120         Holding_Buffer = Float_Overlay [0];
 121
 122         Float_Overlay [0] = htonl (Holding_Buffer);
 123     } else {
 124         return;
 125     }
 126 }
 127
 128
 129 static void ugear2fg( gps *gpspacket, imu *imupacket, nav *navpacket,
 130                       servo *servopacket, health *healthpacket,
 131                       FGNetFDM *fdm, FGNetCtrls *ctrls )
 132 {
 133     unsigned int i;
 134
 135     // Version sanity checking
 136     fdm->version = FG_NET_FDM_VERSION;
 137
 138     // Aero parameters
 139     fdm->longitude = navpacket->lon * SG_DEGREES_TO_RADIANS;
 140     fdm->latitude = navpacket->lat * SG_DEGREES_TO_RADIANS;
 141     fdm->altitude = navpacket->alt + alt_offset;
 142     fdm->agl = -9999.0;
 143     fdm->psi = imupacket->psi; // heading
 144     fdm->phi = imupacket->phi; // roll
 145     fdm->theta = imupacket->the; // pitch;
 146
 147     fdm->phidot = 0.0;
 148     fdm->thetadot = 0.0;
 149     fdm->psidot = 0.0;
 150
 151     // estimate speed
 152     // double az1, az2, dist;
 153     // geo_inverse_wgs_84( fdm->altitude, last_lat, last_lon,
 154     //                     fdm->latitude, fdm->longitude, &az1, &az2, &dist );
 155     // last_lat = fdm->latitude;
 156     // last_lon = fdm->longitude;
 157     // double v_ms = dist / (frame_us / 1000000);
 158     // double v_kts = v_ms * SG_METER_TO_NM * 3600;
 159     // kts_filter = (0.9 * kts_filter) + (0.1 * v_kts);
 160     // printf("dist = %.5f  kts est = %.2f\n", dist, kts_filter);
 161
 162     double vn = navpacket->vn;
 163     double ve = navpacket->ve;
 164     double vd = navpacket->vd;
 165
 166     if ( use_ground_track_hdg ) {
 167         fdm->psi = SGD_PI * 0.5 - atan2(vn, ve); // heading
 168     }
 169
 170     double mps = 0.0;
 171     if ( use_ground_speed ) {
 172         mps = sqrt( vn*vn + ve*ve + vd*vd );
 173     } else {
 174         mps = imupacket->Pt;
 175     }
 176     // double mph = mps * 3600 / 1609.3440;
 177     double kts = mps * SG_MPS_TO_KT;
 178     fdm->vcas = kts;
 179     // printf("speed = %.2f mph  %.2f kts\n", mph, kts );
 180     static double Ps = 0.0, Ps_last = 0.0, t_last = 0.0;
 181     Ps_last = Ps;
 182     Ps = 0.92 * Ps + 0.08 * imupacket->Ps;
 183     double climb = (Ps - Ps_last) / (imupacket->time - t_last);
 184     t_last = imupacket->time;
 185     static double climbf = 0.0;
 186     climbf = 0.994 * climbf + 0.006 * climb;
 187     fdm->climb_rate = climbf; // fps
 188
 189     static double Ps_error = 0.0;
 190     static double Ps_count = 0;
 191     const double span = 10000.0;
 192     Ps_count += 1.0; if (Ps_count > (span-1.0)) { Ps_count = (span-1.0); }
 193     double error = navpacket->alt - Ps;
 194     Ps_error = (Ps_count/span) * Ps_error + ((span-Ps_count)/span) * error;
 195     fdm->altitude = Ps +  Ps_error;
 196
 197     /* printf("%.3f, %.3f, %.3f, %.3f, %.8f, %.8f, %.3f, %.3f, %.3f, %.3f, %.3f\n",
 198            imupacket->time, imupacket->the, -navpacket->vd, climbf,
 199            navpacket->lat, navpacket->lon, gpspacket->alt, navpacket->alt,
 200            imupacket->Ps, Ps, Ps + Ps_error); */
 201
 202     // cout << "climb rate = " << aero->hdota << endl;
 203     fdm->v_north = 0.0;
 204     fdm->v_east = 0.0;
 205     fdm->v_down = 0.0;
 206     fdm->v_wind_body_north = 0.0;
 207     fdm->v_wind_body_east = 0.0;
 208     fdm->v_wind_body_down = 0.0;
 209     fdm->stall_warning = 0.0;
 210
 211     fdm->A_X_pilot = 0.0;
 212     fdm->A_Y_pilot = 0.0;
 213     fdm->A_Z_pilot = 0.0 /* (should be -G) */;
 214
 215     // Engine parameters
 216     fdm->num_engines = 1;
 217     fdm->eng_state[0] = 2;
 218     // cout << "state = " << fdm->eng_state[0] << endl;
 219     double rpm = 5000.0 - ((double)servopacket->chn[2] / 65536.0)*3500.0;
 220     if ( rpm < 0.0 ) { rpm = 0.0; }
 221     if ( rpm > 5000.0 ) { rpm = 5000.0; }
 222     fdm->rpm[0] = rpm;
 223
 224     fdm->fuel_flow[0] = 0.0;
 225     fdm->egt[0] = 0.0;
 226     // cout << "egt = " << aero->EGT << endl;
 227     fdm->oil_temp[0] = 0.0;
 228     fdm->oil_px[0] = 0.0;
 229
 230     // Consumables
 231     fdm->num_tanks = 2;
 232     fdm->fuel_quantity[0] = 0.0;
 233     fdm->fuel_quantity[1] = 0.0;
 234
 235     // Gear and flaps
 236     fdm->num_wheels = 3;
 237     fdm->wow[0] = 0;
 238     fdm->wow[1] = 0;
 239     fdm->wow[2] = 0;
 240
 241     // the following really aren't used in this context
 242     fdm->cur_time = 0;
 243     fdm->warp = 0;
 244     fdm->visibility = 0;
 245
 246     // cout << "Flap deflection = " << aero->dflap << endl;
 247     fdm->left_flap = 0.0;
 248     fdm->right_flap = 0.0;
 249
 250     if ( est_controls ) {
 251         static float est_elev = 0.0;
 252         static float est_aileron = 0.0;
 253         static float est_rudder = 0.0;
 254         est_elev = 0.99 * est_elev + 0.01 * (imupacket->q * 4);
 255         est_aileron = 0.95 * est_aileron + 0.05 * (imupacket->p * 5);
 256         est_rudder = 0.95 * est_rudder + 0.05 * (imupacket->r * 2);
 257         ctrls->elevator = fdm->elevator = -est_elev;
 258         ctrls->aileron = fdm->left_aileron = est_aileron;
 259         fdm->right_aileron = -est_aileron;
 260         ctrls->rudder = fdm->rudder = est_rudder;
 261     } else {
 262         ctrls->elevator = fdm->elevator = 1.0 - ((double)servopacket->chn[1] / 32768.0);
 263         ctrls->aileron = fdm->left_aileron = 1.0 - ((double)servopacket->chn[0] / 32768.0);
 264         fdm->right_aileron = ((double)servopacket->chn[0] / 32768.0) - 1.0;
 265         ctrls->rudder = fdm->rudder = 1.0 - ((double)servopacket->chn[3] / 32768.0);
 266         ctrls->elevator *= 3.0;
 267         ctrls->aileron *= 3.0;
 268     }
 269     fdm->elevator_trim_tab = 0.0;
 270     fdm->left_flap = 0.0;
 271     fdm->right_flap = 0.0;
 272     fdm->nose_wheel = 0.0;
 273     fdm->speedbrake = 0.0;
 274     fdm->spoilers = 0.0;
 275
 276     // Convert the net buffer to network format
 277     fdm->version = htonl(fdm->version);
 278
 279     htond(fdm->longitude);
 280     htond(fdm->latitude);
 281     htond(fdm->altitude);
 282     htonf(fdm->agl);
 283     htonf(fdm->phi);
 284     htonf(fdm->theta);
 285     htonf(fdm->psi);
 286     htonf(fdm->alpha);
 287     htonf(fdm->beta);
 288
 289     htonf(fdm->phidot);
 290     htonf(fdm->thetadot);
 291     htonf(fdm->psidot);
 292     htonf(fdm->vcas);
 293     htonf(fdm->climb_rate);
 294     htonf(fdm->v_north);
 295     htonf(fdm->v_east);
 296     htonf(fdm->v_down);
 297     htonf(fdm->v_wind_body_north);
 298     htonf(fdm->v_wind_body_east);
 299     htonf(fdm->v_wind_body_down);
 300
 301     htonf(fdm->A_X_pilot);
 302     htonf(fdm->A_Y_pilot);
 303     htonf(fdm->A_Z_pilot);
 304
 305     htonf(fdm->stall_warning);
 306     htonf(fdm->slip_deg);
 307
 308     for ( i = 0; i < fdm->num_engines; ++i ) {
 309         fdm->eng_state[i] = htonl(fdm->eng_state[i]);
 310         htonf(fdm->rpm[i]);
 311         htonf(fdm->fuel_flow[i]);
 312         htonf(fdm->egt[i]);
 313         htonf(fdm->cht[i]);
 314         htonf(fdm->mp_osi[i]);
 315         htonf(fdm->tit[i]);
 316         htonf(fdm->oil_temp[i]);
 317         htonf(fdm->oil_px[i]);
 318     }
 319     fdm->num_engines = htonl(fdm->num_engines);
 320
 321     for ( i = 0; i < fdm->num_tanks; ++i ) {
 322         htonf(fdm->fuel_quantity[i]);
 323     }
 324     fdm->num_tanks = htonl(fdm->num_tanks);
 325
 326     for ( i = 0; i < fdm->num_wheels; ++i ) {
 327         fdm->wow[i] = htonl(fdm->wow[i]);
 328         htonf(fdm->gear_pos[i]);
 329         htonf(fdm->gear_steer[i]);
 330         htonf(fdm->gear_compression[i]);
 331     }
 332     fdm->num_wheels = htonl(fdm->num_wheels);
 333
 334     fdm->cur_time = htonl( fdm->cur_time );
 335     fdm->warp = htonl( fdm->warp );
 336     htonf(fdm->visibility);
 337
 338     htonf(fdm->elevator);
 339     htonf(fdm->elevator_trim_tab);
 340     htonf(fdm->left_flap);
 341     htonf(fdm->right_flap);
 342     htonf(fdm->left_aileron);
 343     htonf(fdm->right_aileron);
 344     htonf(fdm->rudder);
 345     htonf(fdm->nose_wheel);
 346     htonf(fdm->speedbrake);
 347     htonf(fdm->spoilers);
 348
 349 #if 0
 350     ctrls->version = FG_NET_CTRLS_VERSION;
 351     ctrls->elevator_trim = 0.0;
 352     ctrls->flaps = 0.0;
 353
 354     htonl(ctrls->version);
 355     htond(ctrls->aileron);
 356     htond(ctrls->rudder);
 357     htond(ctrls->elevator);
 358     htond(ctrls->elevator_trim);
 359     htond(ctrls->flaps);
 360 #endif
 361 }
 362
 363
 364 static void ugear2opengc( gps *gpspacket, imu *imupacket, nav *navpacket,
 365                           servo *servopacket, health *healthpacket,
 366                           ogcFGData *ogc )
 367 {
 368     // Version sanity checking
 369     ogc->version_id = OGC_VERSION;
 370
 371     // Aero parameters
 372     ogc->longitude = navpacket->lon;
 373     ogc->latitude = navpacket->lat;
 374     ogc->heading = imupacket->psi * SG_RADIANS_TO_DEGREES; // heading
 375     ogc->bank = imupacket->phi * SG_RADIANS_TO_DEGREES; // roll
 376     ogc->pitch = imupacket->the * SG_RADIANS_TO_DEGREES; // pitch;
 377
 378     ogc->phi_dot = 0.0;
 379     ogc->theta_dot = 0.0;
 380     ogc->psi_dot = 0.0;
 381
 382     ogc->alpha = 0.0;
 383     ogc->beta = 0.0;
 384     ogc->alpha_dot = 0.0;
 385     ogc->beta_dot = 0.0;
 386
 387     ogc->left_aileron = 1.0 - ((double)servopacket->chn[0] / 32768.0);
 388     ogc->right_aileron = ((double)servopacket->chn[0] / 32768.0) - 1.0;
 389     ogc->elevator = 1.0 - ((double)servopacket->chn[1] / 32768.0);
 390     ogc->elevator_trim = 0.0;
 391     ogc->rudder = 1.0 - ((double)servopacket->chn[3] / 32768.0);
 392     ogc->flaps = 0.0;
 393     ogc->flaps_cmd = 0.0;
 394
 395     ogc->wind = 0.0;
 396     ogc->wind_dir = 0.0;
 397
 398     // estimate speed
 399     // double az1, az2, dist;
 400     // geo_inverse_wgs_84( fdm->altitude, last_lat, last_lon,
 401     //                     fdm->latitude, fdm->longitude, &az1, &az2, &dist );
 402     // last_lat = fdm->latitude;
 403     // last_lon = fdm->longitude;
 404     // double v_ms = dist / (frame_us / 1000000);
 405     // double v_kts = v_ms * SG_METER_TO_NM * 3600;
 406     // kts_filter = (0.9 * kts_filter) + (0.1 * v_kts);
 407     // printf("dist = %.5f  kts est = %.2f\n", dist, kts_filter);
 408
 409     double vn = navpacket->vn;
 410     double ve = navpacket->ve;
 411     double vd = navpacket->vd;
 412
 413     if ( use_ground_track_hdg ) {
 414         ogc->heading = (SGD_PI * 0.5 - atan2(vn, ve)) * SG_RADIANS_TO_DEGREES;
 415     }
 416
 417     if ( ogc->heading < 0 ) { ogc->heading += 360.0; }
 418
 419     double mps = 0.0;
 420     if ( use_ground_speed ) {
 421         mps = sqrt( vn*vn + ve*ve + vd*vd );
 422     } else {
 423         mps = imupacket->Pt;
 424     }
 425     // double mph = mps * 3600 / 1609.3440;
 426     double kts = mps * SG_MPS_TO_KT;
 427     ogc->v_kcas = kts;
 428     // printf("speed = %.2f mph  %.2f kts\n", mph, kts );
 429     static double Ps = 0.0, Ps_last = 0.0, t_last = 0.0;
 430     Ps_last = Ps;
 431     Ps = 0.92 * Ps + 0.08 * imupacket->Ps;
 432     double climb = (Ps - Ps_last) / (imupacket->time - t_last);
 433     t_last = imupacket->time;
 434     static double climbf = 0.0;
 435     climbf = 0.994 * climbf + 0.006 * climb;
 436     ogc->vvi = climbf; // fps
 437
 438     // uncomment one of the following schemes for setting elevation:
 439
 440     // use the navigation (inertially augmented gps estimate)
 441     // ogc->altitude = ogc->elevation
 442     //     = (navpacket->alt + alt_offset * SG_METER_TO_FEET);
 443
 444     // use estimate error between pressure sensor and gps altitude over time
 445     // use pressure sensor + error average for altitude estimate.
 446     static double Ps_error = 0.0;
 447     static double Ps_count = 0;
 448     const double span = 10000.0;
 449     Ps_count += 1.0; if (Ps_count > (span-1.0)) { Ps_count = (span-1.0); }
 450     double error = navpacket->alt - Ps;
 451     Ps_error = (Ps_count/span) * Ps_error + ((span-Ps_count)/span) * error;
 452     ogc->elevation = (Ps +  Ps_error) * SG_METER_TO_FEET;
 453
 454     /* printf("%.3f, %.3f, %.3f, %.3f, %.8f, %.8f, %.3f, %.3f, %.3f, %.3f, %.3f\n",
 455            imupacket->time, imupacket->the, -navpacket->vd, climbf,
 456            navpacket->lat, navpacket->lon, gpspacket->alt, navpacket->alt,
 457            imupacket->Ps, Ps, Ps + Ps_error); */
 458
 459     if ( est_controls ) {
 460         static float est_elev = 0.0;
 461         static float est_aileron = 0.0;
 462         static float est_rudder = 0.0;
 463         est_elev = 0.99 * est_elev + 0.01 * (imupacket->q * 4);
 464         est_aileron = 0.95 * est_aileron + 0.05 * (imupacket->p * 5);
 465         est_rudder = 0.95 * est_rudder + 0.05 * (imupacket->r * 2);
 466         ogc->elevator = -est_elev;
 467         ogc->left_aileron = est_aileron;
 468         ogc->right_aileron = -est_aileron;
 469         ogc->rudder = est_rudder;
 470     } else {
 471         ogc->elevator = 1.0 - ((double)servopacket->chn[1] / 32768.0);
 472         ogc->left_aileron = 1.0 - ((double)servopacket->chn[0] / 32768.0);
 473         ogc->right_aileron = ((double)servopacket->chn[0] / 32768.0) - 1.0;
 474         ogc->rudder = 1.0 - ((double)servopacket->chn[3] / 32768.0);
 475     }
 476     ogc->elevator *= 4.0;
 477     ogc->left_aileron *= 4.0;
 478     ogc->right_aileron *= 4.0;
 479     ogc->rudder *= 4.0;
 480
 481     // additional "abused" data fields
 482     ogc->egt[0] = ogc->bank - healthpacket->target_roll_deg; // flight director target roll
 483     ogc->egt[1] = -ogc->pitch + healthpacket->target_pitch_deg; // flight director target pitch
 484     ogc->egt[2] = healthpacket->target_heading_deg; // target heading bug
 485     ogc->egt[3] = healthpacket->target_climb_fps;   // target VVI bug
 486     ogc->epr[0] = healthpacket->target_altitude_ft; // target altitude bug
 487     ogc->epr[1] = 30.0;                              // target speed bug
 488     ogc->epr[2] = gps_status;   // gps status box
 489 }
 490
 491
 492 static void send_data_udp( gps *gpspacket, imu *imupacket, nav *navpacket,
 493                            servo *servopacket, health *healthpacket )
 494 {
 495     int len;
 496     int ogcsize = sizeof( ogcFGData );
 497     int fdmsize = sizeof( FGNetFDM );
 498     // int ctrlsize = sizeof( FGNetCtrls );
 499
 500     // cout << "Running main loop" << endl;
 501
 502     ogcFGData fgogc;
 503     FGNetFDM fgfdm;
 504     FGNetCtrls fgctrls;
 505
 506     ugear2fg( gpspacket, imupacket, navpacket, servopacket, healthpacket,
 507               &fgfdm, &fgctrls );
 508     ugear2opengc( gpspacket, imupacket, navpacket, servopacket, healthpacket,
 509                   &fgogc );
 510     len = opengc_sock.send(&fgogc, ogcsize, 0);
 511     len = fdm_sock.send(&fgfdm, fdmsize, 0);
 512     // len = ctrls_sock.send(&fgctrls, ctrlsize, 0);
 513 }
 514
 515
 516 void usage( const string &argv0 ) {
 517     cout << "Usage: " << argv0 << endl;
 518     cout << "\t[ --help ]" << endl;
 519     cout << "\t[ --infile <infile_name>" << endl;
 520     cout << "\t[ --flight <flight_dir>" << endl;
 521     cout << "\t[ --serial <dev_name>" << endl;
 522     cout << "\t[ --outfile <outfile_name> (capture the data to a file)" << endl;
 523     cout << "\t[ --hertz <hertz> ]" << endl;
 524     cout << "\t[ --host <hostname> ]" << endl;
 525     cout << "\t[ --broadcast ]" << endl;
 526     cout << "\t[ --opengc-port <opengc output port #> ]" << endl;
 527     cout << "\t[ --fdm-port <fdm output port #> ]" << endl;
 528     cout << "\t[ --ctrls-port <ctrls output port #> ]" << endl;
 529     cout << "\t[ --groundtrack-heading ]" << endl;
 530     cout << "\t[ --ground-speed ]" << endl;
 531     cout << "\t[ --estimate-control-deflections ]" << endl;
 532     cout << "\t[ --altitude-offset <meters> ]" << endl;
 533     cout << "\t[ --skip-seconds <seconds> ]" << endl;
 534     cout << "\t[ --no-real-time ]" << endl;
 535     cout << "\t[ --ignore-checksum ]" << endl;
 536     cout << "\t[ --sg-swap ]" << endl;
 537 }
 538
 539
 540 int main( int argc, char **argv ) {
 541     double hertz = 60.0;
 542     string out_host = "localhost";
 543     bool do_broadcast = false;
 544
 545     // process command line arguments
 546     for ( int i = 1; i < argc; ++i ) {
 547         if ( strcmp( argv[i], "--help" ) == 0 ) {
 548             usage( argv[0] );
 549             exit( 0 );
 550         } else if ( strcmp( argv[i], "--hertz" ) == 0 ) {
 551             ++i;
 552             if ( i < argc ) {
 553                 hertz = atof( argv[i] );
 554             } else {
 555                 usage( argv[0] );
 556                 exit( -1 );
 557             }
 558         } else if ( strcmp( argv[i], "--infile" ) == 0 ) {
 559             ++i;
 560             if ( i < argc ) {
 561                 infile = argv[i];
 562             } else {
 563                 usage( argv[0] );
 564                 exit( -1 );
 565             }
 566         } else if ( strcmp( argv[i], "--flight" ) == 0 ) {
 567             ++i;
 568             if ( i < argc ) {
 569                 flight_dir = argv[i];
 570             } else {
 571                 usage( argv[0] );
 572                 exit( -1 );
 573             }
 574         } else if ( strcmp( argv[i], "--outfile" ) == 0 ) {
 575             ++i;
 576             if ( i < argc ) {
 577                 outfile = argv[i];
 578             } else {
 579                 usage( argv[0] );
 580                 exit( -1 );
 581             }
 582         } else if ( strcmp( argv[i], "--serial" ) == 0 ) {
 583             ++i;
 584             if ( i < argc ) {
 585                 serialdev = argv[i];
 586             } else {
 587                 usage( argv[0] );
 588                 exit( -1 );
 589             }
 590         } else if ( strcmp( argv[i], "--host" ) == 0 ) {
 591             ++i;
 592             if ( i < argc ) {
 593                 out_host = argv[i];
 594             } else {
 595                 usage( argv[0] );
 596                 exit( -1 );
 597             }
 598         } else if ( strcmp( argv[i], "--broadcast" ) == 0 ) {
 599           do_broadcast = true;
 600         } else if ( strcmp( argv[i], "--opengc-port" ) == 0 ) {
 601             ++i;
 602             if ( i < argc ) {
 603                 opengc_port = atoi( argv[i] );
 604             } else {
 605                 usage( argv[0] );
 606                 exit( -1 );
 607             }
 608         } else if ( strcmp( argv[i], "--fdm-port" ) == 0 ) {
 609             ++i;
 610             if ( i < argc ) {
 611                 fdm_port = atoi( argv[i] );
 612             } else {
 613                 usage( argv[0] );
 614                 exit( -1 );
 615             }
 616         } else if ( strcmp( argv[i], "--ctrls-port" ) == 0 ) {
 617             ++i;
 618             if ( i < argc ) {
 619                 ctrls_port = atoi( argv[i] );
 620             } else {
 621                 usage( argv[0] );
 622                 exit( -1 );
 623             }
 624         } else if ( strcmp (argv[i], "--groundtrack-heading" ) == 0 ) {
 625             use_ground_track_hdg = true;
 626         } else if ( strcmp (argv[i], "--ground-speed" ) == 0 ) {
 627             use_ground_speed = true;
 628         } else if (strcmp (argv[i], "--estimate-control-deflections" ) == 0) {
 629             est_controls = true;
 630         } else if ( strcmp( argv[i], "--altitude-offset" ) == 0 ) {
 631             ++i;
 632             if ( i < argc ) {
 633                 alt_offset = atof( argv[i] );
 634             } else {
 635                 usage( argv[0] );
 636                 exit( -1 );
 637             }
 638         } else if ( strcmp( argv[i], "--skip-seconds" ) == 0 ) {
 639             ++i;
 640             if ( i < argc ) {
 641                 skip = atof( argv[i] );
 642             } else {
 643                 usage( argv[0] );
 644                 exit( -1 );
 645             }
 646         } else if ( strcmp( argv[i], "--no-real-time" ) == 0 ) {
 647             run_real_time = false;
 648         } else if ( strcmp( argv[i], "--ignore-checksum" ) == 0 ) {
 649             ignore_checksum = true;
 650         } else if ( strcmp( argv[i], "--sg-swap" ) == 0 ) {
 651             sg_swap = true;
 652         } else {
 653             usage( argv[0] );
 654             exit( -1 );
 655         }
 656     }
 657
 658     // Setup up outgoing network connections
 659
 660     netInit( &argc,argv ); // We must call this before any other net stuff
 661
 662     if ( ! opengc_sock.open( false ) ) {  // open a UDP socket
 663         cout << "error opening opengc output socket" << endl;
 664         return -1;
 665     }
 666     if ( ! fdm_sock.open( false ) ) {  // open a UDP socket
 667         cout << "error opening fdm output socket" << endl;
 668         return -1;
 669     }
 670     if ( ! ctrls_sock.open( false ) ) {  // open a UDP socket
 671         cout << "error opening ctrls output socket" << endl;
 672         return -1;
 673     }
 674     cout << "open net channels" << endl;
 675
 676     opengc_sock.setBlocking( false );
 677     fdm_sock.setBlocking( false );
 678     ctrls_sock.setBlocking( false );
 679     cout << "blocking false" << endl;
 680
 681     if ( do_broadcast ) {
 682         opengc_sock.setBroadcast( true );
 683         fdm_sock.setBroadcast( true );
 684         ctrls_sock.setBroadcast( true );
 685     }
 686
 687     if ( opengc_sock.connect( out_host.c_str(), opengc_port ) == -1 ) {
 688         perror("connect");
 689         cout << "error connecting to outgoing opengc port: " << out_host
 690              << ":" << opengc_port << endl;
 691         return -1;
 692     }
 693     cout << "connected outgoing opengc socket" << endl;
 694
 695     if ( fdm_sock.connect( out_host.c_str(), fdm_port ) == -1 ) {
 696         perror("connect");
 697         cout << "error connecting to outgoing fdm port: " << out_host
 698              << ":" << fdm_port << endl;
 699         return -1;
 700     }
 701     cout << "connected outgoing fdm socket" << endl;
 702
 703     if ( ctrls_sock.connect( out_host.c_str(), ctrls_port ) == -1 ) {
 704         perror("connect");
 705         cout << "error connecting to outgoing ctrls port: " << out_host
 706              << ":" << ctrls_port << endl;
 707         return -1;
 708     }
 709     cout << "connected outgoing ctrls socket" << endl;
 710
 711     if ( sg_swap ) {
 712         track.set_stargate_swap_mode();
 713     }
 714
 715     if ( infile.length() || flight_dir.length() ) {
 716         if ( infile.length() ) {
 717             // Load data from a stream log data file
 718             track.load_stream( infile, ignore_checksum );
 719         } else if ( flight_dir.length() ) {
 720             // Load data from a flight directory
 721             track.load_flight( flight_dir );
 722         }
 723         cout << "Loaded " << track.gps_size() << " gps records." << endl;
 724         cout << "Loaded " << track.imu_size() << " imu records." << endl;
 725         cout << "Loaded " << track.nav_size() << " nav records." << endl;
 726         cout << "Loaded " << track.servo_size() << " servo records." << endl;
 727         cout << "Loaded " << track.health_size() << " health records." << endl;
 728
 729         int size = track.imu_size();
 730
 731         double current_time = track.get_imupt(0).time;
 732         cout << "Track begin time is " << current_time << endl;
 733         double end_time = track.get_imupt(size-1).time;
 734         cout << "Track end time is " << end_time << endl;
 735         cout << "Duration = " << end_time - current_time << endl;
 736
 737         if ( track.gps_size() > 0 ) {
 738             double tmp = track.get_gpspt(track.gps_size()-1).ITOW;
 739             int days = (int)(tmp / (24 * 60 * 60));
 740             tmp -= days * 24 * 60 * 60;
 741             int hours = (int)(tmp / (60 * 60));
 742             tmp -= hours * 60 * 60;
 743             int min = (int)(tmp / 60);
 744             tmp -= min * 60;
 745             double sec = tmp;
 746             printf("[GPS  ]:ITOW= %.3f[sec]  %dd %02d:%02d:%06.3f\n",
 747                    tmp, days, hours, min, sec);
 748         }
 749
 750
 751         // advance skip seconds forward
 752         current_time += skip;
 753
 754         frame_us = 1000000.0 / hertz;
 755         if ( frame_us < 0.0 ) {
 756             frame_us = 0.0;
 757         }
 758
 759         SGTimeStamp start_time;
 760         start_time.stamp();
 761         int gps_count = 0;
 762         int imu_count = 0;
 763         int nav_count = 0;
 764         int servo_count = 0;
 765         int health_count = 0;
 766
 767         gps gps0, gps1;
 768         gps0 = gps1 = track.get_gpspt( 0 );
 769
 770         imu imu0, imu1;
 771         imu0 = imu1 = track.get_imupt( 0 );
 772
 773         nav nav0, nav1;
 774         nav0 = nav1 = track.get_navpt( 0 );
 775
 776         servo servo0, servo1;
 777         servo0 = servo1 = track.get_servopt( 0 );
 778
 779         health health0, health1;
 780         health0 = health1 = track.get_healthpt( 0 );
 781
 782         double last_lat = -999.9, last_lon = -999.9;
 783
 784         printf("<gpx>\n");
 785         printf(" <trk>\n");
 786         printf("  <trkseg>\n");
 787         while ( current_time < end_time ) {
 788             // cout << "current_time = " << current_time << " end_time = "
 789             //      << end_time << endl;
 790
 791             // Advance gps pointer
 792             while ( current_time > gps1.time
 793                     && gps_count < track.gps_size() - 1 )
 794             {
 795                 gps0 = gps1;
 796                 ++gps_count;
 797                 // cout << "count = " << count << endl;
 798                 gps1 = track.get_gpspt( gps_count );
 799             }
 800             // cout << "p0 = " << p0.get_time() << " p1 = " << p1.get_time()
 801             //      << endl;
 802
 803             // Advance imu pointer
 804             while ( current_time > imu1.time
 805                     && imu_count < track.imu_size() - 1 )
 806             {
 807                 imu0 = imu1;
 808                 ++imu_count;
 809                 // cout << "count = " << count << endl;
 810                 imu1 = track.get_imupt( imu_count );
 811             }
 812             //  cout << "pos0 = " << pos0.get_seconds()
 813             // << " pos1 = " << pos1.get_seconds() << endl;
 814
 815             // Advance nav pointer
 816             while ( current_time > nav1.time
 817                     && nav_count < track.nav_size() - 1 )
 818             {
 819                 nav0 = nav1;
 820                 ++nav_count;
 821                 // cout << "nav count = " << nav_count << endl;
 822                 nav1 = track.get_navpt( nav_count );
 823             }
 824             //  cout << "pos0 = " << pos0.get_seconds()
 825             // << " pos1 = " << pos1.get_seconds() << endl;
 826
 827             // Advance servo pointer
 828             while ( current_time > servo1.time
 829                     && servo_count < track.servo_size() - 1 )
 830             {
 831                 servo0 = servo1;
 832                 ++servo_count;
 833                 // cout << "count = " << count << endl;
 834                 servo1 = track.get_servopt( servo_count );
 835             }
 836             //  cout << "pos0 = " << pos0.get_seconds()
 837             // << " pos1 = " << pos1.get_seconds() << endl;
 838
 839             // Advance health pointer
 840             while ( current_time > health1.time
 841                     && health_count < track.health_size() - 1 )
 842             {
 843                 health0 = health1;
 844                 ++health_count;
 845                 // cout << "count = " << count << endl;
 846                 health1 = track.get_healthpt( health_count );
 847             }
 848             //  cout << "pos0 = " << pos0.get_seconds()
 849             // << " pos1 = " << pos1.get_seconds() << endl;
 850
 851             double gps_percent;
 852             if ( fabs(gps1.time - gps0.time) < 0.00001 ) {
 853                 gps_percent = 0.0;
 854             } else {
 855                 gps_percent =
 856                     (current_time - gps0.time) /
 857                     (gps1.time - gps0.time);
 858             }
 859             // cout << "Percent = " << percent << endl;
 860
 861             double imu_percent;
 862             if ( fabs(imu1.time - imu0.time) < 0.00001 ) {
 863                 imu_percent = 0.0;
 864             } else {
 865                 imu_percent =
 866                     (current_time - imu0.time) /
 867                     (imu1.time - imu0.time);
 868             }
 869             // cout << "Percent = " << percent << endl;
 870
 871             double nav_percent;
 872             if ( fabs(nav1.time - nav0.time) < 0.00001 ) {
 873                 nav_percent = 0.0;
 874             } else {
 875                 nav_percent =
 876                     (current_time - nav0.time) /
 877                     (nav1.time - nav0.time);
 878             }
 879             // cout << "Percent = " << percent << endl;
 880
 881             double servo_percent;
 882             if ( fabs(servo1.time - servo0.time) < 0.00001 ) {
 883                 servo_percent = 0.0;
 884             } else {
 885                 servo_percent =
 886                     (current_time - servo0.time) /
 887                     (servo1.time - servo0.time);
 888             }
 889             // cout << "Percent = " << percent << endl;
 890
 891             double health_percent;
 892             if ( fabs(health1.time - health0.time) < 0.00001 ) {
 893                 health_percent = 0.0;
 894             } else {
 895                 health_percent =
 896                     (current_time - health0.time) /
 897                     (health1.time - health0.time);
 898             }
 899             // cout << "Percent = " << percent << endl;
 900
 901             gps gpspacket = UGEARInterpGPS( gps0, gps1, gps_percent );
 902             imu imupacket = UGEARInterpIMU( imu0, imu1, imu_percent );
 903             nav navpacket = UGEARInterpNAV( nav0, nav1, nav_percent );
 904             servo servopacket = UGEARInterpSERVO( servo0, servo1,
 905                                                   servo_percent );
 906             health healthpacket = UGEARInterpHEALTH( health0, health1,
 907                                                   health_percent );
 908
 909             // cout << current_time << " " << p0.lat_deg << ", " << p0.lon_deg
 910             //      << endl;
 911             // cout << current_time << " " << p1.lat_deg << ", " << p1.lon_deg
 912             //      << endl;
 913             // cout << (double)current_time << " " << pos.lat_deg << ", "
 914             //      << pos.lon_deg << " " << att.yaw_deg << endl;
 915             if ( gpspacket.lat > -500 ) {
 916                 // printf( "%.3f  %.4f %.4f %.1f  %.2f %.2f %.2f\n",
 917                 //         current_time,
 918                 //         navpacket.lat, navpacket.lon, navpacket.alt,
 919                 //         imupacket.psi, imupacket.the, imupacket.phi );
 920                 double dlat = last_lat - navpacket.lat;
 921                 double dlon = last_lon - navpacket.lon;
 922                 double dist = sqrt( dlat*dlat + dlon*dlon );
 923                 if ( dist > 0.01 ) {
 924                     printf("   <trkpt lat=\"%.8f\" lon=\"%.8f\"></trkpt>\n",
 925                            navpacket.lat, navpacket.lon );
 926                     // printf(" </wpt>\n");
 927                     last_lat = navpacket.lat;
 928                     last_lon = navpacket.lon;
 929                 }
 930             }
 931
 932             if ( (fabs(gpspacket.lat) < 0.0001 &&
 933                   fabs(gpspacket.lon) < 0.0001 &&
 934                   fabs(gpspacket.alt) < 0.0001) )
 935             {
 936                 printf("WARNING: LOST GPS!!!\n");
 937                 gps_status = -1.0;
 938             } else {
 939                 gps_status = 1.0;
 940             }
 941
 942             send_data_udp( &gpspacket, &imupacket, &navpacket, &servopacket,
 943                            &healthpacket );
 944
 945             if ( run_real_time ) {
 946                 // Update the elapsed time.
 947                 static bool first_time = true;
 948                 if ( first_time ) {
 949                     last_time_stamp.stamp();
 950                     first_time = false;
 951                 }
 952
 953                 current_time_stamp.stamp();
 954                 /* Convert to ms */
 955                 double elapsed_us = current_time_stamp - last_time_stamp;
 956                 if ( elapsed_us < (frame_us - 2000) ) {
 957                     double requested_us = (frame_us - elapsed_us) - 2000 ;
 958                     ulMilliSecondSleep ( (int)(requested_us / 1000.0) ) ;
 959                 }
 960                 current_time_stamp.stamp();
 961                 while ( current_time_stamp - last_time_stamp < frame_us ) {
 962                     current_time_stamp.stamp();
 963                 }
 964             }
 965
 966             current_time += (frame_us / 1000000.0);
 967             last_time_stamp = current_time_stamp;
 968         }
 969
 970         printf("   <trkpt lat=\"%.8f\" lon=\"%.8f\"></trkpt>\n",
 971                nav1.lat, nav1.lon );
 972
 973         printf("  </trkseg>\n");
 974         printf(" </trk>\n");
 975         nav0 = track.get_navpt( 0 );
 976         nav1 = track.get_navpt( track.nav_size() - 1 );
 977         printf(" <wpt lat=\"%.8f\" lon=\"%.8f\"></wpt>\n",
 978                nav0.lat, nav0.lon );
 979         printf(" <wpt lat=\"%.8f\" lon=\"%.8f\"></wpt>\n",
 980                nav1.lat, nav1.lon );
 981         printf("<gpx>\n");
 982
 983         cout << "Processed " << imu_count << " entries in "
 984              << (current_time_stamp - start_time) / 1000000 << " seconds."
 985              << endl;
 986     } else if ( serialdev.length() ) {
 987         // process incoming data from the serial port
 988
 989         int count = 0;
 990         double current_time = 0.0;
 991         double last_time = 0.0;
 992
 993         gps gpspacket; bzero( &gpspacket, sizeof(gpspacket) );
 994         imu imupacket; bzero( &imupacket, sizeof(imupacket) );
 995         nav navpacket; bzero( &navpacket, sizeof(navpacket) );
 996         servo servopacket; bzero( &servopacket, sizeof(servopacket) );
 997         health healthpacket; bzero( &healthpacket, sizeof(healthpacket) );
 998
 999         double gps_time = 0.0;
1000         double imu_time = 0.0;
1001         double nav_time = 0.0;
1002         double servo_time = 0.0;
1003         double health_time = 0.0;
1004         double command_time = 0.0;
1005         double command_heartbeat = 0.0;
1006
1007         // open the serial port device
1008         SGSerialPort uavcom( serialdev, 115200 );
1009         if ( !uavcom.is_enabled() ) {
1010             cout << "Cannot open: " << serialdev << endl;
1011             return false;
1012         }
1013
1014         // open up the data log file if requested
1015         if ( !outfile.length() ) {
1016             cout << "no --outfile <name> specified, cannot capture data!"
1017                  << endl;
1018             return false;
1019         }
1020         SGFile log( outfile );
1021         if ( !log.open( SG_IO_OUT ) ) {
1022             cout << "Cannot open: " << outfile << endl;
1023             return false;
1024         }
1025
1026         // create the command channel manager
1027         UGCommand command;
1028
1029         // add some test commands
1030         //command.add("ap,alt,1000");
1031         //command.add("home,158.0,32.5");
1032         //command.add("go,home");
1033         //command.add("go,route");
1034
1035         while ( uavcom.is_enabled() ) {
1036             // cout << "looking for next message ..." << endl;
1037             int id = track.next_message( &uavcom, &log, &gpspacket,
1038                                          &imupacket, &navpacket, &servopacket,
1039                                          &healthpacket, ignore_checksum );
1040             // cout << "message id = " << id << endl;
1041             count++;
1042
1043             if ( id == GPS_PACKET ) {
1044                 if ( gpspacket.time > gps_time ) {
1045                     gps_time = gpspacket.time;
1046                     current_time = gps_time;
1047                 } else {
1048                   cout << "oops gps back in time: " << gpspacket.time << " " << gps_time << endl;
1049                 }
1050             } else if ( id == IMU_PACKET ) {
1051                 if ( imupacket.time > imu_time ) {
1052                     imu_time = imupacket.time;
1053                     current_time = imu_time;
1054                 } else {
1055                     cout << "oops imu back in time: " << imupacket.time << " " << imu_time << endl;
1056                 }
1057             } else if ( id == NAV_PACKET ) {
1058                 if ( navpacket.time > nav_time ) {
1059                     nav_time = navpacket.time;
1060                     current_time = nav_time;
1061                 } else {
1062                     cout << "oops nav back in time: " << navpacket.time << " " << nav_time << endl;
1063                 }
1064             } else if ( id == SERVO_PACKET ) {
1065                 if ( servopacket.time > servo_time ) {
1066                     servo_time = servopacket.time;
1067                     current_time = servo_time;
1068                 } else {
1069                     cout << "oops servo back in time: " << servopacket.time << " " << servo_time << endl;
1070                 }
1071             } else if ( id == HEALTH_PACKET ) {
1072                 if ( healthpacket.time > health_time ) {
1073                     health_time = healthpacket.time;
1074                     current_time = health_time;
1075                     printf("Received a health packet, sequence: %d\n",
1076                            healthpacket.command_sequence);
1077                     command.update_cmd_sequence(healthpacket.command_sequence);
1078                 } else {
1079                     cout << "oops health back in time: " << healthpacket.time << " " << health_time << endl;
1080                 }
1081
1082             }
1083
1084             if ( (current_time > gps_time + 2) ||
1085                  (fabs(gpspacket.lat) < 0.0001 &&
1086                   fabs(gpspacket.lon) < 0.0001 &&
1087                   fabs(gpspacket.alt) < 0.0001) )
1088             {
1089                 printf("WARNING: LOST GPS!!!\n");
1090                 gps_status = -1.0;
1091             } else {
1092                 gps_status = 1.0;
1093             }
1094
1095             // Generate a ground station heart beat every 5 seconds
1096             if ( current_time >= command_heartbeat + 5 ) {
1097                 command.add("hb");
1098                 command_heartbeat = current_time;
1099             }
1100
1101             // Command update @ 1hz
1102             if ( current_time >= command_time + 1 ) {
1103                 command.update(&uavcom);
1104                 command_time = current_time;
1105             }
1106
1107             // Relay data on to FlightGear and LFSTech Glass
1108             if ( current_time >= last_time + (1/hertz) ) {
1109                 // if ( gpspacket.lat > -500 ) {
1110                 int londeg = (int)navpacket.lon;
1111                 double lonmin = fabs(navpacket.lon - londeg);
1112                 int latdeg = (int)navpacket.lat;
1113                 double latmin = fabs(navpacket.lat - latdeg);
1114                 char londir = 'E'; if ( londeg < 0 ) londir = 'W';
1115                 char latdir = 'N'; if ( latdeg < 0 ) latdir = 'S';
1116                 londeg = abs(londeg);
1117                 latdeg = abs(latdeg);
1118                 /*printf( "%.2f  %c%02d:%.4f %c%03d:%.4f %.1f  %.2f %.2f %.2f\n",
1119                         current_time,
1120                         latdir, latdeg, latmin, londir, londeg, lonmin,
1121                         navpacket.alt,
1122                         imupacket.phi*SGD_RADIANS_TO_DEGREES,
1123                         imupacket.the*SGD_RADIANS_TO_DEGREES,
1124                         imupacket.psi*SGD_RADIANS_TO_DEGREES ); */
1125                 // }
1126
1127                 last_time = current_time;
1128                 send_data_udp( &gpspacket, &imupacket, &navpacket,
1129                                &servopacket, &healthpacket );
1130             }
1131         }
1132     }
1133
1134     return 0;
1135 }