Fix GPS intercept controller:

- handle bad intersection geometry gracefully (end controller)
- fix termination condition
- always use target track to compute position.

diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx
index 1d0acf23bbc6e3951038bb0781e24bb2cff4ec0b..3b3fa726bbda34951d3b3eb3a489a6605998dc77 100644 (file)
--- a/src/Instrumentation/gps.cxx
+++ b/src/Instrumentation/gps.cxx
@@ -563,9 +563,9 @@ void GPS::currentWaypointChanged()
   }
   
   _currentWaypt = _route->currentLeg()->waypoint();
+    wp1Changed(); // rebuild the active controller
   _desiredCourse = getLegMagCourse();
   _desiredCourseNode->fireValueChanged();
-  wp1Changed();
 }
 
 

diff --git a/src/Instrumentation/rnav_waypt_controller.cxx b/src/Instrumentation/rnav_waypt_controller.cxx
index a0e45933a035400688a98502e5d67d60a2ed3bba..2fc750ebdbc3c93c205be20f6386a38651262903 100644 (file)
--- a/src/Instrumentation/rnav_waypt_controller.cxx
+++ b/src/Instrumentation/rnav_waypt_controller.cxx
@@ -528,16 +528,43 @@ public:
   virtual void init()
   {
     RadialIntercept* w = (RadialIntercept*) _waypt.get();
-    _trueRadial = w->radialDegMagnetic() + _rnav->magvarDeg();
+      _trueRadial = w->radialDegMagnetic() + _rnav->magvarDeg();
     _targetTrack = w->courseDegMagnetic() + _rnav->magvarDeg();
   }
   
   virtual void update(double)
   {
+      SGGeoc c,
+        geocPos = SGGeoc::fromGeod(_rnav->position()),
+        geocWayptPos = SGGeoc::fromGeod(_waypt->position());
+
+      bool ok = geocRadialIntersection(geocPos, _targetTrack,
+                                       geocWayptPos, _trueRadial, c);
+      if (!ok) {
+          // try with a backwards offset from the waypt pos, in case the
+          // procedure waypt location is too close. (eg, KSFO OCEAN SID)
+
+          SGGeoc navidAdjusted;
+          SGGeodesy::advanceRadM(geocWayptPos, _trueRadial, SG_NM_TO_METER * -10, navidAdjusted);
+
+          ok = geocRadialIntersection(geocPos, _targetTrack,
+                                      navidAdjusted, _trueRadial, c);
+          if (!ok) {
+              SG_LOG(SG_INSTR, SG_WARN, "InterceptCtl, bad intersection, skipping waypt");
+              setDone();
+              return;
+          }
+      }
+
+      _projectedPosition = SGGeod::fromGeoc(c);
+
+
     // note we want the outbound radial from the waypt, hence the ordering
     // of arguments to courseDeg
     double r = SGGeodesy::courseDeg(_waypt->position(), _rnav->position());
-    if (fabs(r - _trueRadial) < 0.5) {
+      double bearingDiff = r - _trueRadial;
+      SG_NORMALIZE_RANGE(bearingDiff, -180.0, 180.0);
+    if (fabs(bearingDiff) < 0.5) {
       setDone();
     }
   }
@@ -549,31 +576,11 @@ public:
 
     virtual SGGeod position() const
     {
-        SGGeoc c,
-        geocPos = SGGeoc::fromGeod(_rnav->position()),
-        geocWayptPos = SGGeoc::fromGeod(_waypt->position());
-
-        bool ok = geocRadialIntersection(geocPos, _rnav->trackDeg(),
-                                         geocWayptPos, _trueRadial, c);
-        if (!ok) {
-            // try with a backwards offset from the waypt pos, in case the
-            // procedure waypt location is too close. (eg, KSFO OCEAN SID)
-
-            SGGeoc navidAdjusted;
-            SGGeodesy::advanceRadM(geocWayptPos, _trueRadial, SG_NM_TO_METER * -10, navidAdjusted);
-
-            ok = geocRadialIntersection(geocPos, _rnav->trackDeg(),
-                                        navidAdjusted, _trueRadial, c);
-            if (!ok) {
-                // fallback for the broken case
-                return _waypt->position();
-            }
-        }
-        
-        return SGGeod::fromGeoc(c);
+        return _projectedPosition;
     }
 private:
   double _trueRadial;
+    SGGeod _projectedPosition;
 };
 
 class DMEInterceptCtl : public WayptController