From afb1e7ffe99c2655b8c28a65b58530801e37275c Mon Sep 17 00:00:00 2001 From: jmt Date: Thu, 15 Oct 2009 21:30:10 +0000 Subject: [PATCH] Further GPS and route manager behavioural fixes * When the nav-radio is slaved, calculated radial/target-hdg-deg (needed by some autopilot logic) * Handle editing (including deletion) of route waypoints correctly, including deleting the active waypoint * Add a signal to the route manager when the last wpt is reached, and use it in the GPS to revert to OBS mode. * Change the altitude handling to use the specified cruise altitude * Fix a bug where autopilot/locks/altitude was treated as a boolean --- src/Autopilot/route_mgr.cxx | 97 ++++++++++++++++---------------- src/Autopilot/route_mgr.hxx | 18 +++++- src/Instrumentation/gps.cxx | 45 ++++++++++++--- src/Instrumentation/gps.hxx | 6 +- src/Instrumentation/navradio.cxx | 7 ++- src/Instrumentation/navradio.hxx | 3 +- 6 files changed, 111 insertions(+), 65 deletions(-) diff --git a/src/Autopilot/route_mgr.cxx b/src/Autopilot/route_mgr.cxx index 9e8d3c23a..228108e9c 100644 --- a/src/Autopilot/route_mgr.cxx +++ b/src/Autopilot/route_mgr.cxx @@ -139,8 +139,11 @@ void FGRouteMgr::init() { airborne = fgGetNode(RM "airborne", true); airborne->setBoolValue(false); - currentWp = fgGetNode(RM "current-wp", true); - currentWp->setIntValue(_route->current_index()); + _edited = fgGetNode(RM "signals/edited", true); + _finished = fgGetNode(RM "signals/finished", true); + + rm->tie("current-wp", SGRawValueMethods + (*this, &FGRouteMgr::currentWaypoint, &FGRouteMgr::jumpToIndex)); // temporary distance / eta calculations, for backward-compatability wp0 = fgGetNode(RM "wp", 0, true); @@ -277,23 +280,38 @@ void FGRouteMgr::setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDi } void FGRouteMgr::add_waypoint( const SGWayPoint& wp, int n ) { - _route->add_waypoint( wp, n ); - update_mirror(); + _route->add_waypoint( wp, n ); + + if (_route->current_index() > n) { + _route->set_current(_route->current_index() + 1); + } + + update_mirror(); + _edited->fireValueChanged(); } SGWayPoint FGRouteMgr::pop_waypoint( int n ) { - SGWayPoint wp; - - if ( _route->size() > 0 ) { - if ( n < 0 ) - n = _route->size() - 1; - wp = _route->get_waypoint(n); - _route->delete_waypoint(n); - } + if ( _route->size() <= 0 ) { + return SGWayPoint(); + } + + if ( n < 0 ) { + n = _route->size() - 1; + } + + if (_route->current_index() > n) { + _route->set_current(_route->current_index() - 1); + } - update_mirror(); - return wp; + SGWayPoint wp = _route->get_waypoint(n); + _route->delete_waypoint(n); + + update_mirror(); + _edited->fireValueChanged(); + checkFinished(); + + return wp; } @@ -310,10 +328,6 @@ void FGRouteMgr::new_waypoint( const string& target, int n ) { add_waypoint( *wp, n ); delete wp; - - if ( !near_ground() ) { - fgSetString( "/autopilot/locks/heading", "true-heading-hold" ); - } } @@ -420,20 +434,6 @@ void FGRouteMgr::update_mirror() { mirror->setIntValue("num", _route->size()); } - -bool FGRouteMgr::near_ground() { - SGPropertyNode *gear = fgGetNode( "/gear/gear/wow", false ); - if ( !gear || gear->getType() == simgear::props::NONE ) - return fgGetBool( "/sim/presets/onground", true ); - - if ( fgGetDouble("/position/altitude-agl-ft", 300.0) - < fgGetDouble("/autopilot/route-manager/min-lock-altitude-agl-ft") ) - return true; - - return gear->getBoolValue(); -} - - // command interface /autopilot/route-manager/input: // // @CLEAR ... clear route @@ -544,10 +544,7 @@ void FGRouteMgr::sequence() return; } - if (_route->current_index() == _route->size()) { - SG_LOG(SG_AUTOPILOT, SG_INFO, "reached end of active route"); - // what now? - active->setBoolValue(false); + if (checkFinished()) { return; } @@ -555,6 +552,19 @@ void FGRouteMgr::sequence() currentWaypointChanged(); } +bool FGRouteMgr::checkFinished() +{ + int lastWayptIndex = _route->size() - 1; + if (_route->current_index() < lastWayptIndex) { + return false; + } + + SG_LOG(SG_AUTOPILOT, SG_INFO, "reached end of active route"); + _finished->fireValueChanged(); + active->setBoolValue(false); + return true; +} + void FGRouteMgr::jumpToIndex(int index) { if (!active->getBoolValue()) { @@ -588,7 +598,6 @@ void FGRouteMgr::currentWaypointChanged() wp1->getChild("id")->setStringValue(""); } - currentWp->setIntValue(_route->current_index()); SG_LOG(SG_AUTOPILOT, SG_INFO, "route manager, current-wp is now " << _route->current_index()); } @@ -706,7 +715,7 @@ void FGRouteMgr::parseRouteWaypoint(SGPropertyNode* aWP) } SGPropertyNode_ptr altProp = aWP->getChild("altitude-ft"); - double alt = -9999.0; + double alt = cruise->getDoubleValue("altitude-ft") * SG_FEET_TO_METER; if (altProp) { alt = altProp->getDoubleValue(); } @@ -714,10 +723,6 @@ void FGRouteMgr::parseRouteWaypoint(SGPropertyNode* aWP) string ident(aWP->getStringValue("ident")); if (aWP->hasChild("longitude-deg")) { // explicit longitude/latitude - if (alt < -9990.0) { - alt = 0.0; // don't export wyapoints with invalid altitude - } - SGWayPoint swp(aWP->getDoubleValue("longitude-deg"), aWP->getDoubleValue("latitude-deg"), alt, SGWayPoint::WGS84, ident, aWP->getStringValue("name")); @@ -740,10 +745,6 @@ void FGRouteMgr::parseRouteWaypoint(SGPropertyNode* aWP) SGGeodesy::direct(p->geod(), radialDeg, offsetNm * SG_NM_TO_METER, pos, az2); } - if (alt < -9990.0) { - alt = p->elevation(); - } - SGWayPoint swp(pos.getLongitudeDeg(), pos.getLatitudeDeg(), alt, SGWayPoint::WGS84, ident, ""); add_waypoint(swp); @@ -754,10 +755,6 @@ void FGRouteMgr::parseRouteWaypoint(SGPropertyNode* aWP) throw sg_io_exception("bad route file, unknown waypoint:" + ident); } - if (alt < -9990.0) { - alt = p->elevation(); - } - SGWayPoint swp(p->longitude(), p->latitude(), alt, SGWayPoint::WGS84, p->ident(), p->name()); add_waypoint(swp); diff --git a/src/Autopilot/route_mgr.hxx b/src/Autopilot/route_mgr.hxx index cb79cb07f..2cc6b42f9 100644 --- a/src/Autopilot/route_mgr.hxx +++ b/src/Autopilot/route_mgr.hxx @@ -63,7 +63,6 @@ private: SGPropertyNode_ptr active; SGPropertyNode_ptr airborne; - SGPropertyNode_ptr currentWp; SGPropertyNode_ptr wp0; SGPropertyNode_ptr wp1; @@ -72,6 +71,16 @@ private: SGPropertyNode_ptr _pathNode; + /** + * Signal property to notify people that the route was edited + */ + SGPropertyNode_ptr _edited; + + /** + * Signal property to notify when the last waypoint is reached + */ + SGPropertyNode_ptr _finished; + void setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDistance); class InputListener : public SGPropertyChangeListener { @@ -99,7 +108,6 @@ private: void update_mirror(); - bool near_ground(); void currentWaypointChanged(); @@ -107,6 +115,12 @@ private: * Parse a route/wp node (from a saved, property-lsit formatted route) */ void parseRouteWaypoint(SGPropertyNode* aWP); + + /** + * Check if we've reached the final waypoint. + * Returns true if we have. + */ + bool checkFinished(); public: FGRouteMgr(); diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx index 76a65004f..b626e4dc1 100644 --- a/src/Instrumentation/gps.cxx +++ b/src/Instrumentation/gps.cxx @@ -135,6 +135,10 @@ public: _gps->routeActivated(); } else if (prop == _gps->_ref_navaid_id_node) { _gps->referenceNavaidSet(prop->getStringValue("")); + } else if (prop == _gps->_routeEditedSignal) { + _gps->routeEdited(); + } else if (prop == _gps->_routeFinishedSignal) { + _gps->routeFinished(); } _guard = false; @@ -412,12 +416,9 @@ GPS::init () // should these move to the route manager? _routeDistanceNm = node->getChild("route-distance-nm", 0, true); _routeETE = node->getChild("ETE", 0, true); - - // disable auto-sequencing in the route manager; we'll deal with it - // ourselves using turn anticipation - SGPropertyNode_ptr autoSeq = fgGetNode("/autopilot/route-manager/auto-sequence", true); - autoSeq->setBoolValue(false); - + _routeEditedSignal = fgGetNode("/autopilot/route-manager/signals/edited", true); + _routeFinishedSignal = fgGetNode("/autopilot/route-manager/signals/finished", true); + // add listener to various things _listener = new GPSListener(this); _route_current_wp_node = fgGetNode("/autopilot/route-manager/current-wp", true); @@ -425,14 +426,20 @@ GPS::init () _route_active_node = fgGetNode("/autopilot/route-manager/active", true); _route_active_node->addChangeListener(_listener); _ref_navaid_id_node->addChangeListener(_listener); - + _routeEditedSignal->addChangeListener(_listener); + _routeFinishedSignal->addChangeListener(_listener); + // navradio slaving properties node->tie("cdi-deflection", SGRawValueMethods (*this, &GPS::getCDIDeflection)); SGPropertyNode* toFlag = node->getChild("to-flag", 0, true); toFlag->alias(wp1_node->getChild("to-flag")); + + SGPropertyNode* fromFlag = node->getChild("from-flag", 0, true); + fromFlag->alias(wp1_node->getChild("from-flag")); + // autopilot drive properties _apTrueHeading = fgGetNode("/autopilot/settings/true-heading-deg",true); _apTargetAltitudeFt = fgGetNode("/autopilot/settings/target-altitude-ft", true); @@ -747,6 +754,28 @@ void GPS::routeManagerSequenced() 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; @@ -977,10 +1006,8 @@ void GPS::wp1Changed() double altFt = _wp1_position.getElevationFt(); if (altFt < -9990.0) { _apTargetAltitudeFt->setDoubleValue(0.0); - _apAltitudeLock->setBoolValue(false); } else { _apTargetAltitudeFt->setDoubleValue(altFt); - _apAltitudeLock->setBoolValue(true); } } diff --git a/src/Instrumentation/gps.hxx b/src/Instrumentation/gps.hxx index 45ea1c270..124bfd867 100644 --- a/src/Instrumentation/gps.hxx +++ b/src/Instrumentation/gps.hxx @@ -206,6 +206,8 @@ private: void routeActivated(); void routeManagerSequenced(); + void routeEdited(); + void routeFinished(); void updateTurn(); void updateOverflight(); @@ -329,7 +331,9 @@ private: SGPropertyNode_ptr _route_current_wp_node; SGPropertyNode_ptr _routeDistanceNm; SGPropertyNode_ptr _routeETE; - + SGPropertyNode_ptr _routeEditedSignal; + SGPropertyNode_ptr _routeFinishedSignal; + double _selectedCourse; bool _last_valid; diff --git a/src/Instrumentation/navradio.cxx b/src/Instrumentation/navradio.cxx index aa238d3a9..2a77d0b25 100644 --- a/src/Instrumentation/navradio.cxx +++ b/src/Instrumentation/navradio.cxx @@ -247,7 +247,8 @@ FGNavRadio::init () gps_has_gs_node = fgGetNode("/instrumentation/gps/has-gs", true); gps_course_node = fgGetNode("/instrumentation/gps/selected-course-deg", true); gps_xtrack_error_nm_node = fgGetNode("/instrumentation/gps/wp/wp[1]/course-error-nm", true); - + _magvarNode = fgGetNode("/environment/magnetic-variation-deg", true); + std::ostringstream temp; temp << _name << "nav-ident" << _num; nav_fx_name = temp.str(); @@ -621,7 +622,9 @@ void FGNavRadio::updateGPSSlaved() _cdiCrossTrackErrorM = gps_xtrack_error_nm_node->getDoubleValue() * SG_NM_TO_METER; _gsNeedleDeflection = 0.0; // FIXME, supply this - //sel_radial_node->setDoubleValue(gps_course_node->getDoubleValue()); + double trtrue = gps_course_node->getDoubleValue() + _magvarNode->getDoubleValue(); + SG_NORMALIZE_RANGE(trtrue, 0.0, 360.0); + target_radial_true_node->setDoubleValue( trtrue ); } void FGNavRadio::updateCDI(double dt) diff --git a/src/Instrumentation/navradio.hxx b/src/Instrumentation/navradio.hxx index 79a2d87a8..0619962c0 100644 --- a/src/Instrumentation/navradio.hxx +++ b/src/Instrumentation/navradio.hxx @@ -115,7 +115,8 @@ class FGNavRadio : public SGSubsystem SGPropertyNode_ptr gps_has_gs_node; SGPropertyNode_ptr gps_course_node; SGPropertyNode_ptr gps_xtrack_error_nm_node; - + SGPropertyNode_ptr _magvarNode; + // internal (private) values int play_count; -- 2.39.5