GPS/FP radial-intercept tests

By Jonathan Redpath, with some additions from me. Starting to exercise
both GPS and route-path for radial intercepts

test_suite/unit_tests/Instrumentation/test_gps.cxx

@@ -1215,3 +1215,61 @@ void GPSTests::testTurnAnticipation()
 {
 }
 
+void GPSTests::testRadialIntercept()
+{
+    //FGTestApi::setUp::logPositionToKML("gps_radial_intercept");
+
+    auto rm = globals->get_subsystem<FGRouteMgr>();
+    auto fp = new FlightPlan;
+    rm->setFlightPlan(fp);
+       
+    FGTestApi::setUp::populateFPWithoutNasal(fp, "LFKC", "36", "LIRF", "25", "BUNAX BEBEV AJO");
+    
+    // insert KC502 manually
+    fp->insertWayptAtIndex(new BasicWaypt(SGGeod::fromDeg(8.78333, 42.566), "KC502", fp), 1);
+    
+    
+    SGGeod pos = SGGeod::fromDeg(8.445556,42.216944);
+    auto intc = new RadialIntercept(fp, "INTC", pos, 230, 5);
+    fp->insertWayptAtIndex(intc, 3);
+    
+  //  FGTestApi::writeFlightPlanToKML(fp);
+    
+// position slightly before BUNAX
+    SGGeod initPos = fp->pointAlongRoute(2, -3.0);
+    
+    // takes the place of the Nasal delegates
+    auto testDelegate = new TestFPDelegate;
+    testDelegate->thePlan = fp;
+    CPPUNIT_ASSERT(rm->activate());
+    fp->addDelegate(testDelegate);
+    auto gps = setupStandardGPS();
+    
+    FGTestApi::setPositionAndStabilise(initPos);
+    
+    auto gpsNode = globals->get_props()->getNode("instrumentation/gps");
+    gpsNode->setBoolValue("config/delegate-sequencing", true);
+    gpsNode->setStringValue("command", "leg");
+    
+    fp->setCurrentIndex(2);
+
+    CPPUNIT_ASSERT_EQUAL(string{"BUNAX"}, string{gpsNode->getStringValue("wp/wp[1]/ID")});
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(312, gpsNode->getDoubleValue("wp/leg-true-course-deg"), 1.0);
+
+  
+    auto pilot = SGSharedPtr<FGTestApi::TestPilot>(new FGTestApi::TestPilot);
+    pilot->resetAtPosition(initPos);
+    pilot->setCourseTrue(fp->legAtIndex(2)->courseDeg());
+    pilot->setSpeedKts(300); // decent speed to make things tougher
+    pilot->flyGPSCourse(gps);
+    
+    bool ok = FGTestApi::runForTimeWithCheck(600.0, [fp]() {
+        return fp->currentIndex() == 4;
+    });
+    CPPUNIT_ASSERT(ok);
+    
+    // flying to BEBEV now
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(185, gpsNode->getDoubleValue("wp/leg-true-course-deg"), 1.0);
+    
+    FGTestApi::runForTime(30.0);
+}

test_suite/unit_tests/Instrumentation/test_gps.hxx

@@ -52,6 +52,7 @@ class GPSTests : public CppUnit::TestFixture
     CPPUNIT_TEST(testLegIntercept);
     CPPUNIT_TEST(testDirectToLegOnFlightplanAndResumeBuiltin);
     CPPUNIT_TEST(testBuiltinRevertToOBSAtEnd);
+    CPPUNIT_TEST(testRadialIntercept);
     
     CPPUNIT_TEST_SUITE_END();
 
@@ -84,6 +85,7 @@ public:
     void testLegIntercept();
     void testDirectToLegOnFlightplanAndResumeBuiltin();
     void testBuiltinRevertToOBSAtEnd();
+    void testRadialIntercept();
 };
 
 #endif  // _FG_GPS_UNIT_TESTS_HXX

test_suite/unit_tests/Instrumentation/test_rnav_procedures.cxx

