Route tweaks, especially in-flight activation and deactivation

* Don't require a valid departure and destination airport
* Handle in-air route activation cleanly
* Handle end-of-route situation cleanly

diff --git a/src/Autopilot/route_mgr.cxx b/src/Autopilot/route_mgr.cxx
index 0ef72afb53d23bdeebbb332e0e85440bd2f95e13..11cf31d26d08ef938c3f0eb15cd22c0ff481df17 100644 (file)
--- a/src/Autopilot/route_mgr.cxx
+++ b/src/Autopilot/route_mgr.cxx
@@ -216,12 +216,8 @@ void FGRouteMgr::update( double dt ) {
     }
     
   // basic course/distance information
-    double inboundCourse, dummy, wp_course, wp_distance;
+    double wp_course, wp_distance;
     SGWayPoint wp = _route->get_current();
-  
-    wp.CourseAndDistance(_route->get_waypoint(_route->current_index() - 1), 
-      &inboundCourse, &dummy);
-    
     wp.CourseAndDistance( lon->getDoubleValue(), lat->getDoubleValue(),
                           alt->getDoubleValue(), &wp_course, &wp_distance );
 
@@ -478,49 +474,39 @@ void FGRouteMgr::InputListener::valueChanged(SGPropertyNode *prop)
 bool FGRouteMgr::activate()
 {
   const FGAirport* depApt = fgFindAirportID(departure->getStringValue("airport"));
-  if (!depApt) {
-    SG_LOG(SG_AUTOPILOT, SG_ALERT, 
-      "unable to activate route: departure airport is invalid:" 
-        << departure->getStringValue("airport") );
-    return false;
-  }
-  
-  string runwayId(departure->getStringValue("runway"));
-  FGRunway* runway = NULL;
-  if (depApt->hasRunwayWithIdent(runwayId)) {
-    runway = depApt->getRunwayByIdent(runwayId);
-  } else {
-    SG_LOG(SG_AUTOPILOT, SG_INFO, 
-      "route-manager, departure runway not found:" << runwayId);
-    runway = depApt->getActiveRunwayForUsage();
+  if (depApt) {
+    string runwayId(departure->getStringValue("runway"));
+    FGRunway* runway = NULL;
+    if (depApt->hasRunwayWithIdent(runwayId)) {
+      runway = depApt->getRunwayByIdent(runwayId);
+    } else {
+      SG_LOG(SG_AUTOPILOT, SG_INFO, 
+        "route-manager, departure runway not found:" << runwayId);
+      runway = depApt->getActiveRunwayForUsage();
+    }
+    
+    SGWayPoint swp(runway->threshold(), 
+      depApt->ident() + "-" + runway->ident(), runway->name());
+    add_waypoint(swp, 0);
   }
   
-  SGWayPoint swp(runway->threshold(), 
-    depApt->ident() + "-" + runway->ident(), runway->name());
-  add_waypoint(swp, 0);
-  
   const FGAirport* destApt = fgFindAirportID(destination->getStringValue("airport"));
-  if (!destApt) {
-    SG_LOG(SG_AUTOPILOT, SG_ALERT, 
-      "unable to activate route: destination airport is invalid:" 
-        << destination->getStringValue("airport") );
-    return false;
+  if (destApt) {
+    string runwayId = (destination->getStringValue("runway"));
+    if (destApt->hasRunwayWithIdent(runwayId)) {
+      FGRunway* runway = destApt->getRunwayByIdent(runwayId);
+      SGWayPoint swp(runway->end(), 
+        destApt->ident() + "-" + runway->ident(), runway->name());
+      add_waypoint(swp);
+    } else {
+      // quite likely, since destination runway may not be known until enroute
+      // probably want a listener on the 'destination' node to allow an enroute
+      // update
+      add_waypoint(SGWayPoint(destApt->geod(), destApt->ident(), destApt->name()));
+    }
   }
 
-  runwayId = (destination->getStringValue("runway"));
-  if (destApt->hasRunwayWithIdent(runwayId)) {
-    FGRunway* runway = destApt->getRunwayByIdent(runwayId);
-    SGWayPoint swp(runway->end(), 
-      destApt->ident() + "-" + runway->ident(), runway->name());
-    add_waypoint(swp);
-  } else {
-    // quite likely, since destination runway may not be known until enroute
-    // probably want a listener on the 'destination' node to allow an enroute
-    // update
-    add_waypoint(SGWayPoint(destApt->geod(), destApt->ident(), destApt->name()));
-  }
-  
-  _route->set_current(1);
+  _route->set_current(0);
   
   double routeDistanceNm = _route->total_distance() * SG_METER_TO_NM;
   totalDistance->setDoubleValue(routeDistanceNm);

diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx
index 731e8f486babc30e2538adae1e31743b1950baec..43646cbf0e9e2ab815a49058220fd0d1d3cda8b7 100644 (file)
--- a/src/Instrumentation/gps.cxx
+++ b/src/Instrumentation/gps.cxx
@@ -723,6 +723,12 @@ 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();
@@ -738,19 +744,22 @@ void GPS::routeManagerSequenced()
   
   int index = _routeMgr->currentWaypoint(),
     count = _routeMgr->size();
-  if ((index < 1) || (index >= count)) {
+  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);
-  SGWayPoint wp0(_routeMgr->get_waypoint(index - 1));
-  SGWayPoint wp1(_routeMgr->get_waypoint(index));
-    
-  _wp0Ident = wp0.get_id();
-  _wp0Name = wp0.get_name();
-  _wp0_position = wp0.get_target();
+  
+  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();
@@ -1030,7 +1039,7 @@ double GPS::getLegDistance() const
 
 double GPS::getLegCourse() const
 {
-  if (!_dataValid || (_mode == "obs")) {
+  if (!_dataValid) {
     return -9999.0;
   }
   
@@ -1039,7 +1048,7 @@ double GPS::getLegCourse() const
 
 double GPS::getLegMagCourse() const
 {
-  if (!_dataValid || (_mode == "obs")) {
+  if (!_dataValid) {
     return 0.0;
   }
   
@@ -1646,6 +1655,11 @@ void GPS::selectLegMode()
   SG_LOG(SG_INSTR, SG_INFO, "GPS switching to LEG mode");
   _mode = "leg";
   
+  // depending on the situation, this will either get over-written 
+  // in routeManagerSequenced or not; either way it does no harm to
+  // set it here.
+  _wp0_position = _indicated_pos;
+
   // not really sequenced, but does all the work of updating wp0/1
   routeManagerSequenced();
 }