src/Instrumentation/gps.cxx

   1 // gps.cxx - distance-measuring equipment.
   2 // Written by David Megginson, started 2003.
   3 //
   4 // This file is in the Public Domain and comes with no warranty.
   5
   6 #ifdef HAVE_CONFIG_H
   7 #  include <config.h>
   8 #endif
   9
  10 #include "gps.hxx"
  11
  12 #include <simgear/compiler.h>
  13 #include <Aircraft/aircraft.hxx>
  14 #include <Main/fg_props.hxx>
  15 #include <Main/util.hxx> // for fgLowPass
  16 #include <Navaids/positioned.hxx>
  17
  18 #include <simgear/math/sg_random.h>
  19 #include <simgear/sg_inlines.h>
  20 #include <simgear/math/sg_geodesy.hxx>
  21
  22 using std::string;
  23
  24
  25 void SGGeodProperty::init(SGPropertyNode* base, const char* lonStr, const char* latStr, const char* altStr)
  26 {
  27     _lon = base->getChild(lonStr, 0, true);
  28     _lat = base->getChild(latStr, 0, true);
  29     if (altStr) {
  30         _alt = base->getChild(altStr, 0, true);
  31     }
  32 }
  33
  34 void SGGeodProperty::init(const char* lonStr, const char* latStr, const char* altStr)
  35 {
  36     _lon = fgGetNode(lonStr, true);
  37     _lat = fgGetNode(latStr, true);
  38     if (altStr) {
  39         _alt = fgGetNode(altStr, true);
  40     }
  41 }
  42
  43 void SGGeodProperty::clear()
  44 {
  45     _lon = _lat = _alt = NULL;
  46 }
  47
  48 void SGGeodProperty::operator=(const SGGeod& geod)
  49 {
  50     _lon->setDoubleValue(geod.getLongitudeDeg());
  51     _lat->setDoubleValue(geod.getLatitudeDeg());
  52     if (_alt) {
  53         _alt->setDoubleValue(geod.getElevationFt());
  54     }
  55 }
  56
  57 SGGeod SGGeodProperty::get() const
  58 {
  59     double lon = _lon->getDoubleValue(),
  60         lat = _lat->getDoubleValue();
  61     if (_alt) {
  62         return SGGeod::fromDegFt(lon, lat, _alt->getDoubleValue());
  63     } else {
  64         return SGGeod::fromDeg(lon,lat);
  65     }
  66 }
  67
  68
  69 GPS::GPS ( SGPropertyNode *node)
  70     : _last_valid(false),
  71       _alt_dist_ratio(0),
  72       _distance_m(0),
  73       _course_deg(0),
  74       _name(node->getStringValue("name", "gps")),
  75       _num(node->getIntValue("number", 0))
  76 {
  77 }
  78
  79 GPS::~GPS ()
  80 {
  81 }
  82
  83 void
  84 GPS::init ()
  85 {
  86     string branch;
  87     branch = "/instrumentation/" + _name;
  88
  89     SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
  90     _position.init("/position/longitude-deg", "/position/latitude-deg", "/position/altitude-ft");
  91     _magvar_node = fgGetNode("/environment/magnetic-variation-deg", true);
  92     _serviceable_node = node->getChild("serviceable", 0, true);
  93     _electrical_node = fgGetNode("/systems/electrical/outputs/gps", true);
  94
  95     SGPropertyNode *wp_node = node->getChild("wp", 0, true);
  96     SGPropertyNode *wp0_node = wp_node->getChild("wp", 0, true);
  97     SGPropertyNode *wp1_node = wp_node->getChild("wp", 1, true);
  98
  99     _wp0_position.init(wp0_node, "longitude-deg", "latitude-deg", "altitude-ft");
 100     _wp0_ID_node = wp0_node->getChild("ID", 0, true);
 101     _wp0_name_node = wp0_node->getChild("name", 0, true);
 102     _wp0_course_node = wp0_node->getChild("desired-course-deg", 0, true);
 103     _wp0_distance_node = wp0_node->getChild("distance-nm", 0, true);
 104     _wp0_ttw_node = wp0_node->getChild("TTW", 0, true);
 105     _wp0_bearing_node = wp0_node->getChild("bearing-true-deg", 0, true);
 106     _wp0_mag_bearing_node = wp0_node->getChild("bearing-mag-deg", 0, true);
 107     _wp0_course_deviation_node =
 108         wp0_node->getChild("course-deviation-deg", 0, true);
 109     _wp0_course_error_nm_node = wp0_node->getChild("course-error-nm", 0, true);
 110     _wp0_to_flag_node = wp0_node->getChild("to-flag", 0, true);
 111     _true_wp0_bearing_error_node =
 112         wp0_node->getChild("true-bearing-error-deg", 0, true);
 113     _magnetic_wp0_bearing_error_node =
 114         wp0_node->getChild("magnetic-bearing-error-deg", 0, true);
 115
 116     _wp1_position.init(wp1_node, "longitude-deg", "latitude-deg", "altitude-ft");
 117     _wp1_ID_node = wp1_node->getChild("ID", 0, true);
 118     _wp1_name_node = wp1_node->getChild("name", 0, true);
 119     _wp1_course_node = wp1_node->getChild("desired-course-deg", 0, true);
 120     _wp1_distance_node = wp1_node->getChild("distance-nm", 0, true);
 121     _wp1_ttw_node = wp1_node->getChild("TTW", 0, true);
 122     _wp1_bearing_node = wp1_node->getChild("bearing-true-deg", 0, true);
 123     _wp1_mag_bearing_node = wp1_node->getChild("bearing-mag-deg", 0, true);
 124     _wp1_course_deviation_node =
 125         wp1_node->getChild("course-deviation-deg", 0, true);
 126     _wp1_course_error_nm_node = wp1_node->getChild("course-error-nm", 0, true);
 127     _wp1_to_flag_node = wp1_node->getChild("to-flag", 0, true);
 128     _true_wp1_bearing_error_node =
 129         wp1_node->getChild("true-bearing-error-deg", 0, true);
 130     _magnetic_wp1_bearing_error_node =
 131         wp1_node->getChild("magnetic-bearing-error-deg", 0, true);
 132     _get_nearest_airport_node =
 133         wp1_node->getChild("get-nearest-airport", 0, true);
 134
 135     _tracking_bug_node = node->getChild("tracking-bug", 0, true);
 136     _raim_node = node->getChild("raim", 0, true);
 137
 138     _indicated_pos.init(node, "indicated-longitude-deg",
 139         "indicated-latitude-deg", "indicated-altitude-ft");
 140
 141     _indicated_vertical_speed_node =
 142         node->getChild("indicated-vertical-speed", 0, true);
 143     _true_track_node =
 144         node->getChild("indicated-track-true-deg", 0, true);
 145     _magnetic_track_node =
 146         node->getChild("indicated-track-magnetic-deg", 0, true);
 147     _speed_node =
 148         node->getChild("indicated-ground-speed-kt", 0, true);
 149     _odometer_node =
 150         node->getChild("odometer", 0, true);
 151     _trip_odometer_node =
 152         node->getChild("trip-odometer", 0, true);
 153     _true_bug_error_node =
 154         node->getChild("true-bug-error-deg", 0, true);
 155     _magnetic_bug_error_node =
 156         node->getChild("magnetic-bug-error-deg", 0, true);
 157
 158     _leg_distance_node =
 159         wp_node->getChild("leg-distance-nm", 0, true);
 160     _leg_course_node =
 161         wp_node->getChild("leg-true-course-deg", 0, true);
 162     _leg_magnetic_course_node =
 163         wp_node->getChild("leg-mag-course-deg", 0, true);
 164     _alt_dist_ratio_node =
 165         wp_node->getChild("alt-dist-ratio", 0, true);
 166     _leg_course_deviation_node =
 167         wp_node->getChild("leg-course-deviation-deg", 0, true);
 168     _leg_course_error_nm_node =
 169         wp_node->getChild("leg-course-error-nm", 0, true);
 170     _leg_to_flag_node =
 171         wp_node->getChild("leg-to-flag", 0, true);
 172     _alt_deviation_node =
 173         wp_node->getChild("alt-deviation-ft", 0, true);
 174
 175     _serviceable_node->setBoolValue(true);
 176 }
 177
 178 void
 179 GPS::clearOutput()
 180 {
 181     _last_valid = false;
 182     _last_speed_kts = 0;
 183     _last_pos = SGGeod();
 184     _raim_node->setDoubleValue(false);
 185     _indicated_pos = SGGeod();
 186           _indicated_vertical_speed_node->setDoubleValue(0);
 187     _true_track_node->setDoubleValue(0);
 188     _magnetic_track_node->setDoubleValue(0);
 189     _speed_node->setDoubleValue(0);
 190     _wp1_distance_node->setDoubleValue(0);
 191     _wp1_bearing_node->setDoubleValue(0);
 192     _wp1_position = SGGeod();
 193     _wp1_course_node->setDoubleValue(0);
 194     _odometer_node->setDoubleValue(0);
 195     _trip_odometer_node->setDoubleValue(0);
 196     _tracking_bug_node->setDoubleValue(0);
 197     _true_bug_error_node->setDoubleValue(0);
 198     _magnetic_bug_error_node->setDoubleValue(0);
 199           _true_wp1_bearing_error_node->setDoubleValue(0);
 200           _magnetic_wp1_bearing_error_node->setDoubleValue(0);
 201 }
 202
 203 void
 204 GPS::update (double delta_time_sec)
 205 {
 206    // If it's off, don't bother.
 207     if (!_serviceable_node->getBoolValue() || !_electrical_node->getBoolValue()) {
 208         clearOutput();
 209         return;
 210     }
 211
 212     UpdateContext ctx;
 213     ctx.dt = delta_time_sec;
 214     ctx.waypoint_changed = false;
 215     ctx.pos = _position.get();
 216
 217     // TODO: Add noise and other errors.
 218 /*
 219
 220     // Bias and random error
 221     double random_factor = sg_random();
 222     double random_error = 1.4;
 223     double error_radius = 5.1;
 224     double bias_max_radius = 5.1;
 225     double random_max_radius = 1.4;
 226
 227     bias_length += (random_factor-0.5) * 1.0e-3;
 228     if (bias_length <= 0.0) bias_length = 0.0;
 229     else if (bias_length >= bias_max_radius) bias_length = bias_max_radius;
 230     bias_angle  += (random_factor-0.5) * 1.0e-3;
 231     if (bias_angle <= 0.0) bias_angle = 0.0;
 232     else if (bias_angle >= 360.0) bias_angle = 360.0;
 233
 234     double random_length = random_factor * random_max_radius;
 235     double random_angle = random_factor * 360.0;
 236
 237     double bias_x = bias_length * cos(bias_angle * SG_PI / 180.0);
 238     double bias_y = bias_length * sin(bias_angle * SG_PI / 180.0);
 239     double random_x = random_length * cos(random_angle * SG_PI / 180.0);
 240     double random_y = random_length * sin(random_angle * SG_PI / 180.0);
 241     double error_x = bias_x + random_x;
 242     double error_y = bias_y + random_y;
 243     double error_length = sqrt(error_x*error_x + error_y*error_y);
 244     double error_angle = atan(error_y / error_x) * 180.0 / SG_PI;
 245
 246     double lat2;
 247     double lon2;
 248     double az2;
 249     geo_direct_wgs_84 ( altitude_m, latitude_deg,
 250                         longitude_deg, error_angle,
 251                         error_length, &lat2, &lon2,
 252                         &az2 );
 253     //cout << lat2 << " " << lon2 << endl;
 254     printf("%f %f \n", bias_length, bias_angle);
 255     printf("%3.7f %3.7f \n", lat2, lon2);
 256     printf("%f %f \n", error_length, error_angle);
 257
 258 */
 259     _raim_node->setBoolValue(true);
 260     _indicated_pos = ctx.pos;
 261
 262     if (_last_valid) {
 263         updateWithValid(ctx);
 264     } else {
 265         _true_track_node->setDoubleValue(0.0);
 266         _magnetic_track_node->setDoubleValue(0.0);
 267         _speed_node->setDoubleValue(0.0);
 268         _last_valid = true;
 269     }
 270
 271     _last_pos = ctx.pos;
 272 }
 273
 274 void
 275 GPS::updateNearestAirport(UpdateContext& ctx)
 276 {
 277     if (!_get_nearest_airport_node->getBoolValue()) {
 278         return;
 279     }
 280
 281     // If the get-nearest-airport-node is true.
 282     // Get the nearest airport, and set it as waypoint 1.
 283
 284     FGPositioned::TypeFilter aptFilter(FGPositioned::AIRPORT);
 285     FGPositionedRef a = FGPositioned::findClosest(ctx.pos, 360.0, &aptFilter);
 286     if (!a) {
 287         return;
 288     }
 289
 290     _wp1_position = a->geod();
 291     _wp1_ID_node->setStringValue(a->ident().c_str());
 292     _wp1_name_node->setStringValue(a->name().c_str());
 293     _get_nearest_airport_node->setBoolValue(false);
 294     _last_wp1_ID = a->ident(); // don't trigger updateWaypoint1();
 295     ctx.waypoint_changed = true;
 296 }
 297
 298 void
 299 GPS::updateWithValid(UpdateContext& ctx)
 300 {
 301     assert(_last_valid);
 302     double distance_m;
 303     SGGeodesy::inverse(_last_pos, ctx.pos, ctx.track1_deg, ctx.track2_deg, distance_m );
 304
 305     ctx.speed_kt = ((distance_m * SG_METER_TO_NM) * ((1 / ctx.dt) * 3600.0));
 306
 307     double vertical_speed_mpm = ((ctx.pos.getElevationM() - _last_pos.getElevationM()) * 60 /
 308                               ctx.dt);
 309           _indicated_vertical_speed_node->setDoubleValue(vertical_speed_mpm * SG_METER_TO_FEET);
 310     _true_track_node->setDoubleValue(ctx.track1_deg);
 311
 312     ctx.magvar_deg = _magvar_node->getDoubleValue();
 313     double mag_track_bearing = ctx.track1_deg - ctx.magvar_deg;
 314     SG_NORMALIZE_RANGE(mag_track_bearing, 0.0, 360.0);
 315     _magnetic_track_node->setDoubleValue(mag_track_bearing);
 316     ctx.speed_kt = fgGetLowPass(_last_speed_kts, ctx.speed_kt, ctx.dt/20.0);
 317     _last_speed_kts = ctx.speed_kt;
 318     _speed_node->setDoubleValue(ctx.speed_kt);
 319
 320     double odometer = _odometer_node->getDoubleValue();
 321     _odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
 322     odometer = _trip_odometer_node->getDoubleValue();
 323     _trip_odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
 324
 325     updateNearestAirport(ctx);
 326     updateWaypoint0(ctx);
 327     updateWaypoint1(ctx);
 328
 329     ctx.wp0_pos = _wp0_position.get();
 330     ctx.wp1_pos = _wp1_position.get();
 331     // if this flag is set, we need to recompute leg data, because either
 332     // WP0 or WP1 has been updated
 333     if (ctx.waypoint_changed) {
 334       waypointChanged(ctx);
 335     }
 336
 337     ctx.wp0_course_deg = _wp0_course_node->getDoubleValue();
 338     ctx.wp1_course_deg = _wp1_course_node->getDoubleValue();
 339
 340     updateWaypoint0Course(ctx);
 341     updateWaypoint1Course(ctx);
 342     updateLegCourse(ctx);
 343
 344     // Altitude deviation
 345     //double desired_altitude_m = wp1_altitude_m
 346     //        + wp1_distance * _alt_dist_ratio;
 347     //double altitude_deviation_m = altitude_m - desired_altitude_m;
 348     //    _alt_deviation_node->setDoubleValue(altitude_deviation_m * SG_METER_TO_FEET);
 349
 350     updateTrackingBug(ctx);
 351 }
 352
 353 void
 354 GPS::updateLegCourse(UpdateContext& ctx)
 355 {
 356      // Leg course deviation is the diffenrence between the bearing
 357     // and the course.
 358     double course_deviation_deg = ctx.wp1_bearing_deg - _course_deg;
 359     SG_NORMALIZE_RANGE(course_deviation_deg, -180.0, 180.0);
 360
 361     // If the course deviation is less than 90 degrees to either side,
 362     // our desired course is towards the waypoint.
 363     // It does not matter if we are actually moving
 364     // towards or from the waypoint.
 365     if (fabs(course_deviation_deg) < 90.0) {
 366         _leg_to_flag_node->setBoolValue(true);
 367     }
 368     // If it's more than 90 degrees the desired
 369     // course is from the waypoint.
 370     else if (fabs(course_deviation_deg) > 90.0) {
 371         _leg_to_flag_node->setBoolValue(false);
 372         // When the course is away from the waypoint,
 373         // it makes sense to change the sign of the deviation.
 374         course_deviation_deg *= -1.0;
 375         SG_NORMALIZE_RANGE(course_deviation_deg, -90.0, 90.0);
 376     }
 377
 378     _leg_course_deviation_node->setDoubleValue(course_deviation_deg);
 379
 380     // Cross track error.
 381     double course_error_m = sin(course_deviation_deg * SG_PI / 180.0)
 382             * (_distance_m);
 383     _leg_course_error_nm_node->setDoubleValue(course_error_m * SG_METER_TO_NM);
 384
 385 }
 386
 387 void
 388 GPS::updateTrackingBug(UpdateContext& ctx)
 389 {
 390     double tracking_bug = _tracking_bug_node->getDoubleValue();
 391     double true_bug_error = tracking_bug - ctx.track1_deg;
 392     double magnetic_bug_error = tracking_bug - _magnetic_track_node->getDoubleValue();
 393
 394     // Get the errors into the (-180,180) range.
 395     SG_NORMALIZE_RANGE(true_bug_error, -180.0, 180.0);
 396     SG_NORMALIZE_RANGE(magnetic_bug_error, -180.0, 180.0);
 397
 398     _true_bug_error_node->setDoubleValue(true_bug_error);
 399     _magnetic_bug_error_node->setDoubleValue(magnetic_bug_error);
 400 }
 401
 402 void
 403 GPS::waypointChanged(UpdateContext& ctx)
 404 {
 405     // If any of the two waypoints have changed
 406     // we need to calculate a new course between them,
 407     // and values for vertical navigation.
 408     assert(ctx.waypoint_changed);
 409
 410     double track2;
 411     SGGeodesy::inverse(ctx.wp0_pos, ctx.wp1_pos, _course_deg, track2, _distance_m);
 412
 413     double leg_mag_course = _course_deg - _magvar_node->getDoubleValue();
 414     SG_NORMALIZE_RANGE(leg_mag_course, 0.0, 360.0);
 415
 416     // Get the altitude / distance ratio
 417     if ( _distance_m > 0.0 ) {
 418         double alt_difference_m = ctx.wp0_pos.getElevationM() - ctx.wp1_pos.getElevationM();
 419         _alt_dist_ratio = alt_difference_m / _distance_m;
 420     }
 421
 422     _leg_distance_node->setDoubleValue(_distance_m * SG_METER_TO_NM);
 423     _leg_course_node->setDoubleValue(_course_deg);
 424     _leg_magnetic_course_node->setDoubleValue(leg_mag_course);
 425     _alt_dist_ratio_node->setDoubleValue(_alt_dist_ratio);
 426 }
 427
 428 void
 429 GPS::updateWaypoint0(UpdateContext& ctx)
 430 {
 431     string id(_wp0_ID_node->getStringValue());
 432     if (_last_wp0_ID == id) {
 433         return; // easy, nothing to do
 434     }
 435
 436     FGPositionedRef result = FGPositioned::findClosestWithIdent(id, ctx.pos);
 437     if (!result) {
 438         // not found, hmm
 439         _last_wp0_ID = id;
 440         return;
 441     }
 442
 443     _wp0_position = result->geod();
 444     _wp0_name_node->setStringValue(result->name().c_str());
 445     _last_wp0_ID = id;
 446     ctx.waypoint_changed = true;
 447 }
 448
 449 void
 450 GPS::updateWaypoint1(UpdateContext& ctx)
 451 {
 452     string id(_wp1_ID_node->getStringValue());
 453     if (_last_wp1_ID == id) {
 454         return; // easy, nothing to do
 455     }
 456
 457     FGPositionedRef result = FGPositioned::findClosestWithIdent(id, ctx.pos);
 458     if (!result) {
 459         // not found, hmm
 460         _last_wp1_ID = id;
 461         return;
 462     }
 463
 464     _wp1_position = result->geod();
 465     _wp1_name_node->setStringValue(result->name().c_str());
 466     _last_wp1_ID = id;
 467     ctx.waypoint_changed = true;
 468 }
 469
 470 void
 471 GPS::updateTTWNode(UpdateContext& ctx, double distance_m, SGPropertyNode_ptr node)
 472 {
 473     // Estimate time to waypoint.
 474     // The estimation does not take track into consideration,
 475     // so if you are going away from the waypoint the TTW will
 476     // increase. Makes most sense when travelling directly towards
 477     // the waypoint.
 478     double TTW = 0.0;
 479     if (ctx.speed_kt > 0.0 && distance_m > 0.0) {
 480         TTW = (distance_m * SG_METER_TO_NM) / (ctx.speed_kt / 3600);
 481     }
 482     if (TTW < 356400.5) { // That's 99 hours
 483       unsigned int TTW_seconds = (int) (TTW + 0.5);
 484       unsigned int TTW_minutes = 0;
 485       unsigned int TTW_hours   = 0;
 486       char TTW_str[9];
 487       TTW_hours   = TTW_seconds / 3600;
 488       TTW_minutes = (TTW_seconds / 60) % 60;
 489       TTW_seconds = TTW_seconds % 60;
 490       snprintf(TTW_str, 9, "%02d:%02d:%02d",
 491         TTW_hours, TTW_minutes, TTW_seconds);
 492       node->setStringValue(TTW_str);
 493     } else {
 494       node->setStringValue("--:--:--");
 495     }
 496 }
 497
 498 void
 499 GPS::updateWaypoint0Course(UpdateContext& ctx)
 500 {
 501     // Find the bearing and distance to waypoint 0.
 502     double az2;
 503     SGGeodesy::inverse(ctx.pos, ctx.wp0_pos, ctx.wp0_bearing_deg, az2,ctx.wp0_distance);
 504     _wp0_distance_node->setDoubleValue(ctx.wp0_distance * SG_METER_TO_NM);
 505     _wp0_bearing_node->setDoubleValue(ctx.wp0_bearing_deg);
 506
 507     double mag_bearing_deg = ctx.wp0_bearing_deg - ctx.magvar_deg;
 508     SG_NORMALIZE_RANGE(mag_bearing_deg, 0.0, 360.0);
 509     _wp0_mag_bearing_node->setDoubleValue(mag_bearing_deg);
 510     double bearing_error_deg = ctx.track1_deg - ctx.wp0_bearing_deg;
 511     SG_NORMALIZE_RANGE(bearing_error_deg, -180.0, 180.0);
 512     _true_wp0_bearing_error_node->setDoubleValue(bearing_error_deg);
 513
 514     updateTTWNode(ctx, ctx.wp0_distance, _wp0_ttw_node);
 515
 516     // Course deviation is the diffenrence between the bearing
 517     // and the course.
 518     double course_deviation_deg = ctx.wp0_bearing_deg -
 519         ctx.wp0_course_deg;
 520     SG_NORMALIZE_RANGE(course_deviation_deg, -180.0, 180.0);
 521
 522     // If the course deviation is less than 90 degrees to either side,
 523     // our desired course is towards the waypoint.
 524     // It does not matter if we are actually moving
 525     // towards or from the waypoint.
 526     if (fabs(course_deviation_deg) < 90.0) {
 527         _wp0_to_flag_node->setBoolValue(true);
 528     }
 529     // If it's more than 90 degrees the desired
 530     // course is from the waypoint.
 531     else if (fabs(course_deviation_deg) > 90.0) {
 532       _wp0_to_flag_node->setBoolValue(false);
 533       // When the course is away from the waypoint,
 534       // it makes sense to change the sign of the deviation.
 535       course_deviation_deg *= -1.0;
 536       SG_NORMALIZE_RANGE(course_deviation_deg, -90.0, 90.0);
 537     }
 538
 539     _wp0_course_deviation_node->setDoubleValue(course_deviation_deg);
 540
 541     // Cross track error.
 542     double course_error_m = sin(course_deviation_deg * SG_PI / 180.0)
 543           * (ctx.wp0_distance);
 544     _wp0_course_error_nm_node->setDoubleValue(course_error_m * SG_METER_TO_NM);
 545 }
 546
 547 void
 548 GPS::updateWaypoint1Course(UpdateContext& ctx)
 549 {
 550     // Find the bearing and distance to waypoint 0.
 551     double az2;
 552     SGGeodesy::inverse(ctx.pos, ctx.wp1_pos, ctx.wp1_bearing_deg, az2,ctx.wp1_distance);
 553     _wp1_distance_node->setDoubleValue(ctx.wp1_distance * SG_METER_TO_NM);
 554     _wp1_bearing_node->setDoubleValue(ctx.wp1_bearing_deg);
 555
 556     double mag_bearing_deg = ctx.wp1_bearing_deg - ctx.magvar_deg;
 557     SG_NORMALIZE_RANGE(mag_bearing_deg, 0.0, 360.0);
 558     _wp1_mag_bearing_node->setDoubleValue(mag_bearing_deg);
 559     double bearing_error_deg = ctx.track1_deg - ctx.wp1_bearing_deg;
 560     SG_NORMALIZE_RANGE(bearing_error_deg, -180.0, 180.0);
 561     _true_wp1_bearing_error_node->setDoubleValue(bearing_error_deg);
 562
 563     updateTTWNode(ctx, ctx.wp1_distance, _wp1_ttw_node);
 564
 565     // Course deviation is the diffenrence between the bearing
 566     // and the course.
 567     double course_deviation_deg = ctx.wp1_bearing_deg -
 568         ctx.wp1_course_deg;
 569     SG_NORMALIZE_RANGE(course_deviation_deg, -180.0, 180.0);
 570
 571     // If the course deviation is less than 90 degrees to either side,
 572     // our desired course is towards the waypoint.
 573     // It does not matter if we are actually moving
 574     // towards or from the waypoint.
 575     if (fabs(course_deviation_deg) < 90.0) {
 576         _wp1_to_flag_node->setBoolValue(true);
 577     }
 578     // If it's more than 90 degrees the desired
 579     // course is from the waypoint.
 580     else if (fabs(course_deviation_deg) > 90.0) {
 581       _wp1_to_flag_node->setBoolValue(false);
 582       // When the course is away from the waypoint,
 583       // it makes sense to change the sign of the deviation.
 584       course_deviation_deg *= -1.0;
 585       SG_NORMALIZE_RANGE(course_deviation_deg, -90.0, 90.0);
 586     }
 587
 588     _wp1_course_deviation_node->setDoubleValue(course_deviation_deg);
 589
 590     // Cross track error.
 591     double course_error_m = sin(course_deviation_deg * SG_PI / 180.0)
 592           * (ctx.wp1_distance);
 593     _wp1_course_error_nm_node->setDoubleValue(course_error_m * SG_METER_TO_NM);
 594
 595 }
 596
 597 // end of gps.cxx