+ _last_pos = ctx.pos;
+}
+
+void
+GPS::updateNearestAirport(UpdateContext& ctx)
+{
+ if (!_get_nearest_airport_node->getBoolValue()) {
+ return;
+ }
+
+ // If the get-nearest-airport-node is true.
+ // Get the nearest airport, and set it as waypoint 1.
+
+ FGPositioned::TypeFilter aptFilter(FGPositioned::AIRPORT);
+ FGPositionedRef a = FGPositioned::findClosest(ctx.pos, 360.0, &aptFilter);
+ if (!a) {
+ return;
+ }
+
+ _wp1_position = a->geod();
+ _wp1_ID_node->setStringValue(a->ident().c_str());
+ _wp1_name_node->setStringValue(a->name().c_str());
+ _get_nearest_airport_node->setBoolValue(false);
+ _last_wp1_ID = a->ident(); // don't trigger updateWaypoint1();
+ ctx.waypoint_changed = true;
+}
+
+void
+GPS::updateWithValid(UpdateContext& ctx)
+{
+ assert(_last_valid);
+ double distance_m;
+ SGGeodesy::inverse(_last_pos, ctx.pos, ctx.track1_deg, ctx.track2_deg, distance_m );
+
+ ctx.speed_kt = ((distance_m * SG_METER_TO_NM) * ((1 / ctx.dt) * 3600.0));
+
+ double vertical_speed_mpm = ((ctx.pos.getElevationM() - _last_pos.getElevationM()) * 60 /
+ ctx.dt);
+ _indicated_vertical_speed_node->setDoubleValue(vertical_speed_mpm * SG_METER_TO_FEET);
+ _true_track_node->setDoubleValue(ctx.track1_deg);
+
+ ctx.magvar_deg = _magvar_node->getDoubleValue();
+ double mag_track_bearing = ctx.track1_deg - ctx.magvar_deg;
+ SG_NORMALIZE_RANGE(mag_track_bearing, 0.0, 360.0);
+ _magnetic_track_node->setDoubleValue(mag_track_bearing);
+ ctx.speed_kt = fgGetLowPass(_last_speed_kts, ctx.speed_kt, ctx.dt/20.0);
+ _last_speed_kts = ctx.speed_kt;
+ _speed_node->setDoubleValue(ctx.speed_kt);
+
+ double odometer = _odometer_node->getDoubleValue();
+ _odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
+ odometer = _trip_odometer_node->getDoubleValue();
+ _trip_odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
+
+ updateNearestAirport(ctx);
+ updateWaypoint0(ctx);
+ updateWaypoint1(ctx);
+
+ ctx.wp0_pos = _wp0_position.get();
+ ctx.wp1_pos = _wp1_position.get();
+ // if this flag is set, we need to recompute leg data, because either
+ // WP0 or WP1 has been updated
+ if (ctx.waypoint_changed) {
+ waypointChanged(ctx);
+ }
+
+ ctx.wp0_course_deg = _wp0_course_node->getDoubleValue();
+ ctx.wp1_course_deg = _wp1_course_node->getDoubleValue();
+
+ updateWaypoint0Course(ctx);
+ updateWaypoint1Course(ctx);
+ updateLegCourse(ctx);
+
+ // Altitude deviation
+ //double desired_altitude_m = wp1_altitude_m
+ // + wp1_distance * _alt_dist_ratio;
+ //double altitude_deviation_m = altitude_m - desired_altitude_m;
+ // _alt_deviation_node->setDoubleValue(altitude_deviation_m * SG_METER_TO_FEET);
+
+ updateTrackingBug(ctx);
+}
+
+void
+GPS::updateLegCourse(UpdateContext& ctx)
+{
+ // Leg course deviation is the diffenrence between the bearing
+ // and the course.
+ double course_deviation_deg = ctx.wp1_bearing_deg - _course_deg;
+ SG_NORMALIZE_RANGE(course_deviation_deg, -180.0, 180.0);
+
+ // If the course deviation is less than 90 degrees to either side,
+ // our desired course is towards the waypoint.
+ // It does not matter if we are actually moving
+ // towards or from the waypoint.
+ if (fabs(course_deviation_deg) < 90.0) {
+ _leg_to_flag_node->setBoolValue(true);
+ }
+ // If it's more than 90 degrees the desired
+ // course is from the waypoint.
+ else if (fabs(course_deviation_deg) > 90.0) {
+ _leg_to_flag_node->setBoolValue(false);
+ // When the course is away from the waypoint,
+ // it makes sense to change the sign of the deviation.
+ course_deviation_deg *= -1.0;
+ SG_NORMALIZE_RANGE(course_deviation_deg, -90.0, 90.0);
+ }
+
+ _leg_course_deviation_node->setDoubleValue(course_deviation_deg);
+
+ // Cross track error.
+ double course_error_m = sin(course_deviation_deg * SG_PI / 180.0)
+ * (_distance_m);
+ _leg_course_error_nm_node->setDoubleValue(course_error_m * SG_METER_TO_NM);
+
+}
+
+void
+GPS::updateTrackingBug(UpdateContext& ctx)
+{
+ double tracking_bug = _tracking_bug_node->getDoubleValue();
+ double true_bug_error = tracking_bug - ctx.track1_deg;
+ double magnetic_bug_error = tracking_bug - _magnetic_track_node->getDoubleValue();
+
+ // Get the errors into the (-180,180) range.
+ SG_NORMALIZE_RANGE(true_bug_error, -180.0, 180.0);
+ SG_NORMALIZE_RANGE(magnetic_bug_error, -180.0, 180.0);
+
+ _true_bug_error_node->setDoubleValue(true_bug_error);
+ _magnetic_bug_error_node->setDoubleValue(magnetic_bug_error);
+}
+
+void
+GPS::waypointChanged(UpdateContext& ctx)
+{
+ // If any of the two waypoints have changed
+ // we need to calculate a new course between them,
+ // and values for vertical navigation.
+ assert(ctx.waypoint_changed);
+
+ double track2;
+ SGGeodesy::inverse(ctx.wp0_pos, ctx.wp1_pos, _course_deg, track2, _distance_m);
+
+ double leg_mag_course = _course_deg - _magvar_node->getDoubleValue();
+ SG_NORMALIZE_RANGE(leg_mag_course, 0.0, 360.0);
+
+ // Get the altitude / distance ratio
+ if ( _distance_m > 0.0 ) {
+ double alt_difference_m = ctx.wp0_pos.getElevationM() - ctx.wp1_pos.getElevationM();
+ _alt_dist_ratio = alt_difference_m / _distance_m;
+ }
+
+ _leg_distance_node->setDoubleValue(_distance_m * SG_METER_TO_NM);
+ _leg_course_node->setDoubleValue(_course_deg);
+ _leg_magnetic_course_node->setDoubleValue(leg_mag_course);
+ _alt_dist_ratio_node->setDoubleValue(_alt_dist_ratio);
+}
+
+void
+GPS::updateWaypoint0(UpdateContext& ctx)
+{
+ string id(_wp0_ID_node->getStringValue());
+ if (_last_wp0_ID == id) {
+ return; // easy, nothing to do
+ }
+
+ FGPositionedRef result = FGPositioned::findClosestWithIdent(id, ctx.pos);
+ if (!result) {
+ // not found, hmm
+ _last_wp0_ID = id;
+ return;
+ }
+
+ _wp0_position = result->geod();
+ _wp0_name_node->setStringValue(result->name().c_str());
+ _last_wp0_ID = id;
+ ctx.waypoint_changed = true;
+}
+
+void
+GPS::updateWaypoint1(UpdateContext& ctx)
+{
+ string id(_wp1_ID_node->getStringValue());
+ if (_last_wp1_ID == id) {
+ return; // easy, nothing to do
+ }
+
+ FGPositionedRef result = FGPositioned::findClosestWithIdent(id, ctx.pos);
+ if (!result) {
+ // not found, hmm
+ _last_wp1_ID = id;
+ return;
+ }
+
+ _wp1_position = result->geod();
+ _wp1_name_node->setStringValue(result->name().c_str());
+ _last_wp1_ID = id;
+ ctx.waypoint_changed = true;
+}
+
+void
+GPS::updateTTWNode(UpdateContext& ctx, double distance_m, SGPropertyNode_ptr node)
+{
+ // Estimate time to waypoint.
+ // The estimation does not take track into consideration,
+ // so if you are going away from the waypoint the TTW will
+ // increase. Makes most sense when travelling directly towards
+ // the waypoint.
+ double TTW = 0.0;
+ if (ctx.speed_kt > 0.0 && distance_m > 0.0) {
+ TTW = (distance_m * SG_METER_TO_NM) / (ctx.speed_kt / 3600);
+ }
+
+ unsigned int TTW_seconds = (int) (TTW + 0.5);
+ if (TTW_seconds < 356400) { // That's 99 hours
+ unsigned int TTW_minutes = 0;
+ unsigned int TTW_hours = 0;
+ char TTW_str[9];
+ while (TTW_seconds >= 3600) {
+ TTW_seconds -= 3600;
+ TTW_hours++;
+ }
+ while (TTW_seconds >= 60) {
+ TTW_seconds -= 60;
+ TTW_minutes++;
+ }
+ snprintf(TTW_str, 9, "%02d:%02d:%02d",
+ TTW_hours, TTW_minutes, TTW_seconds);
+ node->setStringValue(TTW_str);
+ } else {
+ node->setStringValue("--:--:--");
+ }
+}
+
+void
+GPS::updateWaypoint0Course(UpdateContext& ctx)
+{
+ // Find the bearing and distance to waypoint 0.
+ double az2;
+ SGGeodesy::inverse(ctx.pos, ctx.wp0_pos, ctx.wp0_bearing_deg, az2,ctx.wp0_distance);
+ _wp0_distance_node->setDoubleValue(ctx.wp0_distance * SG_METER_TO_NM);
+ _wp0_bearing_node->setDoubleValue(ctx.wp0_bearing_deg);
+
+ double mag_bearing_deg = ctx.wp0_bearing_deg - ctx.magvar_deg;
+ SG_NORMALIZE_RANGE(mag_bearing_deg, 0.0, 360.0);
+ _wp0_mag_bearing_node->setDoubleValue(mag_bearing_deg);
+ double bearing_error_deg = ctx.track1_deg - ctx.wp0_bearing_deg;
+ SG_NORMALIZE_RANGE(bearing_error_deg, -180.0, 180.0);
+ _true_wp0_bearing_error_node->setDoubleValue(bearing_error_deg);
+
+ updateTTWNode(ctx, ctx.wp0_distance, _wp0_ttw_node);
+
+ // Course deviation is the diffenrence between the bearing
+ // and the course.
+ double course_deviation_deg = ctx.wp0_bearing_deg -
+ ctx.wp0_course_deg;
+ SG_NORMALIZE_RANGE(course_deviation_deg, -180.0, 180.0);
+
+ // If the course deviation is less than 90 degrees to either side,
+ // our desired course is towards the waypoint.
+ // It does not matter if we are actually moving
+ // towards or from the waypoint.
+ if (fabs(course_deviation_deg) < 90.0) {
+ _wp0_to_flag_node->setBoolValue(true);
+ }
+ // If it's more than 90 degrees the desired
+ // course is from the waypoint.
+ else if (fabs(course_deviation_deg) > 90.0) {
+ _wp0_to_flag_node->setBoolValue(false);
+ // When the course is away from the waypoint,
+ // it makes sense to change the sign of the deviation.
+ course_deviation_deg *= -1.0;
+ SG_NORMALIZE_RANGE(course_deviation_deg, -90.0, 90.0);
+ }
+
+ _wp0_course_deviation_node->setDoubleValue(course_deviation_deg);
+
+ // Cross track error.
+ double course_error_m = sin(course_deviation_deg * SG_PI / 180.0)
+ * (ctx.wp0_distance);
+ _wp0_course_error_nm_node->setDoubleValue(course_error_m * SG_METER_TO_NM);
+}
+
+void
+GPS::updateWaypoint1Course(UpdateContext& ctx)
+{
+ // Find the bearing and distance to waypoint 0.
+ double az2;
+ SGGeodesy::inverse(ctx.pos, ctx.wp1_pos, ctx.wp1_bearing_deg, az2,ctx.wp1_distance);
+ _wp1_distance_node->setDoubleValue(ctx.wp1_distance * SG_METER_TO_NM);
+ _wp1_bearing_node->setDoubleValue(ctx.wp1_bearing_deg);
+
+ double mag_bearing_deg = ctx.wp1_bearing_deg - ctx.magvar_deg;
+ SG_NORMALIZE_RANGE(mag_bearing_deg, 0.0, 360.0);
+ _wp1_mag_bearing_node->setDoubleValue(mag_bearing_deg);
+ double bearing_error_deg = ctx.track1_deg - ctx.wp1_bearing_deg;
+ SG_NORMALIZE_RANGE(bearing_error_deg, -180.0, 180.0);
+ _true_wp1_bearing_error_node->setDoubleValue(bearing_error_deg);
+
+ updateTTWNode(ctx, ctx.wp1_distance, _wp1_ttw_node);
+
+ // Course deviation is the diffenrence between the bearing
+ // and the course.
+ double course_deviation_deg = ctx.wp1_bearing_deg -
+ ctx.wp1_course_deg;
+ SG_NORMALIZE_RANGE(course_deviation_deg, -180.0, 180.0);
+
+ // If the course deviation is less than 90 degrees to either side,
+ // our desired course is towards the waypoint.
+ // It does not matter if we are actually moving
+ // towards or from the waypoint.
+ if (fabs(course_deviation_deg) < 90.0) {
+ _wp1_to_flag_node->setBoolValue(true);
+ }
+ // If it's more than 90 degrees the desired
+ // course is from the waypoint.
+ else if (fabs(course_deviation_deg) > 90.0) {
+ _wp1_to_flag_node->setBoolValue(false);
+ // When the course is away from the waypoint,
+ // it makes sense to change the sign of the deviation.
+ course_deviation_deg *= -1.0;
+ SG_NORMALIZE_RANGE(course_deviation_deg, -90.0, 90.0);
+ }
+
+ _wp1_course_deviation_node->setDoubleValue(course_deviation_deg);
+
+ // Cross track error.
+ double course_error_m = sin(course_deviation_deg * SG_PI / 180.0)
+ * (ctx.wp1_distance);
+ _wp1_course_error_nm_node->setDoubleValue(course_error_m * SG_METER_TO_NM);
+