GPS/RNAV: configurable max-flyBy-angle

Make the max-flyBy angle configuable, since for exmaple AIrbus uses 90
degrees. Expose this via a new gps/config property, and extend the tests
to verify that angles greater than the fly-by angle behav as fly-over
waypoints.

Extend the RoutePath code to share this configuration, so that route
visualisations match the configured angle.

SF-ID: https://sourceforge.net/p/flightgear/codetickets/2694/

src/Instrumentation/gps.cxx

@@ -91,6 +91,14 @@ void GPS::Config::bind(GPS* aOwner, SGPropertyNode* aCfg)
   aOwner->tie(aCfg, "over-flight-arm-distance-nm", SGRawValuePointer<double>(&_overflightArmDistance));
   aOwner->tie(aCfg, "over-flight-arm-angle-deg", SGRawValuePointer<double>(&_overflightArmAngle));
   aOwner->tie(aCfg, "delegate-sequencing", SGRawValuePointer<bool>(&_delegateSequencing));
+  aOwner->tie(aCfg, "max-fly-by-turn-angle-deg", SGRawValueMethods<GPS, double>(*aOwner, &GPS::maxFlyByTurnAngleDeg, &GPS::setFlyByMaxTurnAngle));
+}
+
+void GPS::setFlyByMaxTurnAngle(double maxAngle)
+{
+    _config.setMaxFlyByTurnAngle(maxAngle);
+    // keep the FlightPlan in sync, so RoutePath matches
+    _route->setMaxFlyByTurnAngle(maxAngle);
 }
 
 ////////////////////////////////////////////////////////////////////////////
@@ -481,6 +489,11 @@ double GPS::selectedMagCourse()
   return _selectedCourse;
 }
 
+double GPS::maxFlyByTurnAngleDeg() const
+{
+    return _config.maxFlyByTurnAngleDeg();
+}
+
 simgear::optional<double> GPS::nextLegTrack()
 {
     auto next = _route->nextLeg();

src/Instrumentation/gps.hxx

@@ -86,6 +86,7 @@ public:
     double overflightArmDistanceM() override;
     double overflightArmAngleDeg() override;
     bool canFlyBy() const override;
+    double maxFlyByTurnAngleDeg() const override;
 
     simgear::optional<LegData> previousLegData() override;
     
@@ -95,6 +96,8 @@ public:
 private:
     friend class SearchFilter;
 
+    void setFlyByMaxTurnAngle(double maxAngle);
+
     /**
      * Configuration manager, track data relating to aircraft installation
      */
@@ -158,6 +161,14 @@ private:
 
         
         bool delegateDoesSequencing() const    { return _delegateSequencing; }
+
+        double maxFlyByTurnAngleDeg() const { return _maxFlyByTurnAngle; }
+
+        void setMaxFlyByTurnAngle(double deg)
+        {
+            _maxFlyByTurnAngle = deg;
+        }
+
     private:
         bool _enableTurnAnticipation;
 
@@ -194,6 +205,8 @@ private:
         // do we handle waypoint sequencing ourselves, or let the delegate do it?
         // default is we do it, for backwards compatability
         bool _delegateSequencing = false;
+
+        double _maxFlyByTurnAngle = 90.0;
     };
 
     class SearchFilter : public FGPositioned::Filter

src/Instrumentation/rnav_waypt_controller.cxx

@@ -326,7 +326,7 @@ public:
     _flyByTurnAngle = nextLegTrack.value() - _finalLegCourse;
     SG_NORMALIZE_RANGE(_flyByTurnAngle, -180.0, 180.0);
 
-    if (fabs(_flyByTurnAngle) > 120.0) {
+    if (fabs(_flyByTurnAngle) > _rnav->maxFlyByTurnAngleDeg()) {
         // too sharp, don't anticipate
         return;
     }

src/Instrumentation/rnav_waypt_controller.hxx

@@ -65,6 +65,16 @@ public:
         return false;
     }
 
+    /**
+     * maximum angle in degrees where flyBy is permitted. Turns larger
+     * than this value will always be executed as fly-over to avoid
+     * a very large cut.
+     */
+    virtual double maxFlyByTurnAngleDeg() const
+    {
+        return 120.0;
+    }
+
   /**
    * minimum distance to switch next waypoint.
    */

