Tests: add RNAV Heading-to-Alt basic test

test_suite/FGTestApi/TestPilot.cxx

@@ -80,6 +80,11 @@ void TestPilot::setSpeedKts(double knots)
     _speedKnots = knots;
 }
 
+void TestPilot::setVerticalFPM(double fpm)
+{
+    _vspeedFPM = fpm;
+}
+
 void TestPilot::setCourseTrue(double deg)
 {
     _trueCourseDeg = deg;
@@ -233,4 +238,10 @@ void TestPilot::setTargetAltitudeFtMSL(double altFt)
     _altActive = true;
 }
 
+bool TestPilot::isOnHeading(double heading) const
+{
+    const double hdgDelta = (_trueCourseDeg - heading);
+    return fabs(hdgDelta) < 0.5;
+}
+
 } // of namespace

test_suite/FGTestApi/TestPilot.hxx

@@ -61,6 +61,7 @@ public:
     
     void flyGPSCourseOffset(GPS *gps, double offsetNm);
 
+    bool isOnHeading(double heading) const;
 private:
     enum class LateralMode
     {

test_suite/unit_tests/Instrumentation/test_rnav_procedures.cxx

@@ -70,7 +70,7 @@ public:
     {
     }
     
-    virtual void departureChanged() override
+    void departureChanged() override
     {
         // mimic the default delegate, inserting the SID waypoints
         
@@ -149,6 +149,14 @@ void RNAVProcedureTests::setupRouteManager()
     rm->postinit();
 }
 
+void RNAVProcedureTests::setPositionAndStabilise(const SGGeod& g)
+{
+    FGTestApi::setPosition(g);
+    for (int i=0; i<60; ++i) {
+        m_gps->update(0.015);
+    }
+}
+
 /////////////////////////////////////////////////////////////////////////////
 
 #if 0
@@ -166,6 +174,93 @@ void RNAVProcedureTests::testBasic()
 }
 #endif
 
+void RNAVProcedureTests::testHeadingToAlt()
+{
+    auto vhhh = FGAirport::findByIdent("VHHH");
+    FGTestApi::setUp::logPositionToKML("heading_to_alt");
+
+    auto rm = globals->get_subsystem<FGRouteMgr>();
+    auto fp = new FlightPlan;
+    
+    auto testDelegate = new TestFPDelegate;
+    testDelegate->thePlan = fp;
+    fp->addDelegate(testDelegate);
+
+    rm->setFlightPlan(fp);
+    
+    // we don't have Nasal, but our delegate does the same work
+    FGTestApi::setUp::populateFPWithNasal(fp, "VHHH", "25R", "EGLL", "27R", "HAZEL");
+    
+    auto wp = new HeadingToAltitude(fp, "TO_4000", 270);
+    wp->setAltitude(4000, RESTRICT_ABOVE);
+    fp->insertWayptAtIndex(wp, 1); // between the runway WP and HAZEL
+
+    FGTestApi::writeFlightPlanToKML(fp);
+
+    auto depRwy = fp->departureRunway();
+    
+    setupStandardGPS();
+    setPositionAndStabilise(fp->departureRunway()->pointOnCenterline(0.0));
+
+    fp->activate();
+    m_gpsNode->setStringValue("command", "leg");
+
+    CPPUNIT_ASSERT_EQUAL(fp->currentIndex(), 0);
+
+    auto pilot = SGSharedPtr<FGTestApi::TestPilot>(new FGTestApi::TestPilot);
+    pilot->resetAtPosition(depRwy->pointOnCenterline(0.0));
+    pilot->setCourseTrue(depRwy->headingDeg()); 
+    pilot->setSpeedKts(200);
+    pilot->flyGPSCourse(m_gps);
+    
+    // check we sequence to the heading-to-alt wp
+    bool ok = FGTestApi::runForTimeWithCheck(240.0, [fp] () {
+      if (fp->currentIndex() == 1) {
+          return true;
+      }
+      return false;
+    });
+    CPPUNIT_ASSERT(ok);
+
+    // leisurely climb out
+    pilot->setVerticalFPM(1800);
+    pilot->setTargetAltitudeFtMSL(8000);
+
+    CPPUNIT_ASSERT_EQUAL(std::string{"VHHH-25R"}, std::string{m_gpsNode->getStringValue("wp/wp[0]/ID")});
+    CPPUNIT_ASSERT_EQUAL(std::string{"TO_4000"}, std::string{m_gpsNode->getStringValue("wp/wp[1]/ID")});
+    
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(270.0, m_gpsNode->getDoubleValue("wp/wp[1]/bearing-true-deg"), 0.5);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(270.0, m_gpsNode->getDoubleValue("wp/leg-true-course-deg"), 0.5);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.05);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5);
+    
+    // fly until we're turned to to heading
+    ok = FGTestApi::runForTimeWithCheck(20, [pilot] () {
+        return pilot->isOnHeading(270.);
+    });
+    CPPUNIT_ASSERT(ok);
+    
+    // capture the position now
+    SGGeod posAtHdgAltStart = globals->get_aircraft_position();
+    FGTestApi::runForTime(40.0);
+    
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(270.0, m_gpsNode->getDoubleValue("wp/wp[1]/bearing-true-deg"), 0.5);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(270.0, m_gpsNode->getDoubleValue("wp/leg-true-course-deg"), 0.5);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.05);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5);
+    
+    const double crs = SGGeodesy::courseDeg(posAtHdgAltStart, globals->get_aircraft_position());
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(270.0, crs, 1.0);
+    
+    ok = FGTestApi::runForTimeWithCheck(180.0, [fp] () {
+        return (fp->currentIndex() == 2);
+    });
+    CPPUNIT_ASSERT(ok);
+    
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(4000.0, globals->get_aircraft_position().getElevationFt(), 100.0);
+
+     FGTestApi::runForTime(40.0);
+}
 
 void RNAVProcedureTests::testEGPH_TLA6C()
 {
@@ -187,7 +282,7 @@ void RNAVProcedureTests::testEGPH_TLA6C()
     fp->addDelegate(testDelegate);
     
     rm->setFlightPlan(fp);
-    FGTestApi::setUp::populateFPWithoutNasal(fp, "EGPH", "24", "EGLL", "27R", "DCS POL DTY");
+    FGTestApi::setUp::populateFPWithNasal(fp, "EGPH", "24", "EGLL", "27R", "DCS POL DTY");
     
     fp->setSID(sid);
     

test_suite/unit_tests/Instrumentation/test_rnav_procedures.hxx

@@ -39,11 +39,11 @@ class RNAVProcedureTests : public CppUnit::TestFixture
     CPPUNIT_TEST_SUITE(RNAVProcedureTests);
     
     CPPUNIT_TEST(testEGPH_TLA6C);
-  
+    CPPUNIT_TEST(testHeadingToAlt);
     
     CPPUNIT_TEST_SUITE_END();
 
-    void setPositionAndStabilise(GPS* gps, const SGGeod& g);
+    void setPositionAndStabilise(const SGGeod& g);
 
     GPS* setupStandardGPS(SGPropertyNode_ptr config = {},
         const std::string name = "gps", const int index = 0);
@@ -60,7 +60,7 @@ public:
     //void testBasic();
     
     void testEGPH_TLA6C();
-    
+    void testHeadingToAlt();
 private:
     GPS* m_gps = nullptr;
     SGPropertyNode_ptr m_gpsNode;