- double speed_kt, vertical_speed_mpm;
-
- _raim_node->setBoolValue(true);
- _indicated_longitude_node->setDoubleValue(longitude_deg);
- _indicated_latitude_node->setDoubleValue(latitude_deg);
- _indicated_altitude_node->setDoubleValue(altitude_m * SG_METER_TO_FEET);
-
- 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;
- }
-
- // If the waypoint 1 ID has changed, try to find the new ID
- // in the airport-, fix-, nav-database.
- if ( !(_last_wp1_ID == wp1_ID) ) {
- string waypont_type =
- _wp1_waypoint_type_node->getStringValue();
- if (waypont_type == "airport") {
- const FGAirport* a = globals->get_airports()->search( wp1_ID );
- if ( a ) {
- wp1_longitude_deg = a->getLongitude();
- wp1_latitude_deg = a->getLatitude();
- _wp1_name_node->setStringValue(a->getName().c_str());
- }
- }
- else if (waypont_type == "nav") {
- FGNavRecord *n
- = globals->get_navlist()->findByIdent(wp1_ID.c_str(),
- longitude_deg,
- latitude_deg);
- if ( n != NULL ) {
- //cout << "Nav found" << endl;
- wp1_longitude_deg = n->get_lon();
- wp1_latitude_deg = n->get_lat();
- _wp1_name_node->setStringValue(n->get_name().c_str());
- }
- }
- else if (waypont_type == "fix") {
- FGFix f;
- if ( globals->get_fixlist()->query(wp1_ID, &f) ) {
- //cout << "Fix found" << endl;
- wp1_longitude_deg = f.get_lon();
- wp1_latitude_deg = f.get_lat();
- _wp1_name_node->setStringValue(wp1_ID.c_str());
- }
- }
- _last_wp1_ID = wp1_ID;
- }
-
-
-
- // If any of the two waypoints have changed
- // we need to calculate a new course between them,
- // and values for vertical navigation.
- if ( wp0_longitude_deg != _wp0_longitude_deg ||
- wp0_latitude_deg != _wp0_latitude_deg ||
- wp0_altitude_m != _wp0_altitude_m ||
- wp1_longitude_deg != _wp1_longitude_deg ||
- wp1_latitude_deg != _wp1_latitude_deg ||
- wp1_altitude_m != _wp1_altitude_m )
- {
- // Update the global variables
- _wp0_longitude_deg = wp0_longitude_deg;
- _wp0_latitude_deg = wp0_latitude_deg;
- _wp0_altitude_m = wp0_altitude_m;
- _wp1_longitude_deg = wp1_longitude_deg;
- _wp1_latitude_deg = wp1_latitude_deg;
- _wp1_altitude_m = wp1_altitude_m;
-
- // Get the course and distance from wp0 to wp1
- SGWayPoint wp0(wp0_longitude_deg,
- wp0_latitude_deg, wp0_altitude_m);
- SGWayPoint wp1(wp1_longitude_deg,
- wp1_latitude_deg, wp1_altitude_m);
-
- wp1.CourseAndDistance(wp0, &_course_deg, &_distance_m);
- double leg_mag_course = _course_deg - magvar_deg;
- SG_NORMALIZE_RANGE(leg_mag_course, 0.0, 360.0);
-
- // Get the altitude / distance ratio
- if ( distance_m > 0.0 ) {
- double alt_difference_m = wp0_altitude_m - wp1_altitude_m;
- _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);
-
- _wp0_longitude_node->setDoubleValue(wp0_longitude_deg);
- _wp0_latitude_node->setDoubleValue(wp0_latitude_deg);
- _wp1_longitude_node->setDoubleValue(wp1_longitude_deg);
- _wp1_latitude_node->setDoubleValue(wp1_latitude_deg);
- }
-
-
- // Find the bearing and distance to waypoint 0.
- SGWayPoint wp0(wp0_longitude_deg, wp0_latitude_deg, wp0_altitude_m);
- wp0.CourseAndDistance(longitude_deg, latitude_deg, altitude_m,
- &wp0_bearing_deg, &wp0_distance);
- _wp0_distance_node->setDoubleValue(wp0_distance * SG_METER_TO_NM);
- _wp0_bearing_node->setDoubleValue(wp0_bearing_deg);
- double wp0_mag_bearing_deg = wp0_bearing_deg - magvar_deg;
- SG_NORMALIZE_RANGE(wp0_mag_bearing_deg, 0.0, 360.0);
- _wp0_mag_bearing_node->setDoubleValue(wp0_mag_bearing_deg);
- wp0_bearing_error_deg = track1_deg - wp0_bearing_deg;
- SG_NORMALIZE_RANGE(wp0_bearing_error_deg, -180.0, 180.0);
- _true_wp0_bearing_error_node->setDoubleValue(wp0_bearing_error_deg);
-
- // Estimate time to waypoint 0.
- // 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.
- if (speed_kt > 0.0 && wp0_distance > 0.0) {
- wp0_TTW = (wp0_distance * SG_METER_TO_NM) / (speed_kt / 3600);
- }
- else {
- wp0_TTW = 0.0;
- }
- unsigned int wp0_TTW_seconds = (int) (wp0_TTW + 0.5);
- if (wp0_TTW_seconds < 356400) { // That's 99 hours
- unsigned int wp0_TTW_minutes = 0;
- unsigned int wp0_TTW_hours = 0;
- char wp0_TTW_str[9];
- while (wp0_TTW_seconds >= 3600) {
- wp0_TTW_seconds -= 3600;
- wp0_TTW_hours++;
- }
- while (wp0_TTW_seconds >= 60) {
- wp0_TTW_seconds -= 60;
- wp0_TTW_minutes++;
- }
- snprintf(wp0_TTW_str, 9, "%02d:%02d:%02d",
- wp0_TTW_hours, wp0_TTW_minutes, wp0_TTW_seconds);
- _wp0_ttw_node->setStringValue(wp0_TTW_str);
- }
- else
- _wp0_ttw_node->setStringValue("--:--:--");
-
- // Course deviation is the diffenrence between the bearing
- // and the course.
- wp0_course_deviation_deg = wp0_bearing_deg -
- wp0_course_deg;
- SG_NORMALIZE_RANGE(wp0_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(wp0_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(wp0_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.
- wp0_course_deviation_deg *= -1.0;
- SG_NORMALIZE_RANGE(wp0_course_deviation_deg, -90.0, 90.0);
- }
-
- _wp0_course_deviation_node->setDoubleValue(wp0_course_deviation_deg);
-
- // Cross track error.
- wp0_course_error_m = sin(wp0_course_deviation_deg * SG_PI / 180.0)
- * (wp0_distance);
- _wp0_course_error_nm_node->setDoubleValue(wp0_course_error_m
- * SG_METER_TO_NM);
-
-
-
- // Find the bearing and distance to waypoint 1.
- SGWayPoint wp1(wp1_longitude_deg, wp1_latitude_deg, wp1_altitude_m);
- wp1.CourseAndDistance(longitude_deg, latitude_deg, altitude_m,
- &wp1_bearing_deg, &wp1_distance);
- _wp1_distance_node->setDoubleValue(wp1_distance * SG_METER_TO_NM);
- _wp1_bearing_node->setDoubleValue(wp1_bearing_deg);
- double wp1_mag_bearing_deg = wp1_bearing_deg - magvar_deg;
- SG_NORMALIZE_RANGE(wp1_mag_bearing_deg, 0.0, 360.0);
- _wp1_mag_bearing_node->setDoubleValue(wp1_mag_bearing_deg);
- wp1_bearing_error_deg = track1_deg - wp1_bearing_deg;
- SG_NORMALIZE_RANGE(wp1_bearing_error_deg, -180.0, 180.0);
- _true_wp1_bearing_error_node->setDoubleValue(wp1_bearing_error_deg);
-
- // Estimate time to waypoint 1.
- // 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.
- if (speed_kt > 0.0 && wp1_distance > 0.0) {
- wp1_TTW = (wp1_distance * SG_METER_TO_NM) / (speed_kt / 3600);
- }
- else {
- wp1_TTW = 0.0;
- }
- unsigned int wp1_TTW_seconds = (int) (wp1_TTW + 0.5);
- if (wp1_TTW_seconds < 356400) { // That's 99 hours
- unsigned int wp1_TTW_minutes = 0;
- unsigned int wp1_TTW_hours = 0;
- char wp1_TTW_str[9];
- while (wp1_TTW_seconds >= 3600) {
- wp1_TTW_seconds -= 3600;
- wp1_TTW_hours++;
- }
- while (wp1_TTW_seconds >= 60) {
- wp1_TTW_seconds -= 60;
- wp1_TTW_minutes++;
- }
- snprintf(wp1_TTW_str, 9, "%02d:%02d:%02d",
- wp1_TTW_hours, wp1_TTW_minutes, wp1_TTW_seconds);
- _wp1_ttw_node->setStringValue(wp1_TTW_str);
- }
- else
- _wp1_ttw_node->setStringValue("--:--:--");
-
- // Course deviation is the diffenrence between the bearing
- // and the course.
- wp1_course_deviation_deg = wp1_bearing_deg - wp1_course_deg;
- SG_NORMALIZE_RANGE(wp1_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(wp1_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(wp1_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.
- wp1_course_deviation_deg *= -1.0;
- SG_NORMALIZE_RANGE(wp1_course_deviation_deg, -90.0, 90.0);
- }
-
- _wp1_course_deviation_node->setDoubleValue(wp1_course_deviation_deg);
-
- // Cross track error.
- wp1_course_error_m = sin(wp1_course_deviation_deg * SG_PI / 180.0)
- * (wp1_distance);
- _wp1_course_error_nm_node->setDoubleValue(wp1_course_error_m
- * SG_METER_TO_NM);
-
-
- // Leg course deviation is the diffenrence between the bearing
- // and the course.
- double course_deviation_deg = 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);
-
- // 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);
-
-
-
- // Tracking bug.
- double tracking_bug = _tracking_bug_node->getDoubleValue();
- double true_bug_error = tracking_bug - track1_deg;
- double magnetic_bug_error = tracking_bug - mag_track_bearing;
-
- // 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);
-
-
- // Add WP 1 to the route.
- if ( addWp->getBoolValue() )
- {
- addWp->setBoolValue(false);
-
- SGWayPoint tempWp( _wp1_longitude_node->getDoubleValue(),
- _wp1_latitude_node->getDoubleValue(),
- _wp1_altitude_node->getDoubleValue(),
- SGWayPoint::WGS84,
- _wp1_ID_node->getStringValue(),
- _wp1_name_node->getStringValue() );
-
- route->add_waypoint(tempWp);
-
- SGPropertyNode *wp =
- _route->getChild("Waypoint", route->size()-1, true);
- SGPropertyNode *id = wp->getChild("ID", 0, true);
- SGPropertyNode *name = wp->getChild("Name", 0, true);
- SGPropertyNode *lat = wp->getChild("Latitude", 0, true);
- SGPropertyNode *lon = wp->getChild("Longitude", 0, true);
- SGPropertyNode *alt = wp->getChild("Altitude", 0, true);
-
- id->setStringValue( tempWp.get_id().c_str() );
- name->setStringValue( tempWp.get_name().c_str() );
- lat->setDoubleValue( tempWp.get_target_lat() );
- lon->setDoubleValue( tempWp.get_target_lon() );
- alt->setDoubleValue( tempWp.get_target_alt() );
- }
-
- if ( popWp->getBoolValue() )
- {
- popWp->setBoolValue(false);
-
- route->delete_first();
- _route->removeChild("Waypoint", 0, false);
- }
+void
+GPS::updateTrackingBug()
+{
+ double tracking_bug = _tracking_bug_node->getDoubleValue();
+ double true_bug_error = tracking_bug - getTrueTrack();
+ double magnetic_bug_error = tracking_bug - getMagTrack();
+
+ // 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::updateWaypoints()
+{
+ double az2;
+ SGGeodesy::inverse(_indicated_pos, _wp1_position, _wp1TrueBearing, az2,_wp1DistanceM);
+}
+
+void GPS::updateReferenceNavaid(double dt)
+{
+ if (!_ref_navaid_set) {
+ _ref_navaid_elapsed += dt;
+ if (_ref_navaid_elapsed > 5.0) {
+ _ref_navaid_elapsed = 0.0;
+
+ FGPositioned::TypeFilter vorFilter(FGPositioned::VOR);
+ FGPositionedRef nav = FGPositioned::findClosest(_indicated_pos, 400.0, &vorFilter);
+ if (!nav) {
+ SG_LOG(SG_INSTR, SG_INFO, "GPS couldn't find a reference navid");
+ _ref_navaid_id_node->setStringValue("");
+ _ref_navaid_name_node->setStringValue("");
+ _ref_navaid_bearing_node->setDoubleValue(0.0);
+ _ref_navaid_mag_bearing_node->setDoubleValue(0.0);
+ _ref_navaid_distance_node->setDoubleValue(0.0);
+ _ref_navaid_frequency_node->setStringValue("");
+ } else if (nav != _ref_navaid) {
+ SG_LOG(SG_INSTR, SG_INFO, "GPS code selected new ref-navaid:" << nav->ident());
+ _listener->setGuard(true);
+ _ref_navaid_id_node->setStringValue(nav->ident().c_str());
+ _ref_navaid_name_node->setStringValue(nav->name().c_str());
+ FGNavRecord* vor = (FGNavRecord*) nav.ptr();
+ _ref_navaid_frequency_node->setDoubleValue(vor->get_freq() / 100.0);
+ _listener->setGuard(false);
+ tuneNavRadios();
+ } else {
+ // SG_LOG(SG_INSTR, SG_ALERT, "matched existing");
+ }
+
+ _ref_navaid = nav;
+ }
+ }
+
+ if (_ref_navaid) {
+ double trueCourse, distanceM, az2;
+ SGGeodesy::inverse(_indicated_pos, _ref_navaid->geod(), trueCourse, az2, distanceM);
+ _ref_navaid_distance_node->setDoubleValue(distanceM * SG_METER_TO_NM);
+ _ref_navaid_bearing_node->setDoubleValue(trueCourse);
+ _ref_navaid_mag_bearing_node->setDoubleValue(trueCourse - _magvar_node->getDoubleValue());
+ }
+}
+
+void GPS::referenceNavaidSet(const std::string& aNavaid)
+{
+ _ref_navaid = NULL;
+ // allow setting an empty string to restore normal nearest-vor selection
+ if (aNavaid.size() > 0) {
+ FGPositioned::TypeFilter vorFilter(FGPositioned::VOR);
+ _ref_navaid = FGPositioned::findClosestWithIdent(aNavaid,
+ _position.get(), &vorFilter);
+
+ if (!_ref_navaid) {
+ SG_LOG(SG_INSTR, SG_ALERT, "GPS: unknown ref navaid:" << aNavaid);
+ }
+ }
+
+ if (_ref_navaid) {
+ _ref_navaid_set = true;
+ SG_LOG(SG_INSTR, SG_INFO, "GPS code set explict ref-navaid:" << _ref_navaid->ident());
+ _ref_navaid_id_node->setStringValue(_ref_navaid->ident().c_str());
+ _ref_navaid_name_node->setStringValue(_ref_navaid->name().c_str());
+ FGNavRecord* vor = (FGNavRecord*) _ref_navaid.ptr();
+ _ref_navaid_frequency_node->setDoubleValue(vor->get_freq() / 100.0);
+ tuneNavRadios();
+ } else {
+ _ref_navaid_set = false;
+ _ref_navaid_elapsed = 9999.0; // update next tick
+ }
+}
+
+void GPS::tuneNavRadios()
+{
+ if (!_ref_navaid || !_config.tuneNavRadioToRefVor()) {
+ return;
+ }
+
+ SGPropertyNode_ptr navRadio1 = fgGetNode("/instrumentation/nav", false);
+ if (!navRadio1) {
+ return;
+ }
+
+ FGNavRecord* vor = (FGNavRecord*) _ref_navaid.ptr();
+ SGPropertyNode_ptr freqs = navRadio1->getChild("frequencies");
+ freqs->setDoubleValue("selected-mhz", vor->get_freq() / 100.0);
+}
+
+void GPS::routeActivated()
+{
+ if (_route_active_node->getBoolValue()) {
+ SG_LOG(SG_INSTR, SG_INFO, "GPS::route activated, switching to LEG mode");
+ selectLegMode();
+
+ // if we've already passed the current waypoint, sequence.
+ if (_dataValid && getWP1FromFlag()) {
+ SG_LOG(SG_INSTR, SG_INFO, "GPS::route activated, FROM wp1, sequencing");
+ _routeMgr->sequence();
+ }
+ } else if (_mode == "leg") {
+ SG_LOG(SG_INSTR, SG_INFO, "GPS::route deactivated, switching to OBS mode");
+ selectOBSMode();
+ }
+}
+
+void GPS::routeManagerSequenced()
+{
+ if (_mode != "leg") {
+ SG_LOG(SG_INSTR, SG_INFO, "GPS ignoring route sequencing, not in LEG mode");
+ return;
+ }
+
+ int index = _routeMgr->currentWaypoint(),
+ count = _routeMgr->size();
+ if ((index < 0) || (index >= count)) {
+ SG_LOG(SG_INSTR, SG_ALERT, "GPS: malformed route, index=" << index);
+ return;
+ }
+
+ SG_LOG(SG_INSTR, SG_INFO, "GPS waypoint index is now " << index);
+
+ if (index > 0) {
+ SGWayPoint wp0(_routeMgr->get_waypoint(index - 1));
+ _wp0Ident = wp0.get_id();
+ _wp0Name = wp0.get_name();
+ _wp0_position = wp0.get_target();
+
+ }
+
+ SGWayPoint wp1(_routeMgr->get_waypoint(index));
+ _wp1Ident = wp1.get_id();
+ _wp1Name = wp1.get_name();
+ _wp1_position = wp1.get_target();
+
+ _selectedCourse = getLegMagCourse();
+ wp1Changed();
+}
+
+void GPS::routeEdited()
+{
+ if (_mode != "leg") {
+ return;
+ }
+
+ SG_LOG(SG_INSTR, SG_INFO, "GPS route edited while in LEG mode, updating waypoints");
+ routeManagerSequenced();
+}
+
+void GPS::routeFinished()
+{
+ if (_mode != "leg") {
+ return;
+ }
+
+ SG_LOG(SG_INSTR, SG_INFO, "GPS route finished, reverting to OBS");
+ _mode = "obs";
+ _wp0_position = _indicated_pos;
+ wp1Changed();
+}
+
+void GPS::updateTurn()
+{
+ bool printProgress = false;
+
+ if (_computeTurnData) {
+ if (_last_speed_kts < 60) {
+ // need valid leg course and sensible ground speed to compute the turn
+ return;
+ }
+
+ computeTurnData();
+ printProgress = true;
+ }
+
+ if (!_anticipateTurn) {
+ updateOverflight();
+ return;
+ }
+
+ updateTurnData();
+ // find bearing to turn centre
+ double bearing, az2, distanceM;
+ SGGeodesy::inverse(_indicated_pos, _turnCentre, bearing, az2, distanceM);
+ double progress = computeTurnProgress(bearing);
+
+ if (printProgress) {
+ SG_LOG(SG_INSTR, SG_INFO,"turn progress=" << progress);
+ }
+
+ if (!_inTurn && (progress > 0.0)) {
+ beginTurn();
+ }
+
+ if (_inTurn && !_turnSequenced && (progress > 0.5)) {
+ _turnSequenced = true;
+ SG_LOG(SG_INSTR, SG_INFO, "turn passed midpoint, sequencing");
+ _routeMgr->sequence();
+ }
+
+ if (_inTurn && (progress >= 1.0)) {
+ endTurn();
+ }
+
+ if (_inTurn) {
+ // drive deviation and desired course
+ double desiredCourse = bearing - copysign(90, _turnAngle);
+ SG_NORMALIZE_RANGE(desiredCourse, 0.0, 360.0);
+ double deviationNm = (distanceM * SG_METER_TO_NM) - _turnRadius;
+ double deviationDeg = desiredCourse - getMagTrack();
+ deviationNm = copysign(deviationNm, deviationDeg);
+ // FXIME
+ //_wp1_course_deviation_node->setDoubleValue(deviationDeg);
+ //_wp1_course_error_nm_node->setDoubleValue(deviationNm);
+ //_cdiDeflectionNode->setDoubleValue(deviationDeg);
+ }
+}
+
+void GPS::updateOverflight()
+{
+ if ((_wp1DistanceM * SG_METER_TO_NM) > _config.overflightArmDistanceNm()) {
+ return;
+ }
+
+ if (getWP1ToFlag()) {
+ return; // still heading towards the WP
+ }
+
+ if (_mode == "dto") {
+ SG_LOG(SG_INSTR, SG_INFO, "GPS DTO reached destination point");
+
+ // check for wp1 being on active route - resume leg mode
+ if (_routeMgr->isRouteActive()) {
+ int index = _routeMgr->findWaypoint(_wp1_position);
+ if (index >= 0) {
+ SG_LOG(SG_INSTR, SG_INFO, "GPS DTO, resuming LEG mode at wp:" << index);
+ _mode = "leg";
+ _routeMgr->jumpToIndex(index);
+ }
+ }
+ } else if (_mode == "leg") {
+ SG_LOG(SG_INSTR, SG_INFO, "GPS doing overflight sequencing");
+ _routeMgr->sequence();
+ } else if (_mode == "obs") {
+ // nothing to do here, TO/FROM will update but that's fine
+ }
+
+ _computeTurnData = true;
+}
+
+void GPS::beginTurn()
+{
+ _inTurn = true;
+ _turnSequenced = false;
+ SG_LOG(SG_INSTR, SG_INFO, "begining turn");
+}
+
+void GPS::endTurn()
+{
+ _inTurn = false;
+ SG_LOG(SG_INSTR, SG_INFO, "ending turn");
+ _computeTurnData = true;
+}
+
+double GPS::computeTurnProgress(double aBearing) const
+{
+ double startBearing = _turnStartBearing + copysign(90, _turnAngle);
+ return (aBearing - startBearing) / _turnAngle;
+}
+
+void GPS::computeTurnData()
+{
+ _computeTurnData = false;
+ if (_mode != "leg") { // and approach modes in the future
+ _anticipateTurn = false;
+ return;
+ }
+
+ int curIndex = _routeMgr->currentWaypoint();
+ if ((curIndex + 1) >= _routeMgr->size()) {
+ _anticipateTurn = false;
+ return;
+ }
+
+ if (!_config.turnAnticipationEnabled()) {
+ _anticipateTurn = false;
+ return;
+ }
+
+ _turnStartBearing = _selectedCourse;
+// compute next leg course
+ SGWayPoint wp1(_routeMgr->get_waypoint(curIndex)),
+ wp2(_routeMgr->get_waypoint(curIndex + 1));
+ double crs, dist;
+ wp2.CourseAndDistance(wp1, &crs, &dist);
+
+
+// compute offset bearing
+ _turnAngle = crs - _turnStartBearing;
+ SG_NORMALIZE_RANGE(_turnAngle, -180.0, 180.0);
+ double median = _turnStartBearing + (_turnAngle * 0.5);
+ double offsetBearing = median + copysign(90, _turnAngle);
+ SG_NORMALIZE_RANGE(offsetBearing, 0.0, 360.0);
+
+ SG_LOG(SG_INSTR, SG_INFO, "GPS computeTurnData: in=" << _turnStartBearing <<
+ ", out=" << crs << "; turnAngle=" << _turnAngle << ", median=" << median
+ << ", offset=" << offsetBearing);
+
+ SG_LOG(SG_INSTR, SG_INFO, "next leg is now:" << wp1.get_id() << "->" << wp2.get_id());
+
+ _turnPt = _wp1_position;
+ _anticipateTurn = true;
+}
+
+void GPS::updateTurnData()
+{
+ // depends on ground speed, so needs to be updated per-frame
+ _turnRadius = computeTurnRadiusNm(_last_speed_kts);
+
+ // compute the turn centre, based on the turn radius.
+ // key thing is to understand that we're working a right-angle triangle,
+ // where the right-angle is the point we start the turn. From that point,
+ // one side is the inbound course to the turn pt, and the other is the
+ // perpendicular line, of length 'r', to the turn centre.
+ // the triangle's hypotenuse, which we need to find, is the distance from the
+ // turn pt to the turn center (in the direction of the offset bearing)
+ // note that d - _turnRadius tell us how much we're 'cutting' the corner.
+
+ double halfTurnAngle = fabs(_turnAngle * 0.5) * SG_DEGREES_TO_RADIANS;
+ double d = _turnRadius / cos(halfTurnAngle);
+
+ // SG_LOG(SG_INSTR, SG_INFO, "turnRadius=" << _turnRadius << ", d=" << d
+ // << " (cut distance=" << d - _turnRadius << ")");
+
+ double median = _turnStartBearing + (_turnAngle * 0.5);
+ double offsetBearing = median + copysign(90, _turnAngle);
+ SG_NORMALIZE_RANGE(offsetBearing, 0.0, 360.0);
+
+ double az2;
+ SGGeodesy::direct(_turnPt, offsetBearing, d * SG_NM_TO_METER, _turnCentre, az2);
+}
+
+double GPS::computeTurnRadiusNm(double aGroundSpeedKts) const
+{
+ // turn time is seconds to execute a 360 turn.
+ double turnTime = 360.0 / _config.turnRateDegSec();
+
+ // c is ground distance covered in that time (circumference of the circle)
+ double c = turnTime * (aGroundSpeedKts / 3600.0); // convert knts to nm/sec
+
+ // divide by 2PI to go from circumference -> radius
+ return c / (2 * M_PI);
+}
+
+void GPS::updateRouteData()
+{
+ double totalDistance = _wp1DistanceM * SG_METER_TO_NM;
+ // walk all waypoints from wp2 to route end, and sum
+ for (int i=_routeMgr->currentWaypoint()+1; i<_routeMgr->size(); ++i) {
+ totalDistance += _routeMgr->get_waypoint(i).get_distance();
+ }
+
+ _routeDistanceNm->setDoubleValue(totalDistance * SG_METER_TO_NM);
+ if (_last_speed_kts > 1.0) {
+ double TTW = ((totalDistance * SG_METER_TO_NM) / _last_speed_kts) * 3600.0;
+ _routeETE->setStringValue(makeTTWString(TTW));
+ }
+}
+
+void GPS::driveAutopilot()
+{
+ if (!_config.driveAutopilot() || !_realismSimpleGps->getBoolValue()) {
+ return;
+ }
+
+ // FIXME: we want to set desired track, not heading, here
+ _apTrueHeading->setDoubleValue(getWP1Bearing());
+}
+
+void GPS::wp1Changed()
+{
+ // update external HSI/CDI/NavDisplay/PFD/etc
+ _config.setExternalCourse(getLegMagCourse());
+
+ if (!_config.driveAutopilot()) {
+ return;
+ }
+
+ double altFt = _wp1_position.getElevationFt();
+ if (altFt < -9990.0) {
+ _apTargetAltitudeFt->setDoubleValue(0.0);
+ } else {
+ _apTargetAltitudeFt->setDoubleValue(altFt);
+ }
+}