src/Navaids/FlightPlan.cxx

@@ -1908,6 +1908,16 @@ bool FlightPlan::followLegTrackToFixes() const
     return _followLegTrackToFix;
 }
 
+void FlightPlan::setMaxFlyByTurnAngle(double deg)
+{
+    _maxFlyByTurnAngle = deg;
+}
+
+double FlightPlan::maxFlyByTurnAngle() const
+{
+    return _maxFlyByTurnAngle;
+}
+
 std::string FlightPlan::icaoAircraftCategory() const
 {
     std::string r;

src/Navaids/FlightPlan.hxx

@@ -76,6 +76,9 @@ public:
     void setFollowLegTrackToFixes(bool tf);
     bool followLegTrackToFixes() const;
 
+    void setMaxFlyByTurnAngle(double deg);
+    double maxFlyByTurnAngle() const;
+
     // aircraft approach category as per CFR 97.3, etc
     // http://www.flightsimaviation.com/data/FARS/part_97-3.html
     std::string icaoAircraftCategory() const;
@@ -482,6 +485,7 @@ private:
     int _cruiseFlightLevel = 0;
     int _cruiseAltitudeFt = 0;
     int _estimatedDuration = 0;
+    double _maxFlyByTurnAngle = 90.0;
 
     FGAirportRef _departure, _destination;
     FGAirportRef _alternate;

src/Navaids/routePath.cxx

@@ -397,7 +397,7 @@ public:
         return turnRadius * fabs(angleDeg) * SG_DEGREES_TO_RADIANS;
     }
 
