diff --git a/test_suite/FGTestApi/TestPilot.cxx b/test_suite/FGTestApi/TestPilot.cxx index 2e9b5886d..bab12e0f0 100644 --- a/test_suite/FGTestApi/TestPilot.cxx +++ b/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); } } diff --git a/test_suite/FGTestApi/globals.cxx b/test_suite/FGTestApi/globals.cxx index b4d73972a..e68f231e4 100644 --- a/test_suite/FGTestApi/globals.cxx +++ b/test_suite/FGTestApi/globals.cxx @@ -24,6 +24,7 @@ #include #include #include +#include #include @@ -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; inumLegs(); ++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 { diff --git a/test_suite/FGTestApi/globals.hxx b/test_suite/FGTestApi/globals.hxx index 032ae7099..b6ef85507 100644 --- a/test_suite/FGTestApi/globals.hxx +++ b/test_suite/FGTestApi/globals.hxx @@ -3,10 +3,13 @@ #include #include +#include +#include #include -class SGGeod; +typedef std::vector 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 runForTimeWithCheck(double t, RunCheck check); + +void writeFlightPlanToKML(flightgear::FlightPlanRef fp); + +void writeGeodsToKML(const std::string &label, const SGGeodVec& geods); namespace tearDown { diff --git a/test_suite/unit_tests/Instrumentation/test_gps.cxx b/test_suite/unit_tests/Instrumentation/test_gps.cxx index c0a7f1668..d85a2f336 100644 --- a/test_suite/unit_tests/Instrumentation/test_gps.cxx +++ b/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(); + 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(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(); + 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(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() { } diff --git a/test_suite/unit_tests/Instrumentation/test_gps.hxx b/test_suite/unit_tests/Instrumentation/test_gps.hxx index 747c79c43..4e0685ed8 100644 --- a/test_suite/unit_tests/Instrumentation/test_gps.hxx +++ b/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