Autopilot: clean up the helpers code (which drives the various /internal/) properties...

[flightgear.git] / src / Instrumentation / gps.cxx

diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx
index 0080ed7d04002dc0897944c24efbd22fda2f21d9..4477dd4eddaf23e5ce1e77b7ad6a0ddfda24694a 100644 (file)
--- a/src/Instrumentation/gps.cxx
+++ b/src/Instrumentation/gps.cxx
@@ -24,6 +24,7 @@
 #include <simgear/math/sg_random.h>
 #include <simgear/sg_inlines.h>
 #include <simgear/math/sg_geodesy.hxx>
+#include <simgear/structure/exception.hxx>
 
 using std::auto_ptr;
 using std::string;
@@ -280,6 +281,7 @@ GPS::Config::setExternalCourse(double aCourseDeg)
 ////////////////////////////////////////////////////////////////////////////
 
 GPS::GPS ( SGPropertyNode *node) : 
+  _selectedCourse(0.0),
   _dataValid(false),
   _lastPosValid(false),
   _mode("init"),
@@ -558,6 +560,9 @@ GPS::update (double delta_time_sec)
   if (_dataValid && (_mode == "init")) {
     // allow a realistic delay in the future, here
     SG_LOG(SG_INSTR, SG_INFO, "GPS initialisation complete");
+    
+    _selectedCourse = _config.getExternalCourse();
+    
     if (_route_active_node->getBoolValue()) {
       // GPS init with active route
       SG_LOG(SG_INSTR, SG_INFO, "GPS init with active route");
@@ -722,6 +727,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();
@@ -737,19 +748,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();
@@ -1029,7 +1043,7 @@ double GPS::getLegDistance() const
 
 double GPS::getLegCourse() const
 {
-  if (!_dataValid || (_mode == "obs")) {
+  if (!_dataValid) {
     return -9999.0;
   }
   
@@ -1038,7 +1052,7 @@ double GPS::getLegCourse() const
 
 double GPS::getLegMagCourse() const
 {
-  if (!_dataValid || (_mode == "obs")) {
+  if (!_dataValid) {
     return 0.0;
   }
   
@@ -1287,8 +1301,40 @@ void GPS::setCommand(const char* aCmd)
     previousResult();
   } else if (!strcmp(aCmd, "define-user-wpt")) {
     defineWaypoint();
+  } else if (!strcmp(aCmd, "route-insert-before")) {
+    int index = _scratchNode->getIntValue("index");
+    if (index < 0 || (_routeMgr->size() == 0)) {
+      index = _routeMgr->size();
+    } else if (index >= _routeMgr->size()) {
+      SG_LOG(SG_INSTR, SG_WARN, "GPS:route-insert-before, bad index:" << index);
+      return;
+    }
+    
+    insertWaypointAtIndex(index);
+  } else if (!strcmp(aCmd, "route-insert-after")) {
+    int index = _scratchNode->getIntValue("index");
+    if (index < 0 || (_routeMgr->size() == 0)) {
+      index = _routeMgr->size();
+    } else if (index >= _routeMgr->size()) {
+      SG_LOG(SG_INSTR, SG_WARN, "GPS:route-insert-after, bad index:" << index);
+      return;
+    } else {
+      ++index; 
+    }
+  
+    insertWaypointAtIndex(index);
+  } else if (!strcmp(aCmd, "route-delete")) {
+    int index = _scratchNode->getIntValue("index");
+    if (index < 0) {
+      index = _routeMgr->size();
+    } else if (index >= _routeMgr->size()) {
+      SG_LOG(SG_INSTR, SG_WARN, "GPS:route-delete, bad index:" << index);
+      return;
+    }
+    
+    removeWaypointAtIndex(index);
   } else {
-    SG_LOG(SG_INSTR, SG_WARN, "GPS:unrecognzied command:" << aCmd);
+    SG_LOG(SG_INSTR, SG_WARN, "GPS:unrecognized command:" << aCmd);
   }
 }
 
@@ -1446,6 +1492,12 @@ void GPS::search()
   string sty(_scratchNode->getStringValue("type"));
   _searchType = FGPositioned::typeFromName(sty);
   _searchQuery = _scratchNode->getStringValue("query");
+  if (_searchQuery.empty()) {
+    SG_LOG(SG_INSTR, SG_WARN, "empty GPS search query");
+    clearScratch();
+    return;
+  }
+  
   _searchExact = _scratchNode->getBoolValue("exact", true);
   _searchOrderByRange = _scratchNode->getBoolValue("order-by-distance", true);
   _searchResultIndex = 0;
@@ -1613,6 +1665,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();
 }
@@ -1679,4 +1736,31 @@ void GPS::defineWaypoint()
   setScratchFromPositioned(wpt.get(), -1);
 }
 
+void GPS::insertWaypointAtIndex(int aIndex)
+{
+  // note we do allow index = routeMgr->size(), that's an append
+  if ((aIndex < 0) || (aIndex > _routeMgr->size())) {
+    throw sg_range_exception("GPS::insertWaypointAtIndex: index out of bounds");
+  }
+  
+  if (!isScratchPositionValid()) {
+    SG_LOG(SG_INSTR, SG_WARN, "GPS:insertWaypointAtIndex: invalid lat/lon");
+    return;
+  }
+  
+  string ident = _scratchNode->getStringValue("ident");
+  string name = _scratchNode->getStringValue("name");
+  
+  _routeMgr->add_waypoint(SGWayPoint(_scratchPos, ident, name), aIndex);
+}
+
+void GPS::removeWaypointAtIndex(int aIndex)
+{
+  if ((aIndex < 0) || (aIndex >= _routeMgr->size())) {
+    throw sg_range_exception("GPS::removeWaypointAtIndex: index out of bounds");
+  }
+  
+  _routeMgr->pop_waypoint(aIndex);
+}
+
 // end of gps.cxx