More GPS sequencing tests

Also extended the test helpers in various ways.

test_suite/FGTestApi/TestPilot.cxx

@@ -113,9 +113,20 @@ void TestPilot::flyDirectTo(const SGGeod& target)
 void TestPilot::updateValues(double dt)
 {
     if (_gps && (_lateralMode == LateralMode::GPSCourse)) {
-        const double deviationDeg = _gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg");
+        double deviationDeg = _gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg");
         _targetCourseDeg = _gpsNode->getDoubleValue("wp/leg-true-course-deg");
-        _targetCourseDeg += deviationDeg;
+        
+        const double absDev = fabs(deviationDeg);
+        const double minInterceptAngle = 1.5;
+        // if we're getting close to the leg track, artifically keep the deviation a bit up,
+        // to avoid really slow convergence on it
+        if ((absDev > 0.05) && (absDev < minInterceptAngle)) {
+            deviationDeg = copysign(minInterceptAngle, deviationDeg);
+        }
+        
+        // set how aggressively we try to correct our course
+        const double courseCorrectionFactor = 8.0;
+        _targetCourseDeg += courseCorrectionFactor * deviationDeg;
         
         SG_NORMALIZE_RANGE(_targetCourseDeg, 0.0, 360.0);
         if (!_turnActive &&(fabs(_trueCourseDeg - _targetCourseDeg) > 0.5)) {
@@ -145,9 +156,14 @@ void TestPilot::updateValues(double dt)
                 errorDeg += 360.0;
             }
             
+            // clamp turn to error value
+            turnDeg = std::min(turnDeg, fabs(errorDeg));
+            
+            // and now ensure we follow the correct sign
             turnDeg = copysign(turnDeg, errorDeg);
+            
             // simple integral
-            _trueCourseDeg += std::min(turnDeg, errorDeg);
+            _trueCourseDeg += turnDeg;
             SG_NORMALIZE_RANGE(_trueCourseDeg, 0.0, 360.0);
         }
     }

test_suite/FGTestApi/globals.cxx

@@ -24,6 +24,7 @@
 #include <Airports/airport.hxx>
 #include <Navaids/FlightPlan.hxx>
 #include <Navaids/waypoint.hxx>
+#include <Navaids/routePath.hxx>
 
 #include <Scripting/NasalSys.hxx>
 
@@ -270,6 +271,35 @@ bool runForTimeWithCheck(double t, RunCheck check)
     
     return false;
 }
