Merge branch 'durk/traffic'

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

diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx
index 290dc6e4da2fbc367c5fc91ad5666948e06df626..4477dd4eddaf23e5ce1e77b7ad6a0ddfda24694a 100644 (file)
--- a/src/Instrumentation/gps.cxx
+++ b/src/Instrumentation/gps.cxx
@@ -281,6 +281,7 @@ GPS::Config::setExternalCourse(double aCourseDeg)
 ////////////////////////////////////////////////////////////////////////////
 
 GPS::GPS ( SGPropertyNode *node) : 
+  _selectedCourse(0.0),
   _dataValid(false),
   _lastPosValid(false),
   _mode("init"),
@@ -559,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");
@@ -723,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();
@@ -738,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();
@@ -1030,7 +1043,7 @@ double GPS::getLegDistance() const
 
 double GPS::getLegCourse() const
 {
-  if (!_dataValid || (_mode == "obs")) {
+  if (!_dataValid) {
     return -9999.0;
   }
   
@@ -1039,7 +1052,7 @@ double GPS::getLegCourse() const
 
 double GPS::getLegMagCourse() const
 {
-  if (!_dataValid || (_mode == "obs")) {
+  if (!_dataValid) {
     return 0.0;
   }
   
@@ -1290,7 +1303,7 @@ void GPS::setCommand(const char* aCmd)
     defineWaypoint();
   } else if (!strcmp(aCmd, "route-insert-before")) {
     int index = _scratchNode->getIntValue("index");
-    if (index < 0) {
+    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);
@@ -1300,7 +1313,7 @@ void GPS::setCommand(const char* aCmd)
     insertWaypointAtIndex(index);
   } else if (!strcmp(aCmd, "route-insert-after")) {
     int index = _scratchNode->getIntValue("index");
-    if (index < 0) {
+    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);
@@ -1479,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;
@@ -1646,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();
 }