-  void computeTurn(double radiusM, bool constrainLegCourse, WayptData& next)
+    void computeTurn(double radiusM, bool constrainLegCourse, double maxFlyByTurnAngleDeg, WayptData& next)
     {
         assert(!skipped);
         assert(next.legCourseValid);
@@ -431,10 +431,11 @@ public:
         SG_NORMALIZE_RANGE(turnExitAngle, -180.0, 180.0);
         turnRadius = radiusM;
 
-    if (!flyOver && fabs(turnExitAngle) > 120.0) {
+        if (!flyOver && fabs(turnExitAngle) > maxFlyByTurnAngleDeg) {
             // flyBy logic blows up for sharp turns - due to the tan() term
             // heading towards infinity. By converting to flyOver we do something
-        // closer to what was requested.
+            // closer to what was requested. This matches logic in the RNAV
+            // Leg controller
             flyOver = true;
         }
 
@@ -445,7 +446,6 @@ public:
             } else {
                 turnEntryPos = pos;
                 turnExitCenter = turnCenterOverflight(pos, legCourseTrue, turnExitAngle, turnRadius);
-
             }
             turnExitPos = pointOnExitTurnFromHeading(next.legCourseTrue);
 
@@ -628,12 +628,12 @@ public:
 
     AircraftPerformance perf;
     bool constrainLegCourses;
+    double maxFlyByTurnAngleDeg = 90.0;
 
     void computeDynamicPosition(int index)
     {
         auto previous(previousValidWaypoint(index));
-    if ((previous == waypoints.end()) || !previous->posValid)
-    {
+        if ((previous == waypoints.end()) || !previous->posValid) {
             SG_LOG(SG_NAVAID, SG_WARN, "couldn't compute position for dynamic waypoint: no preceeding valid waypoint");
             return;
         }
@@ -669,9 +669,7 @@ public:
                 // try again
                 ok = geocRadialIntersection(prevGc, track, navidAdjusted, radial, rGc);
                 if (!ok) {
-          SG_LOG(SG_NAVAID, SG_WARN, "couldn't compute interception for radial:"
-               << previous->turnExitPos << " / " << track << "/" << wpt->position()
-               << "/" << radial);
+                    SG_LOG(SG_NAVAID, SG_WARN, "couldn't compute interception for radial:" << previous->turnExitPos << " / " << track << "/" << wpt->position() << "/" << radial);
                     waypoints[index].pos = wpt->position(); // horrible fallback
 
                 } else {
@@ -915,7 +913,7 @@ void RoutePath::commonInit()
           nextIt->computeLegCourse(&(d->waypoints[i]), radiusM);
 
           if (nextIt->legCourseValid) {
-              d->waypoints[i].computeTurn(radiusM, d->constrainLegCourses, *nextIt);
+              d->waypoints[i].computeTurn(radiusM, d->constrainLegCourses, d->maxFlyByTurnAngleDeg, *nextIt);
           } else {
             // next waypoint has indeterminate course. Let's create a sharp turn
             // this can happen when the following point is ATC vectors, for example.

test_suite/unit_tests/Instrumentation/test_gps.cxx

@@ -27,10 +27,11 @@
 #include "test_suite/FGTestApi/TestPilot.hxx"
 #include "test_suite/FGTestApi/TestDataLogger.hxx"
 
-#include <Navaids/NavDataCache.hxx>
-#include <Navaids/navrecord.hxx>
-#include <Navaids/navlist.hxx>
 #include <Navaids/FlightPlan.hxx>
+#include <Navaids/NavDataCache.hxx>
+#include <Navaids/fixlist.hxx>
+#include <Navaids/navlist.hxx>
+#include <Navaids/navrecord.hxx>
 
 #include <Instrumentation/gps.hxx>
 #include <Instrumentation/navradio.hxx>
@@ -1379,6 +1380,66 @@ void GPSTests::testTurnAnticipation()
 
 }
 
+void GPSTests::testExceedFlyByMaxAngleTurn()
+{
+    //  FGTestApi::setUp::logPositionToKML("gps_fly_by_exceed_max_angle");
+    auto rm = globals->get_subsystem<FGRouteMgr>();
+    auto fp = FlightPlan::create();
+    rm->setFlightPlan(fp);
+
+    FGTestApi::setUp::populateFPWithoutNasal(fp, "LFBD", "23", "EHAM", "18L", "SOMOS ROYAN TIRAV BOBRI ADABI");
+    FGTestApi::writeFlightPlanToKML(fp);
+
+    // takes the place of the Nasal delegates
+    auto testDelegate = new TestFPDelegate;
+    testDelegate->thePlan = fp;
+    CPPUNIT_ASSERT(rm->activate());
+    fp->addDelegate(testDelegate);
+    auto gps = setupStandardGPS();
+
+    fp->setCurrentIndex(4); // BOBRI
+
+    // somehwat before BOBRI
+    SGGeod initPos = fp->pointAlongRouteNorm(4, -0.1);
+    FGTestApi::setPositionAndStabilise(initPos);
+    FGTestApi::writePointToKML("init point", initPos);
+
+    auto gpsNode = globals->get_props()->getNode("instrumentation/gps");
+    gpsNode->setBoolValue("config/delegate-sequencing", true);
+    gpsNode->setDoubleValue("config/max-fly-by-turn-angle-deg", 75.0);
+    gpsNode->setStringValue("command", "leg");
+
+    FGPositioned::TypeFilter fixFilter(FGPositioned::FIX);
+    auto tiravFix = FGPositioned::findFirstWithIdent("TIRAV", &fixFilter);
+    auto bobriFix = FGPositioned::findFirstWithIdent("BOBRI", &fixFilter);
+    auto adabiFix = FGPositioned::findFirstWithIdent("ADABI", &fixFilter);
+
+    auto pilot = SGSharedPtr<FGTestApi::TestPilot>(new FGTestApi::TestPilot);
+    pilot->resetAtPosition(initPos);
+    pilot->setCourseTrue(fp->legAtIndex(4)->courseDeg());
+    pilot->setSpeedKts(280);
+    pilot->flyGPSCourse(gps);
+
+    bool ok = FGTestApi::runForTimeWithCheck(1200.0, [&fp]() {
+        return fp->currentIndex() == 5;
+    });
+    CPPUNIT_ASSERT(ok);
+    // ensure we did a fly-over
+    CPPUNIT_ASSERT(!gps->previousLegData().value().didFlyBy);
+    FGTestApi::writePointToKML("seq point", globals->get_aircraft_position());
+
+    // ensure leg course is the one we want
+    const auto crs = SGGeodesy::courseDeg(bobriFix->geod(), adabiFix->geod());
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(crs, gpsNode->getDoubleValue("wp/leg-true-course-deg"), 1.0);
+
+    // ensure we fly the comepnsation turns, so that end up back on the next leg's course
+    // exactly
+    FGTestApi::runForTime(60.0);
+
+
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(crs, gpsNode->getDoubleValue("wp/leg-true-course-deg"), 1.0);
+}
+
 void GPSTests::testRadialIntercept()
 {
    // FGTestApi::setUp::logPositionToKML("gps_radial_intercept");
@@ -1624,19 +1685,26 @@ void GPSTests::testCourseLegIntermediateWaypoint()
 
     auto wpNode = gpsNode->getChild("wp", 0, true);
 
-    CPPUNIT_ASSERT_DOUBLES_EQUAL(56.5, legToDecel->courseDeg(), 1.0);
-    CPPUNIT_ASSERT_DOUBLES_EQUAL(56.5, legToBlaca->courseDeg(), 1.0);
+    FGPositioned::TypeFilter fixFilter(FGPositioned::FIX);
+    auto lisbo = FGPositioned::findClosestWithIdent("LISBO", fp->departureAirport()->geod(), &fixFilter);
+    auto blaca = FGPositioned::findClosestWithIdent("BLACA", fp->departureAirport()->geod(), &fixFilter);
 
-    CPPUNIT_ASSERT_DOUBLES_EQUAL(56.5, gpsNode->getDoubleValue("desired-course-deg"), 2.0);
-    CPPUNIT_ASSERT_DOUBLES_EQUAL(56.0, wpNode->getDoubleValue("leg-true-course-deg"), 0.5);
-    CPPUNIT_ASSERT_DOUBLES_EQUAL(56.0, wpNode->getDoubleValue("leg-mag-course-deg"), 0.5);
+    const double crsToDecel = SGGeodesy::courseDeg(lisbo->geod(), decelPos);
+    const double crsToBlaca = SGGeodesy::courseDeg(decelPos, blaca->geod());
+
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToDecel, legToDecel->courseDeg(), 1.0);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToBlaca, legToBlaca->courseDeg(), 1.0);
+
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToDecel, gpsNode->getDoubleValue("desired-course-deg"), 2.0);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToDecel, wpNode->getDoubleValue("leg-true-course-deg"), 0.5);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToDecel, wpNode->getDoubleValue("leg-mag-course-deg"), 0.5);
 
     fp->setCurrentIndex(3); // BLACA
 
     FGTestApi::runForTime(0.1);
