From a865555fedfa16f4b1a1330b032d3ff31294b2f7 Mon Sep 17 00:00:00 2001 From: jmt Date: Wed, 14 Oct 2009 06:50:06 +0000 Subject: [PATCH] Route-manager: remove any interaction with the autopilot, or internal sequencing; assume the GPS always handles this (it should, now). Also fix waypoint altitude handling, which was broken. If not altitude is supplied for a waypoint, default to the requested cruise altitude. --- src/Autopilot/route_mgr.cxx | 91 +++++++------------------------------ src/Autopilot/route_mgr.hxx | 18 +------- 2 files changed, 19 insertions(+), 90 deletions(-) diff --git a/src/Autopilot/route_mgr.cxx b/src/Autopilot/route_mgr.cxx index 1ea1e4f55..9e8d3c23a 100644 --- a/src/Autopilot/route_mgr.cxx +++ b/src/Autopilot/route_mgr.cxx @@ -35,6 +35,8 @@ #include "route_mgr.hxx" +#include + #include #include @@ -64,11 +66,8 @@ static double get_ground_speed() { FGRouteMgr::FGRouteMgr() : _route( new SGRoute ), - _autoSequence(true), - _driveAutopilot(true), input(fgGetNode( RM "input", true )), - mirror(fgGetNode( RM "route", true )), - altitude_set( false ) + mirror(fgGetNode( RM "route", true )) { listener = new InputListener(this); input->setStringValue(""); @@ -91,14 +90,7 @@ void FGRouteMgr::init() { lat = fgGetNode( "/position/latitude-deg", true ); alt = fgGetNode( "/position/altitude-ft", true ); magvar = fgGetNode("/environment/magnetic-variation-deg", true); - -// autopilot drive properties - _apTargetAltitudeFt = fgGetNode( "/autopilot/settings/target-altitude-ft", true ); - _apAltitudeLock = fgGetNode( "/autopilot/locks/altitude", true ); - _apTrueHeading = fgGetNode( "/autopilot/settings/true-heading-deg", true ); - rm->tie("drive-autopilot", SGRawValuePointer(&_driveAutopilot)); - _driveAutopilot = false; - + departure = fgGetNode(RM "departure", true); // init departure information from current location SGGeod pos = SGGeod::fromDegFt(lon->getDoubleValue(), lat->getDoubleValue(), alt->getDoubleValue()); @@ -126,10 +118,12 @@ void FGRouteMgr::init() { alternate->getChild("runway", 0, true); cruise = fgGetNode(RM "cruise", true); - cruise->getChild("altitude", 0, true); + cruise->getChild("altitude-ft", 0, true); + cruise->setDoubleValue("altitude-ft", 10000.0); cruise->getChild("flight-level", 0, true); - cruise->getChild("speed", 0, true); - + cruise->getChild("speed-kts", 0, true); + cruise->setDoubleValue("speed-kts", 160.0); + totalDistance = fgGetNode(RM "total-distance", true); totalDistance->setDoubleValue(0.0); @@ -144,9 +138,7 @@ void FGRouteMgr::init() { airborne = fgGetNode(RM "airborne", true); airborne->setBoolValue(false); - - rm->tie("auto-sequence", SGRawValuePointer(&_autoSequence)); - + currentWp = fgGetNode(RM "current-wp", true); currentWp->setIntValue(_route->current_index()); @@ -190,31 +182,6 @@ void FGRouteMgr::postinit() { void FGRouteMgr::bind() { } void FGRouteMgr::unbind() { } -void FGRouteMgr::updateTargetAltitude() { - if (_route->current_index() == _route->size()) { - altitude_set = false; - return; - } - - SGWayPoint wp = _route->get_current(); - if (wp.get_target().getElevationM() < -9990.0) { - altitude_set = false; - return; - } - - altitude_set = true; - - if (!_driveAutopilot) { - return; - } - - _apTargetAltitudeFt->setDoubleValue(wp.get_target().getElevationFt()); - - if ( !near_ground() ) { - _apAltitudeLock->setStringValue( "altitude-hold" ); - } -} - bool FGRouteMgr::isRouteActive() const { return active->getBoolValue(); @@ -254,19 +221,6 @@ void FGRouteMgr::update( double dt ) { wp.CourseAndDistance( lon->getDoubleValue(), lat->getDoubleValue(), alt->getDoubleValue(), &wp_course, &wp_distance ); - if (_autoSequence && (wp_distance < 2000.0)) { - double courseDeviation = inboundCourse - wp_course; - SG_NORMALIZE_RANGE(courseDeviation, -180.0, 180.0); - if (fabs(courseDeviation) < 90.0) { - SG_LOG( SG_AUTOPILOT, SG_INFO, "route manager doing sequencing"); - sequence(); - } - } - - if (_driveAutopilot) { - _apTrueHeading->setDoubleValue(wp_course); - } - // update wp0 / wp1 / wp-last for legacy users wp0->setDoubleValue("dist", wp_distance * SG_METER_TO_NM); wp_course -= magvar->getDoubleValue(); // expose magnetic bearing @@ -325,9 +279,6 @@ void FGRouteMgr::setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDi void FGRouteMgr::add_waypoint( const SGWayPoint& wp, int n ) { _route->add_waypoint( wp, n ); update_mirror(); - if ((n==0) || (_route->size() == 1)) { - updateTargetAltitude(); - } } @@ -341,7 +292,6 @@ SGWayPoint FGRouteMgr::pop_waypoint( int n ) { _route->delete_waypoint(n); } - updateTargetAltitude(); update_mirror(); return wp; } @@ -368,15 +318,11 @@ void FGRouteMgr::new_waypoint( const string& target, int n ) { SGWayPoint* FGRouteMgr::make_waypoint(const string& tgt ) { - string target = tgt; + string target(boost::to_upper_copy(tgt)); - // make upper case - for (unsigned int i = 0; i < target.size(); i++) { - target[i] = toupper(target[i]); - } - // extract altitude - double alt = -9999.0; + double alt = cruise->getDoubleValue("altitude-ft") * SG_FEET_TO_METER; + size_t pos = target.find( '@' ); if ( pos != string::npos ) { alt = atof( target.c_str() + pos + 1 ); @@ -413,9 +359,10 @@ SGWayPoint* FGRouteMgr::make_waypoint(const string& tgt ) { return NULL; } + SGGeod geod = SGGeod::fromGeodM(p->geod(), alt); if (pieces.size() == 1) { // simple case - return new SGWayPoint(p->geod(), target, p->name()); + return new SGWayPoint(geod, target, p->name()); } if (pieces.size() == 3) { @@ -425,13 +372,10 @@ SGWayPoint* FGRouteMgr::make_waypoint(const string& tgt ) { az2; radial += magvar->getDoubleValue(); // convert to true bearing SGGeod offsetPos; - SGGeodesy::direct(p->geod(), radial, distanceNm * SG_NM_TO_METER, offsetPos, az2); + SGGeodesy::direct(geod, radial, distanceNm * SG_NM_TO_METER, offsetPos, az2); + offsetPos.setElevationM(alt); SG_LOG(SG_AUTOPILOT, SG_INFO, "final offset radial is " << radial); - - if (alt > -9990) { - offsetPos.setElevationM(alt); - } // otherwise use the elevation of navid return new SGWayPoint(offsetPos, p->ident() + pieces[2], target); } @@ -645,7 +589,6 @@ void FGRouteMgr::currentWaypointChanged() } currentWp->setIntValue(_route->current_index()); - updateTargetAltitude(); SG_LOG(SG_AUTOPILOT, SG_INFO, "route manager, current-wp is now " << _route->current_index()); } diff --git a/src/Autopilot/route_mgr.hxx b/src/Autopilot/route_mgr.hxx index 8799ce160..cb79cb07f 100644 --- a/src/Autopilot/route_mgr.hxx +++ b/src/Autopilot/route_mgr.hxx @@ -44,21 +44,14 @@ private: SGRoute* _route; time_t _takeoffTime; time_t _touchdownTime; - bool _autoSequence; // true if we are doing internal sequencing - // false if other code (FMS/GPS) is managing sequencing - + // automatic inputs SGPropertyNode_ptr lon; SGPropertyNode_ptr lat; SGPropertyNode_ptr alt; SGPropertyNode_ptr magvar; - // automatic outputs - bool _driveAutopilot; - SGPropertyNode_ptr _apTargetAltitudeFt; - SGPropertyNode_ptr _apAltitudeLock; - SGPropertyNode_ptr _apTrueHeading; - + // automatic outputs SGPropertyNode_ptr departure; ///< departure airport information SGPropertyNode_ptr destination; ///< destination airport information SGPropertyNode_ptr alternate; ///< alternate airport information @@ -94,7 +87,6 @@ private: InputListener *listener; SGPropertyNode_ptr mirror; - bool altitude_set; /** * Create a SGWayPoint from a string in the following format: @@ -111,12 +103,6 @@ private: void currentWaypointChanged(); - /** - * Helper to update the target_altitude_ft and altitude_set flag when wp0 - * changes - */ - void updateTargetAltitude(); - /** * Parse a route/wp node (from a saved, property-lsit formatted route) */ -- 2.39.5