Canvas: Performance improvements.

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

diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx
index 1add1b97f1187a7b57dc0065d7efcd0e5454db0b..729ad0526f70460098d9127db6f3a991713e3f6e 100644 (file)
--- a/src/Instrumentation/gps.cxx
+++ b/src/Instrumentation/gps.cxx
@@ -29,6 +29,7 @@
 #include <simgear/sg_inlines.h>
 #include <simgear/math/sg_geodesy.hxx>
 #include <simgear/structure/exception.hxx>
+#include <simgear/scene/util/OsgMath.hxx>
 
 using std::auto_ptr;
 using std::string;
@@ -72,6 +73,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 {
@@ -193,7 +201,6 @@ GPS::Config::Config() :
   _turnRate(3.0), // degrees-per-second, so 180 degree turn takes 60 seconds
   _overflightArmDistance(1.0),
   _waypointAlertTime(30.0),
-  _minRunwayLengthFt(0.0),
   _requireHardSurface(true),
   _cdiMaxDeflectionNm(3.0), // linear mode, 3nm at the peg
   _driveAutopilot(true),
@@ -207,7 +214,6 @@ void GPS::Config::bind(GPS* aOwner, SGPropertyNode* aCfg)
   aOwner->tie(aCfg, "turn-rate-deg-sec", SGRawValuePointer<double>(&_turnRate));
   aOwner->tie(aCfg, "turn-anticipation", SGRawValuePointer<bool>(&_enableTurnAnticipation));
   aOwner->tie(aCfg, "wpt-alert-time", SGRawValuePointer<double>(&_waypointAlertTime));
-  aOwner->tie(aCfg, "min-runway-length-ft", SGRawValuePointer<double>(&_minRunwayLengthFt));
   aOwner->tie(aCfg, "hard-surface-runways-only", SGRawValuePointer<bool>(&_requireHardSurface));
   aOwner->tie(aCfg, "cdi-max-deflection-nm", SGRawValuePointer<double>(&_cdiMaxDeflectionNm));
   aOwner->tie(aCfg, "drive-autopilot", SGRawValuePointer<bool>(&_driveAutopilot));
@@ -224,6 +230,11 @@ GPS::GPS ( SGPropertyNode *node) :
   _mode("init"),
   _name(node->getStringValue("name", "gps")),
   _num(node->getIntValue("number", 0)),
+  _searchResultIndex(0),
+  _searchExact(true),
+  _searchIsRoute(false),
+  _searchHasNext(false),
+  _searchNames(false),
   _computeTurnData(false),
   _anticipateTurn(false),
   _inTurn(false)
@@ -317,7 +328,11 @@ GPS::init ()
   
   // last thing, add the deprecated prop watcher
   new DeprecatedPropListener(_gpsNode);
-  
+}
+
+void
+GPS::reinit ()
+{
   clearOutput();
 }
 
@@ -325,10 +340,11 @@ void
 GPS::bind()
 {
   _config.bind(this, _gpsNode->getChild("config", 0, true));
+
 // basic GPS outputs
   tie(_gpsNode, "selected-course-deg", SGRawValueMethods<GPS, double>
     (*this, &GPS::getSelectedCourse, &GPS::setSelectedCourse));
-  
+
   tie(_gpsNode, "desired-course-deg", SGRawValueMethods<GPS, double>
     (*this, &GPS::getDesiredCourse, NULL));
   _desiredCourseNode = _gpsNode->getChild("desired-course-deg", 0, true);
@@ -345,7 +361,7 @@ GPS::bind()
   tie(_gpsNode, "indicated-ground-speed-kt", SGRawValueMethods<GPS, double>
     (*this, &GPS::getGroundspeedKts, NULL));
   
-// command system    
+// command system
   tie(_gpsNode, "mode", SGRawValueMethods<GPS, const char*>(*this, &GPS::getMode, NULL));
   tie(_gpsNode, "command", SGRawValueMethods<GPS, const char*>(*this, &GPS::getCommand, &GPS::setCommand));
     
@@ -398,10 +414,7 @@ GPS::bind()
 void
 GPS::unbind()
 {
-  for (unsigned int t=0; t<_tiedNodes.size(); ++t) {
-    _tiedNodes[t]->untie();
-  }
-  _tiedNodes.clear();
+  _tiedProperties.Untie();
 }
 
 void
@@ -469,8 +482,6 @@ 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");
         
     if (_route_active_node->getBoolValue()) {
       // GPS init with active route
@@ -487,10 +498,16 @@ GPS::update (double delta_time_sec)
         selectOBSMode();
       }
     }
+
+    if (_mode != "init")
+    {
+      // allow a realistic delay in the future, here
+      SG_LOG(SG_INSTR, SG_INFO, "GPS initialisation complete");
+    }
   } // of init mode check
   
   _last_pos = _indicated_pos;
