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

@@ -139,8 +139,11 @@ void FGRouteMgr::init() {
   airborne = fgGetNode(RM "airborne", true);
   airborne->setBoolValue(false);
     
-  currentWp = fgGetNode(RM "current-wp", true);
+  _edited = fgGetNode(RM "signals/edited", true);
-  currentWp->setIntValue(_route->current_index());
+  _finished = fgGetNode(RM "signals/finished", true);
+  
+  rm->tie("current-wp", SGRawValueMethods<FGRouteMgr, int>
+    (*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 );
+  _route->add_waypoint( wp, n );
-    update_mirror();
+    
+  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 ) {
+    return SGWayPoint();
+  }
+  
+  if ( n < 0 ) {
+    n = _route->size() - 1;
+  }
+  
+  if (_route->current_index() > n) {
+    _route->set_current(_route->current_index() - 1);
+  }
 
-    if ( _route->size() > 0 ) {
+  SGWayPoint wp = _route->get_waypoint(n);
-        if ( n < 0 )
+  _route->delete_waypoint(n);
-            n = _route->size() - 1;
+    
-        wp = _route->get_waypoint(n);
+  update_mirror();
-        _route->delete_waypoint(n);
+  _edited->fireValueChanged();
-    }
+  checkFinished();
-
+  
-    update_mirror();
+  return wp;
-    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()) {
+  if (checkFinished()) {
-    SG_LOG(SG_AUTOPILOT, SG_INFO, "reached end of active route");
-    // what now?
-    active->setBoolValue(false);
     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);

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();

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);
-
+  _routeEditedSignal = fgGetNode("/autopilot/route-manager/signals/edited", true);
-  // disable auto-sequencing in the route manager; we'll deal with it
+  _routeFinishedSignal = fgGetNode("/autopilot/route-manager/signals/finished", true);
-  // ourselves using turn anticipation
+  
-  SGPropertyNode_ptr autoSeq = fgGetNode("/autopilot/route-manager/auto-sequence", true);
-  autoSeq->setBoolValue(false);
-
 // 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<GPS,double>
     (*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);
   }
 }
 

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;

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)

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;