-    CPPUNIT_ASSERT_DOUBLES_EQUAL(56.5, gpsNode->getDoubleValue("desired-course-deg"), 2.0);
-    CPPUNIT_ASSERT_DOUBLES_EQUAL(56.5, wpNode->getDoubleValue("leg-true-course-deg"), 0.5);
-    CPPUNIT_ASSERT_DOUBLES_EQUAL(56.5, wpNode->getDoubleValue("leg-mag-course-deg"), 0.5);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToBlaca, gpsNode->getDoubleValue("desired-course-deg"), 2.0);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToBlaca, wpNode->getDoubleValue("leg-true-course-deg"), 0.5);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToBlaca, wpNode->getDoubleValue("leg-mag-course-deg"), 0.5);
 
     auto pilot = SGSharedPtr<FGTestApi::TestPilot>(new FGTestApi::TestPilot);
     pilot->resetAtPosition(initPos);

test_suite/unit_tests/Instrumentation/test_gps.hxx

@@ -56,6 +56,7 @@ class GPSTests : public CppUnit::TestFixture
     CPPUNIT_TEST(testDMEIntercept);
     CPPUNIT_TEST(testFinalLegCourse);
     CPPUNIT_TEST(testCourseLegIntermediateWaypoint);
+    CPPUNIT_TEST(testExceedFlyByMaxAngleTurn);
 
     CPPUNIT_TEST_SUITE_END();
 
@@ -92,6 +93,7 @@ public:
     void testDMEIntercept();
     void testFinalLegCourse();
     void testCourseLegIntermediateWaypoint();
+    void testExceedFlyByMaxAngleTurn();
 };
 
 #endif  // _FG_GPS_UNIT_TESTS_HXX