+
+void writeFlightPlanToKML(flightgear::FlightPlanRef fp)
+{
+    RoutePath rpath(fp);
+    
+    SGGeodVec fullPath;
+    for (int i=0; i<fp->numLegs(); ++i) {
+        SGGeodVec legPath = rpath.pathForIndex(i);
+        fullPath.insert(fullPath.end(), legPath.begin(), legPath.end());
+    }
+    
+    writeGeodsToKML("FlightPlan", fullPath);
+}
+    
+void writeGeodsToKML(const std::string &label, const SGGeodVec& geods)
+{
+    if (global_lineStringOpen) {
+        endCurrentLineString();
+    }
+    
+    beginLineString(label);
+    
+    for (const auto& g : geods) {
+        logCoordinate(g);
+    }
+    
+    endCurrentLineString();
+}
+
     
 namespace tearDown {
 

test_suite/FGTestApi/globals.hxx

@@ -3,10 +3,13 @@
 
 #include <string>
 #include <functional>
+#include <vector>
 
+#include <simgear/math/SGGeod.hxx>
 #include <simgear/structure/SGSharedPtr.hxx>
 
-class SGGeod;
+typedef std::vector<SGGeod> SGGeodVec;
+
 
 namespace flightgear
 {
@@ -21,7 +24,7 @@ namespace setUp {
 void initTestGlobals(const std::string& testName);
 
 bool logPositionToKML(const std::string& testName);
-
+    
 void initStandardNasal();
 
 void populateFPWithoutNasal(flightgear::FlightPlanRef f,
@@ -43,6 +46,10 @@ void runForTime(double t);
     using RunCheck = std::function<bool(void)>;
     
 bool runForTimeWithCheck(double t, RunCheck check);
+
+void writeFlightPlanToKML(flightgear::FlightPlanRef fp);
+    
+void writeGeodsToKML(const std::string &label, const SGGeodVec& geods);
     
 namespace tearDown {
 

test_suite/unit_tests/Instrumentation/test_gps.cxx

@@ -645,6 +645,256 @@ void GPSTests::testLongLegWestbound()
     CPPUNIT_ASSERT_DOUBLES_EQUAL(expectedDefl1 * 10.0, gpsNode->getDoubleValue("cdi-deflection"), 0.01);
 }
 
+void GPSTests::testOverflightSequencing()
+{
+    // check that we sequence as we pass over the top
+    
+    FGTestApi::setUp::logPositionToKML("gps_sequence");
+    auto rm = globals->get_subsystem<FGRouteMgr>();
+    auto fp = new FlightPlan;
+    rm->setFlightPlan(fp);
+    
+    // let's use New Zealand for some southern hemisphere confusion :)
+    
+    // this route has some deliberately sharp turns to work the turn code
+    FGTestApi::setUp::populateFPWithoutNasal(fp, "NZCH", "02", "NZAA", "05L",
+                                             "ALADA NS WB WN MAMOD KAPTI OH");
+    
+    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();
+    
+    auto gpsNode = globals->get_props()->getNode("instrumentation/gps");
+    gpsNode->setStringValue("command", "leg");
+    
+    // very tight tolerance on the overflight. We need a little bit or the
+    // test-piot gets confused right on top
+    gpsNode->setDoubleValue("config/over-flight-distance-nm", 0.01);
+    
+    // enroute to NELSON VOR
+    fp->setCurrentIndex(2);
+    SGGeod pos = fp->pointAlongRoute(2, -5.0);
+    setPositionAndStabilise(gps, pos);
+    
+    auto pilot = SGSharedPtr<FGTestApi::TestPilot>(new FGTestApi::TestPilot);
+    pilot->resetAtPosition(pos);
+    pilot->setSpeedKts(300); // decent speed to make things tougher
+    pilot->flyGPSCourse(gps);
+    
+    bool ok = FGTestApi::runForTimeWithCheck(180.0, [fp] () {
+        if (fp->currentIndex() == 3) {
+            return true;
+        }
+        return false;
+    });
+    CPPUNIT_ASSERT(ok);
+    
+    // check we're on top of NELSON
+    auto nelsonVOR = fp->legAtIndex(2)->waypoint()->source();
+    CPPUNIT_ASSERT_EQUAL(std::string{"NELSON VOR-DME"}, nelsonVOR->name());
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, SGGeodesy::distanceNm(globals->get_aircraft_position(),  nelsonVOR->geod()), 0.1);
+    
+    FGTestApi::runForTime(120.0);
+    // check we're on course to Woodburne
+    
+    auto woodbourneVOR = fp->legAtIndex(3)->waypoint()->source();
+    const double crsToWB = SGGeodesy::courseDeg(globals->get_aircraft_position(),  woodbourneVOR->geod());
+    const double crsNSWB = SGGeodesy::courseDeg(nelsonVOR->geod(), woodbourneVOR->geod());
+    
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToWB, gpsNode->getDoubleValue("wp/wp[1]/bearing-true-deg"), 0.5);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(crsNSWB, gpsNode->getDoubleValue("wp/leg-true-course-deg"), 0.5);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.05);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5);
+    
+    // point a bit before MAMOD
+    SGGeod pos2 = fp->pointAlongRoute(5, -5.0);
+    setPositionAndStabilise(gps, pos2);
+    fp->setCurrentIndex(5);
+    auto mamod = fp->legAtIndex(5)->waypoint()->source();
+
+    ok = FGTestApi::runForTimeWithCheck(180.0, [fp] () {
+        if (fp->currentIndex() == 6) {
+            return true;
+        }
+        return false;
+    });
+    CPPUNIT_ASSERT(ok);
+    
+    // check we're on top of MAMOD
+    CPPUNIT_ASSERT_EQUAL(std::string{"MAMOD"}, mamod->name());
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, SGGeodesy::distanceNm(globals->get_aircraft_position(),  mamod->geod()), 0.1);
+    
+    FGTestApi::runForTime(180.0);
+    // check we're on course to KAPTI
+    
+    auto kapti = fp->legAtIndex(6)->waypoint()->source();
+    const double crsKapti = SGGeodesy::courseDeg(globals->get_aircraft_position(),  kapti->geod());
+    const double crsMamodKapti = SGGeodesy::courseDeg(mamod->geod(), kapti->geod());
+    
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(crsKapti, gpsNode->getDoubleValue("wp/wp[1]/bearing-true-deg"), 0.5);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(crsMamodKapti, gpsNode->getDoubleValue("wp/leg-true-course-deg"), 0.5);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.05);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5);
+}
+
+void GPSTests::testOffcourseSequencing()
+{
+    // ensure that if we pass through the overflight arm cone, but not
+    // over the waypoint direclty, we still sequence at the mid-point
+    
+    FGTestApi::setUp::logPositionToKML("gps_sequence_off");
+    auto rm = globals->get_subsystem<FGRouteMgr>();
+    auto fp = new FlightPlan;
+    rm->setFlightPlan(fp);
+    
+    
+    FGTestApi::setUp::populateFPWithoutNasal(fp, "NZCH", "02", "NZAA", "05L",
+                                             "ALADA NS WB WN MAMOD KAPTI OH");
+
+    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();
+    
+    auto gpsNode = globals->get_props()->getNode("instrumentation/gps");
+    gpsNode->setStringValue("command", "leg");
+    gpsNode->setDoubleValue("config/over-flight-distance-nm", 0.01);
+    
+    // enroute to NELSON VOR
+    fp->setCurrentIndex(2);
+    SGGeod pos = fp->pointAlongRoute(2, -5.0);
+    
+    pos = SGGeodesy::direct(pos, 180, 1.5 * SG_NM_TO_METER);
+    setPositionAndStabilise(gps, pos);
+    
+    
+    auto nelsonVOR = fp->legAtIndex(2)->waypoint()->source();
+    CPPUNIT_ASSERT_EQUAL(std::string{"NELSON VOR-DME"}, nelsonVOR->name());
+    
+    auto pilot = SGSharedPtr<FGTestApi::TestPilot>(new FGTestApi::TestPilot);
+    pilot->resetAtPosition(pos);
+    pilot->setSpeedKts(300); // decent speed to make things tougher
+    
+    // since we're south of the route, but will fly parallel to it, we won't
+    // pass over NELSON directly.
+    
+    const double legCourse =  gpsNode->getDoubleValue("wp/leg-true-course-deg");
+    pilot->turnToCourse(legCourse);
+    
+    bool ok = FGTestApi::runForTimeWithCheck(180.0, [fp] () {
+        if (fp->currentIndex() == 3) {
+            return true;
+        }
+        return false;
+    });
+    CPPUNIT_ASSERT(ok);
+    
+    // check we're close NELSON
+    const double distToNELSON = SGGeodesy::distanceNm(globals->get_aircraft_position(),  nelsonVOR->geod());
+    CPPUNIT_ASSERT(distToNELSON < 1.0);
+    
+    const double bearingToNELSON = SGGeodesy::courseDeg(globals->get_aircraft_position(),  nelsonVOR->geod());
+    // find the inbound course from ALADA at NELSON
+    double finalLegTrack = SGGeodesy::courseDeg(nelsonVOR->geod(), fp->legAtIndex(1)->waypoint()->position()) + 180.0;
+    SG_NORMALIZE_RANGE(finalLegTrack, 0.0, 360.0);
+    
+    double dev = bearingToNELSON - finalLegTrack;
+    SG_NORMALIZE_RANGE(dev, -180.0, 180.0);
+    double absDeviation = fabs(dev);
+    
+    // 90 is the overflight arm angle, i.e we sequence as soon as the waypoint is abeam us
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(90.0, absDeviation, 1.0);
+    
+    pilot->flyGPSCourse(gps);
+    FGTestApi::runForTime(120.0);
+    // check we're on course to Woodburne
+    
+    auto woodbourneVOR = fp->legAtIndex(3)->waypoint()->source();
+    const double crsToWB = SGGeodesy::courseDeg(globals->get_aircraft_position(),  woodbourneVOR->geod());
+    const double crsNSWB = SGGeodesy::courseDeg(nelsonVOR->geod(), woodbourneVOR->geod());
+    
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToWB, gpsNode->getDoubleValue("wp/wp[1]/bearing-true-deg"), 0.5);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(crsNSWB, gpsNode->getDoubleValue("wp/leg-true-course-deg"), 0.5);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.05);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5);
+    
+   
+    // let's go wide at MAMOD
+    SGGeod pos2 = fp->pointAlongRoute(5, -5.0);
+    pos2 = SGGeodesy::direct(pos2, 180, 0.8 * SG_NM_TO_METER);
+
+    fp->setCurrentIndex(5);
+    setPositionAndStabilise(gps, pos2);
+    // start facing the right way
+    pilot->setCourseTrue(gpsNode->getDoubleValue("wp/leg-true-course-deg"));
+    pilot->flyHeading(gpsNode->getDoubleValue("wp/leg-true-course-deg"));
+    auto mamod = fp->legAtIndex(5)->waypoint()->source();
+    
+    ok = FGTestApi::runForTimeWithCheck(180.0, [fp] () {
+        if (fp->currentIndex() == 6) {
+            return true;
+        }
+        return false;
+    });
+    CPPUNIT_ASSERT(ok);
+    
+    // check we're on top of MAMOD
+    CPPUNIT_ASSERT_EQUAL(std::string{"MAMOD"}, mamod->name());
+    const double distToMAMOD = SGGeodesy::distanceNm(globals->get_aircraft_position(),  mamod->geod());
+    CPPUNIT_ASSERT(distToMAMOD < 1.0);
+    
+    // check the angle we sequenced at
+    const double bearingToMAMOD = SGGeodesy::courseDeg(globals->get_aircraft_position(),  mamod->geod());
+    // find the inbound course from WELLINGTON at MAMOD
+    finalLegTrack = SGGeodesy::courseDeg(mamod->geod(), fp->legAtIndex(4)->waypoint()->position()) + 180.0;
+    
+    SG_NORMALIZE_RANGE(finalLegTrack, 0.0, 360.0);
+    dev = bearingToMAMOD - finalLegTrack;
+    SG_NORMALIZE_RANGE(dev, -180.0, 180.0);
+    absDeviation = fabs(dev);
+    
+    // 90 is the overflight arm angle, i.e we sequence as soon as the waypoint is abeam us
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(90.0, absDeviation, 1.0);
+    
+ 
+    // check we don't sequence, if we're too far off course
+    
+    SGGeod pos3 = fp->pointAlongRoute(6, -5.0);
+    pos3 = SGGeodesy::direct(pos3, 90, 1.8 * SG_NM_TO_METER);
+    
+    fp->setCurrentIndex(6);
+    setPositionAndStabilise(gps, pos3);
+    // start facing the right way
+    pilot->setCourseTrue(gpsNode->getDoubleValue("wp/leg-true-course-deg"));
+    pilot->flyHeading(gpsNode->getDoubleValue("wp/leg-true-course-deg"));
+    
+    ok = FGTestApi::runForTimeWithCheck(180.0, [fp] () {
+        if (fp->currentIndex() != 6) {
+            CPPUNIT_ASSERT(false);
+            return true;
+        }
+        return false;
+    });
+    
+    
+}
+
+void GPSTests::testOffsetFlight()
+{
+    // verify the XTK value during waypoint transitions, especially that we
+    // don't get any weird discontinuities
+    
+}
+
 void GPSTests::testTurnAnticipation()
 {
 }

test_suite/unit_tests/Instrumentation/test_gps.hxx

@@ -46,6 +46,9 @@ class GPSTests : public CppUnit::TestFixture
     CPPUNIT_TEST(testDirectToLegOnFlightplan);
     CPPUNIT_TEST(testLongLeg);
     CPPUNIT_TEST(testLongLegWestbound);
+    CPPUNIT_TEST(testOffsetFlight);
+    CPPUNIT_TEST(testOverflightSequencing);
+    CPPUNIT_TEST(testOffcourseSequencing);
     
     CPPUNIT_TEST_SUITE_END();
 
@@ -72,6 +75,9 @@ public:
     void testDirectToLegOnFlightplan();
     void testLongLeg();
     void testLongLegWestbound();
+    void testOffsetFlight();
+    void testOffcourseSequencing();
+    void testOverflightSequencing();
 };
 
 #endif  // _FG_GPS_UNIT_TESTS_HXX