Merge branch 'next' into attenuation

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

diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx
index 1add1b97f1187a7b57dc0065d7efcd0e5454db0b..667ff2c36b50daf7e9f63a773cab5da52d96e2ef 100644 (file)
--- a/src/Instrumentation/gps.cxx
+++ b/src/Instrumentation/gps.cxx
@@ -72,6 +72,13 @@ SGGeod SGGeodProperty::get() const
 {
     double lon = _lon->getDoubleValue(),
         lat = _lat->getDoubleValue();
+        
+    if (osg::isNaN(lon) || osg::isNaN(lat)) {
+      SG_LOG(SG_INSTR, SG_WARN, "read NaN for lon/lat:" << _lon->getPath() 
+        << ", " << _lat->getPath());
+      return SGGeod();
+    }
+        
     if (_alt) {
         return SGGeod::fromDegFt(lon, lat, _alt->getDoubleValue());
     } else {
@@ -490,7 +497,7 @@ GPS::update (double delta_time_sec)
   } // of init mode check
   
   _last_pos = _indicated_pos;
-  _lastPosValid = true;
+  _lastPosValid = !(_last_pos == SGGeod());
 }
 
 ///////////////////////////////////////////////////////////////////////////
@@ -604,12 +611,11 @@ void GPS::updateReferenceNavaid(double dt)
   if (!_ref_navaid_set) {
     _ref_navaid_elapsed += dt;
     if (_ref_navaid_elapsed > 5.0) {
-      _ref_navaid_elapsed = 0.0;
 
       FGPositioned::TypeFilter vorFilter(FGPositioned::VOR);
       FGPositionedRef nav = FGPositioned::findClosest(_indicated_pos, 400.0, &vorFilter);
       if (!nav) {
-        SG_LOG(SG_INSTR, SG_INFO, "GPS couldn't find a reference navid");
+        SG_LOG(SG_INSTR, SG_INFO, "GPS couldn't find a reference navaid");
         _ref_navaid_id_node->setStringValue("");
         _ref_navaid_name_node->setStringValue("");
         _ref_navaid_bearing_node->setDoubleValue(0.0);
@@ -629,6 +635,10 @@ void GPS::updateReferenceNavaid(double dt)
       }
       
       _ref_navaid = nav;
+      // reset elapsed time (do not do that before updating the properties above, since their
+      // listeners may request another update (_ref_navaid_elapsed = 9999), which results in
+      // excessive load (FGPositioned::findClosest called in every update loop...)
+      _ref_navaid_elapsed = 0.0; 
     }
   }
   
@@ -697,6 +707,8 @@ void GPS::routeManagerSequenced()
   int index = _routeMgr->currentIndex(),
     count = _routeMgr->numWaypts();
   if ((index < 0) || (index >= count)) {
+    _currentWaypt=NULL;
+    _prevWaypt=NULL;
     SG_LOG(SG_INSTR, SG_ALERT, "GPS: malformed route, index=" << index);
     return;
   }
@@ -971,6 +983,8 @@ void GPS::driveAutopilot()
 
 void GPS::wp1Changed()
 {
+  if (!_currentWaypt)
+    return;
   if (_mode == "leg") {
     _wayptController.reset(WayptController::createForWaypt(this, _currentWaypt));
   } else if (_mode == "obs") {
@@ -1082,7 +1096,7 @@ double GPS::getCDIDeflection() const
 
 const char* GPS::getWP0Ident() const
 {
-  if (!_dataValid || (_mode != "leg")) {
+  if (!_dataValid || (_mode != "leg") || (!_prevWaypt)) {
     return "";
   }
   
@@ -1096,7 +1110,7 @@ const char* GPS::getWP0Name() const
 
 const char* GPS::getWP1Ident() const
 {
-  if (!_dataValid) {
+  if ((!_dataValid)||(!_currentWaypt)) {
     return "";
   }
   
@@ -1147,7 +1161,7 @@ double GPS::getWP1Bearing() const
 
 double GPS::getWP1MagBearing() const
 {
-  if (!_dataValid) {
+  if (!_dataValid || !_wayptController.get()) {
     return -9999.0;
   }