@@ -177,7 +177,7 @@ void RNAVProcedureTests::testBasic()
 void RNAVProcedureTests::testHeadingToAlt()
 {
     auto vhhh = FGAirport::findByIdent("VHHH");
-    FGTestApi::setUp::logPositionToKML("heading_to_alt");
+   // FGTestApi::setUp::logPositionToKML("heading_to_alt");
 
     auto rm = globals->get_subsystem<FGRouteMgr>();
     auto fp = new FlightPlan;
@@ -195,7 +195,7 @@ void RNAVProcedureTests::testHeadingToAlt()
     wp->setAltitude(4000, RESTRICT_ABOVE);
     fp->insertWayptAtIndex(wp, 1); // between the runway WP and HAZEL
 
-    FGTestApi::writeFlightPlanToKML(fp);
+   // FGTestApi::writeFlightPlanToKML(fp);
 
     auto depRwy = fp->departureRunway();
     
@@ -267,7 +267,7 @@ void RNAVProcedureTests::testHeadingToAlt()
 void RNAVProcedureTests::testUglyHeadingToAlt()
 {
     auto vhhh = FGAirport::findByIdent("VHHH");
-    FGTestApi::setUp::logPositionToKML("heading_to_alt_ugly");
+  //  FGTestApi::setUp::logPositionToKML("heading_to_alt_ugly");
 
     auto rm = globals->get_subsystem<FGRouteMgr>();
     auto fp = new FlightPlan;
@@ -285,7 +285,7 @@ void RNAVProcedureTests::testUglyHeadingToAlt()
     wp->setAltitude(4000, RESTRICT_ABOVE);
     fp->insertWayptAtIndex(wp, 1); // between the runway WP and HAZEL
 
-    FGTestApi::writeFlightPlanToKML(fp);
+  //  FGTestApi::writeFlightPlanToKML(fp);
 
     auto depRwy = fp->departureRunway();
     
@@ -363,7 +363,7 @@ void RNAVProcedureTests::testEGPH_TLA6C()
     if (!sid)
         return;
     
-    FGTestApi::setUp::logPositionToKML("procedure_egph_tla6c");
+   // FGTestApi::setUp::logPositionToKML("procedure_egph_tla6c");
 
     auto rm = globals->get_subsystem<FGRouteMgr>();
     auto fp = new FlightPlan;
@@ -388,7 +388,7 @@ void RNAVProcedureTests::testEGPH_TLA6C()
     
     CPPUNIT_ASSERT_EQUAL(std::string{"D346T"}, fp->legAtIndex(3)->waypoint()->ident());
     
-    FGTestApi::writeFlightPlanToKML(fp);
+  //  FGTestApi::writeFlightPlanToKML(fp);
 
     CPPUNIT_ASSERT(rm->activate());
    

test_suite/unit_tests/Navaids/test_flightplan.cxx

@@ -489,3 +489,18 @@ void FlightplanTests::testLeadingWPDynamic()
     // distance will be invalid, but shouldn;t assert or crash :)
     CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, f->totalDistanceNm(), 0.001);
 }
+
+void FlightplanTests::testRadialIntercept()
+{
+    // replicate AJO1R departure
+    FlightPlanRef f = makeTestFP("LFKC", "36", "LIRF", "25", "BUNAX BEBEV AJO");
+    
+    f->insertWayptAtIndex(new BasicWaypt(SGGeod::fromDeg(8.78333, 42.566), "KC502", f), 1);
+
+    SGGeod pos = SGGeod::fromDeg(8.445556,42.216944);
+    auto intc = new RadialIntercept(f, "INTC", pos, 230, 5);
+    f->insertWayptAtIndex(intc, 3); // between BUNAX and BEBEV
+    
+    RoutePath rtepath(f);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(232, f->legAtIndex(3)->courseDeg(), 1.0);
+}

test_suite/unit_tests/Navaids/test_flightplan.hxx

@@ -44,6 +44,7 @@ class FlightplanTests : public CppUnit::TestFixture
     CPPUNIT_TEST(testOnlyDiscontinuityRoute);
     CPPUNIT_TEST(testBasicDiscontinuity);
     CPPUNIT_TEST(testLeadingWPDynamic);
+    CPPUNIT_TEST(testRadialIntercept);
     
   //  CPPUNIT_TEST(testParseICAORoute);
    // CPPUNIT_TEST(testParseICANLowLevelRoute);
@@ -72,6 +73,7 @@ public:
     void testBasicDiscontinuity();
     void testOnlyDiscontinuityRoute();
     void testLeadingWPDynamic();
+    void testRadialIntercept();
 };
 
 #endif  // FG_FLIGHTPLAN_UNIT_TESTS_HXX

test_suite/unit_tests/Navaids/test_routeManager.cxx

@@ -281,7 +281,7 @@ void RouteManagerTests::testDirectToLegOnFlightplanAndResume()
 
 void RouteManagerTests::testSequenceDiscontinuityAndResume()
 {
-    FGTestApi::setUp::logPositionToKML("rm_seq_discon_resume_leg");
+  //  FGTestApi::setUp::logPositionToKML("rm_seq_discon_resume_leg");
 
     FlightPlanRef fp1 = makeTestFP("LIRF", "16R", "LEBL", "07L",
                                     "GITRI BALEN MUREN TOSNU");