-  _lastPosValid = true;
+  _lastPosValid = !(_last_pos == SGGeod());
 }
 
 ///////////////////////////////////////////////////////////////////////////
@@ -604,12 +621,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 +645,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; 
     }
   }
   
@@ -657,7 +677,7 @@ void GPS::referenceNavaidSet(const std::string& aNavaid)
 
   if (_ref_navaid) {
     _ref_navaid_set = true;
-    SG_LOG(SG_INSTR, SG_INFO, "GPS code set explict ref-navaid:" << _ref_navaid->ident());
+    SG_LOG(SG_INSTR, SG_INFO, "GPS code set explicit ref-navaid:" << _ref_navaid->ident());
     _ref_navaid_id_node->setStringValue(_ref_navaid->ident().c_str());
     _ref_navaid_name_node->setStringValue(_ref_navaid->name().c_str());
     FGNavRecord* vor = (FGNavRecord*) _ref_navaid.ptr();
@@ -697,6 +717,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;
   }
@@ -704,7 +726,7 @@ void GPS::routeManagerSequenced()
   SG_LOG(SG_INSTR, SG_INFO, "GPS waypoint index is now " << index);
   
   if (index > 0) {
-    _prevWaypt = _routeMgr->previousWaypt();
+    _prevWaypt = _routeMgr->wayptAtIndex(index - 1);
     if (_prevWaypt->flag(WPT_DYNAMIC)) {
       _wp0_position = _indicated_pos;
     } else {
@@ -809,7 +831,7 @@ void GPS::updateOverflight()
     
     // check for wp1 being on active route - resume leg mode
     if (_routeMgr->isRouteActive()) {
-      int index = _routeMgr->findWayptIndex(_currentWaypt->position());
+      int index = _routeMgr->flightPlan()->findWayptIndex(_currentWaypt->position());
       if (index >= 0) {
         SG_LOG(SG_INSTR, SG_INFO, "GPS DTO, resuming LEG mode at wp:" << index);
         _mode = "leg";
@@ -837,7 +859,7 @@ void GPS::beginTurn()
 {
   _inTurn = true;
   _turnSequenced = false;
-  SG_LOG(SG_INSTR, SG_INFO, "begining turn");
+  SG_LOG(SG_INSTR, SG_INFO, "beginning turn");
 }
 
 void GPS::endTurn()
@@ -861,7 +883,7 @@ void GPS::computeTurnData()
     return;
   }
   
-  WayptRef next = _routeMgr->nextWaypt();
+  WayptRef next = _routeMgr->wayptAtIndex(_routeMgr->currentIndex() + 1);
   if (!next || next->flag(WPT_DYNAMIC)) {
     _anticipateTurn = false;
     return;
@@ -971,6 +993,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 +1106,7 @@ double GPS::getCDIDeflection() const
 
 const char* GPS::getWP0Ident() const
 {
-  if (!_dataValid || (_mode != "leg")) {
+  if (!_dataValid || (_mode != "leg") || (!_prevWaypt)) {
     return "";
   }
   
@@ -1096,7 +1120,7 @@ const char* GPS::getWP0Name() const
 
 const char* GPS::getWP1Ident() const
 {
-  if (!_dataValid) {
+  if ((!_dataValid)||(!_currentWaypt)) {
     return "";
   }
   
@@ -1147,7 +1171,7 @@ double GPS::getWP1Bearing() const
 
 double GPS::getWP1MagBearing() const
 {
-  if (!_dataValid) {
+  if (!_dataValid || !_wayptController.get()) {
     return -9999.0;
   }
 
@@ -1417,7 +1441,7 @@ FGPositioned::Type GPS::SearchFilter::maxType() const
 FGPositioned::Filter* GPS::createFilter(FGPositioned::Type aTy)
 {
   if (aTy == FGPositioned::AIRPORT) {
-    return new FGAirport::HardSurfaceFilter(_config.minRunwayLengthFt());
+    return new FGAirport::HardSurfaceFilter();
   }
   
   // if we were passed INVALID, assume it means 'all types interesting to a GPS'
@@ -1662,7 +1686,7 @@ void GPS::insertWaypointAtIndex(int aIndex)
   string ident = _scratchNode->getStringValue("ident");
 
   WayptRef wpt = new BasicWaypt(_scratchPos, ident, NULL);
-  _routeMgr->insertWayptAtIndex(wpt, aIndex);
+  _routeMgr->flightPlan()->insertWayptAtIndex(wpt, aIndex);
 }
 
 void GPS::removeWaypointAtIndex(int aIndex)
@@ -1671,7 +1695,7 @@ void GPS::removeWaypointAtIndex(int aIndex)
     throw sg_range_exception("GPS::removeWaypointAtIndex: index out of bounds");
   }
   
-  _routeMgr->removeWayptAtIndex(aIndex);
+  _routeMgr->removeLegAtIndex(aIndex);
 }
 
 void GPS::tieSGGeod(SGPropertyNode* aNode, SGGeod& aRef,