- if (_last_valid) {
- double track1_deg, track2_deg, distance_m, odometer, mag_track_bearing;
- geo_inverse_wgs_84(altitude_m,
- _last_latitude_deg, _last_longitude_deg,
- latitude_deg, longitude_deg,
- &track1_deg, &track2_deg, &distance_m);
- speed_kt = ((distance_m * SG_METER_TO_NM) *
- ((1 / delta_time_sec) * 3600.0));
- vertical_speed_mpm = ((altitude_m - _last_altitude_m) * 60 /
- delta_time_sec);
- _indicated_vertical_speed_node->setDoubleValue
- (vertical_speed_mpm * SG_METER_TO_FEET);
- _true_track_node->setDoubleValue(track1_deg);
- mag_track_bearing = track1_deg - magvar_deg;
- SG_NORMALIZE_RANGE(mag_track_bearing, 0.0, 360.0);
- _magnetic_track_node->setDoubleValue(mag_track_bearing);
- speed_kt = fgGetLowPass(_last_speed_kts, speed_kt, delta_time_sec/20.0);
- _last_speed_kts = speed_kt;
- _speed_node->setDoubleValue(speed_kt);
-
- 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);
-
- // Get waypoint 0 position
- double wp0_longitude_deg = _wp0_longitude_node->getDoubleValue();
- double wp0_latitude_deg = _wp0_latitude_node->getDoubleValue();
- double wp0_altitude_m = _wp0_altitude_node->getDoubleValue()
- * SG_FEET_TO_METER;
- double wp0_course_deg = _wp0_course_node->getDoubleValue();
- double wp0_distance, wp0_bearing_deg, wp0_course_deviation_deg,
- wp0_course_error_m, wp0_TTW, wp0_bearing_error_deg;
- string wp0_ID = _wp0_ID_node->getStringValue();
-
- // Get waypoint 1 position
- double wp1_longitude_deg = _wp1_longitude_node->getDoubleValue();
- double wp1_latitude_deg = _wp1_latitude_node->getDoubleValue();
- double wp1_altitude_m = _wp1_altitude_node->getDoubleValue()
- * SG_FEET_TO_METER;
- double wp1_course_deg = _wp1_course_node->getDoubleValue();
- double wp1_distance, wp1_bearing_deg, wp1_course_deviation_deg,
- wp1_course_error_m, wp1_TTW, wp1_bearing_error_deg;
- string wp1_ID = _wp1_ID_node->getStringValue();
-
- // If the get-nearest-airport-node is true.
- // Get the nearest airport, and set it as waypoint 1.
- if (_get_nearest_airport_node->getBoolValue()) {
- const FGAirport* a = globals->get_airports()->search(longitude_deg, latitude_deg);
- if(a) {
- _wp1_ID_node->setStringValue(a->getId().c_str());
- wp1_longitude_deg = a->getLongitude();
- wp1_latitude_deg = a->getLatitude();
- _wp1_name_node->setStringValue(a->getName().c_str());
- _get_nearest_airport_node->setBoolValue(false);
- _last_wp1_ID = wp1_ID = a->getId().c_str();
- }
- }
-
- // If the waypoint 0 ID has changed, try to find the new ID
- // in the airport-, fix-, nav-database.
- if ( !(_last_wp0_ID == wp0_ID) ) {
- string waypont_type =
- _wp0_waypoint_type_node->getStringValue();
- if (waypont_type == "airport") {
- const FGAirport* a = globals->get_airports()->search( wp0_ID );
- if ( a ) {
- wp0_longitude_deg = a->getLongitude();
- wp0_latitude_deg = a->getLatitude();
- _wp0_name_node->setStringValue(a->getName().c_str());
- }
- }
- else if (waypont_type == "nav") {
- FGNavRecord *n
- = globals->get_navlist()->findByIdent(wp0_ID.c_str(),
- longitude_deg,
- latitude_deg);
- if ( n != NULL ) {
- //cout << "Nav found" << endl;
- wp0_longitude_deg = n->get_lon();
- wp0_latitude_deg = n->get_lat();
- _wp0_name_node->setStringValue(n->get_name().c_str());
- }
- }
- else if (waypont_type == "fix") {
- FGFix f;
- if ( globals->get_fixlist()->query(wp0_ID, &f) ) {
- //cout << "Fix found" << endl;
- wp0_longitude_deg = f.get_lon();
- wp0_latitude_deg = f.get_lat();
- _wp0_name_node->setStringValue(wp0_ID.c_str());
- }
- }
- _last_wp0_ID = wp0_ID;
- }
+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);