Performance optimization

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

diff --git a/src/Instrumentation/rnav_waypt_controller.cxx b/src/Instrumentation/rnav_waypt_controller.cxx
index fbdb5eeb16a136a9e797ac99fc1c483e2d1cd334..f893b9413ccabdb7d6943d7024465c2c1fa90cd1 100644 (file)
--- a/src/Instrumentation/rnav_waypt_controller.cxx
+++ b/src/Instrumentation/rnav_waypt_controller.cxx
@@ -19,6 +19,8 @@
 
 #include "rnav_waypt_controller.hxx"
 
+#include <cassert>
+
 #include <simgear/sg_inlines.h>
 #include <simgear/structure/exception.hxx>
 
@@ -67,8 +69,8 @@ bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double
   double crs12 = SGGeodesy::courseRad(a, b),
     crs21 = SGGeodesy::courseRad(b, a);
     
-  double degCrs12 = crs12 * SG_RADIANS_TO_DEGREES;
-  double degCrs21 = crs21 * SG_RADIANS_TO_DEGREES;
+  //double degCrs12 = crs12 * SG_RADIANS_TO_DEGREES;
+  //double degCrs21 = crs21 * SG_RADIANS_TO_DEGREES;
     
  /* 
   if (sin(diffLon) < 0.0) {
@@ -84,12 +86,12 @@ bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double
   double ang2 = SGMiscd::normalizeAngle2(crs21-crs23);
     
   if ((sin(ang1) == 0.0) && (sin(ang2) == 0.0)) {
-    SG_LOG(SG_GENERAL, SG_WARN, "geocRadialIntersection: infinity of intersections");
+    SG_LOG(SG_INSTR, SG_WARN, "geocRadialIntersection: infinity of intersections");
     return false;
   }
   
   if ((sin(ang1)*sin(ang2))<0.0) {
-    SG_LOG(SG_GENERAL, SG_WARN, "geocRadialIntersection: intersection ambiguous");
+    SG_LOG(SG_INSTR, SG_WARN, "geocRadialIntersection: intersection ambiguous");
     return false;
   }
   
@@ -155,7 +157,9 @@ class BasicWayptCtl : public WayptController
 {
 public:
   BasicWayptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
-    WayptController(aRNAV, aWpt)
+    WayptController(aRNAV, aWpt),
+    _distanceM(0.0),
+    _courseDev(0.0)
   {
     if (aWpt->flag(WPT_DYNAMIC)) {
       throw sg_exception("BasicWayptCtrl doesn't work with dynamic waypoints");
@@ -217,14 +221,17 @@ private:
 
 /**
  * Special controller for runways. For runways, we want very narrow deviation
- * contraints, and to understand that any point along the paved area is
+ * constraints, and to understand that any point along the paved area is
  * equivalent to being 'at' the runway.
  */
 class RunwayCtl : public WayptController
 {
 public:
   RunwayCtl(RNAV* aRNAV, const WayptRef& aWpt) :
-    WayptController(aRNAV, aWpt)
+    WayptController(aRNAV, aWpt),
+    _runway(NULL),
+    _distanceM(0.0),
+    _courseDev(0.0)
   {
   }
   
@@ -245,7 +252,7 @@ public:
     double _courseDev = brg - _targetTrack;
     SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
     
-    if (fabs(_courseDev) > 90.0) {
+    if ((fabs(_courseDev) > 90.0) && (_distanceM < _rnav->overflightArmDistanceM())) {
       setDone();
     }
   } 
@@ -310,30 +317,31 @@ public:
     double curAlt = _rnav->position().getElevationFt();
     
     switch (_waypt->altitudeRestriction()) {
-    case RESTRICT_AT: {
+    case RESTRICT_AT: 
+    case RESTRICT_COMPUTED:  
+    {
       double d = curAlt - _waypt->altitudeFt();
       if (fabs(d) < 50.0) {
-        SG_LOG(SG_GENERAL, SG_INFO, "ConstHdgToAltCtl, reached target altitude " << _waypt->altitudeFt());
+        SG_LOG(SG_INSTR, SG_INFO, "ConstHdgToAltCtl, reached target altitude " << _waypt->altitudeFt());
         setDone();
       }
     } break;
       
     case RESTRICT_ABOVE:
       if (curAlt >= _waypt->altitudeFt()) {
-        SG_LOG(SG_GENERAL, SG_INFO, "ConstHdgToAltCtl, above target altitude " << _waypt->altitudeFt());
+        SG_LOG(SG_INSTR, SG_INFO, "ConstHdgToAltCtl, above target altitude " << _waypt->altitudeFt());
         setDone();
       }
       break;
       
     case RESTRICT_BELOW:
       if (curAlt <= _waypt->altitudeFt()) {
-        SG_LOG(SG_GENERAL, SG_INFO, "ConstHdgToAltCtl, below target altitude " << _waypt->altitudeFt());
+        SG_LOG(SG_INSTR, SG_INFO, "ConstHdgToAltCtl, below target altitude " << _waypt->altitudeFt());
         setDone();
       }
       break;
     
-    case RESTRICT_NONE:
-      assert(false);
+    default:
       break;
     }
   }
@@ -363,8 +371,8 @@ class InterceptCtl : public WayptController
 {
 public:
   InterceptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
-    WayptController(aRNAV, aWpt)
-    
+    WayptController(aRNAV, aWpt),
+    _trueRadial(0.0)
   {
     if (_waypt->type() != "radialIntercept") {
       throw sg_exception("invalid waypoint type", "InterceptCtl ctor");
@@ -385,7 +393,7 @@ public:
     double r = SGGeodesy::courseDeg(_waypt->position(), _rnav->position());
     SG_LOG(SG_AUTOPILOT, SG_INFO, "current radial=" << r);
     if (fabs(r - _trueRadial) < 0.5) {
-      SG_LOG(SG_GENERAL, SG_INFO, "InterceptCtl, intercepted radial " << _trueRadial);
+      SG_LOG(SG_INSTR, SG_INFO, "InterceptCtl, intercepted radial " << _trueRadial);
       setDone();
     }
   }
@@ -410,8 +418,9 @@ class DMEInterceptCtl : public WayptController
 {
 public:
   DMEInterceptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
-    WayptController(aRNAV, aWpt)
-    
+    WayptController(aRNAV, aWpt),
+    _dme(NULL),
+    _distanceNm(0.0)
   {
     if (_waypt->type() != "dmeIntercept") {
       throw sg_exception("invalid waypoint type", "DMEInterceptCtl ctor");
@@ -429,7 +438,7 @@ public:
     _distanceNm = SGGeodesy::distanceNm(_rnav->position(), _dme->position());
     double d = fabs(_distanceNm - _dme->dmeDistanceNm());
     if (d < 0.1) {
-      SG_LOG(SG_GENERAL, SG_INFO, "DMEInterceptCtl, intercepted DME " << _dme->dmeDistanceNm());
+      SG_LOG(SG_INSTR, SG_INFO, "DMEInterceptCtl, intercepted DME " << _dme->dmeDistanceNm());
       setDone();
     }
   }
@@ -544,7 +553,9 @@ private:
 
 DirectToController::DirectToController(RNAV* aRNAV, const WayptRef& aWpt, const SGGeod& aOrigin) :
   WayptController(aRNAV, aWpt),
-  _origin(aOrigin)
+  _origin(aOrigin),
+  _distanceM(0.0),
+  _courseDev(0.0)
 {
 }
 
@@ -598,7 +609,9 @@ SGGeod DirectToController::position() const
 ///////////////////////////////////////////////////////////////////////////////
 
 OBSController::OBSController(RNAV* aRNAV, const WayptRef& aWpt) :
-  WayptController(aRNAV, aWpt)
+  WayptController(aRNAV, aWpt),
+  _distanceM(0.0),
+  _courseDev(0.0)
 {
 }