fly/flightgear
1
0
Fork 0
Code Activity

Basic flight-plan/leg tests for GPS

Browse source
This commit is contained in:
James Turner 2019-04-23 18:10:39 +01:00
parent 3b34158998
commit 21a8e89a0a
8 changed files with 273 additions and 32 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

21
test_suite/FGTestApi/TestPilot.cxx
View file

@ -50,7 +50,7 @@ void TestPilot::resetAtPosition(const SGGeod& pos)
void TestPilot::init()
{
_vspeedFPM = 1200;
}
void TestPilot::update(double dt)
@ -98,6 +98,18 @@ void TestPilot::updateValues(double dt)
double d = _speedKnots * SG_KT_TO_MPS * dt;
SGGeod newPos = SGGeodesy::direct(currentPos, _trueCourseDeg, d);
if (_altActive) {
if (fabs(_targetAltitudeFt - currentPos.getElevationFt()) < 1) {
_altActive = false;
newPos.setElevationFt(_targetAltitudeFt);
} else {
double errorFt = _targetAltitudeFt - currentPos.getElevationFt();
double vspeed = std::min(fabs(errorFt),_vspeedFPM * dt / 60.0);
double dv = copysign(vspeed, errorFt);
newPos.setElevationFt(currentPos.getElevationFt() + dv);
}
}
setPosition(newPos);
}
@ -109,7 +121,14 @@ void TestPilot::setPosition(const SGGeod& pos)
_propRoot->setDoubleValue("orientation/heading-deg", _trueCourseDeg);
_propRoot->setDoubleValue("velocities/speed-knots", _speedKnots);
_propRoot->setDoubleValue("velocities/vertical-fpm", _vspeedFPM);
}
void TestPilot::setTargetAltitudeFtMSL(double altFt)
{
_targetAltitudeFt = altFt;
_altActive = true;
}
} // of namespace

1
test_suite/FGTestApi/TestPilot.hxx
View file

@ -65,6 +65,7 @@ private:
double _vspeedFPM = 0.0;
bool _turnActive = false;
bool _altActive = false;
double _targetCourseDeg = 0.0;
double _targetAltitudeFt = 0.0;
};

36
test_suite/FGTestApi/globals.cxx
View file

@ -17,6 +17,12 @@
#include <simgear/timing/timestamp.hxx>
#include <simgear/math/sg_geodesy.hxx>
#include <Airports/airport.hxx>
#include <Navaids/FlightPlan.hxx>
#include <Navaids/waypoint.hxx>
using namespace flightgear;
namespace FGTestApi {
namespace setUp {
@ -57,6 +63,36 @@ void initTestGlobals(const std::string& testName)
globals->add_subsystem("events", globals->get_event_mgr(), SGSubsystemMgr::DISPLAY);
}
void populateFP(flightgear::FlightPlanRef f,
const std::string& depICAO, const std::string& depRunway,
const std::string& destICAO, const std::string& destRunway,
const std::string& waypoints)
{
FGAirportRef depApt = FGAirport::getByIdent(depICAO);
f->setDeparture(depApt->getRunwayByIdent(depRunway));
FGAirportRef destApt = FGAirport::getByIdent(destICAO);
f->setDestination(destApt->getRunwayByIdent(destRunway));
// since we don't have the Nasal route-manager delegate, insert the
// runway waypoints manually
f->insertWayptAtIndex(new RunwayWaypt(f->departureRunway(), f), -1);
for (auto ws : simgear::strutils::split(waypoints)) {
WayptRef wpt = f->waypointFromString(ws);
f->insertWayptAtIndex(wpt, -1);
}
auto destRwy = f->destinationRunway();
f->insertWayptAtIndex(new BasicWaypt(destRwy->pointOnCenterline(-8 * SG_NM_TO_METER),
destRwy->ident() + "-8", f), -1);
f->insertWayptAtIndex(new RunwayWaypt(destRwy, f), -1);
}
} // End of namespace setUp.
void setPosition(const SGGeod& g)

12
test_suite/FGTestApi/globals.hxx
View file

@ -2,15 +2,27 @@
#define FG_TEST_GLOBALS_HELPERS_HXX
#include <string>
#include <simgear/structure/SGSharedPtr.hxx>
class SGGeod;
namespace flightgear
{
class FlightPlan;
typedef SGSharedPtr<FlightPlan> FlightPlanRef;
}
namespace FGTestApi {
namespace setUp {
void initTestGlobals(const std::string& testName);
void populateFP(flightgear::FlightPlanRef f,
const std::string& depICAO, const std::string& depRunway,
const std::string& destICAO, const std::string& destRunway,
const std::string& waypoints);
} // End of namespace setUp.
void setPosition(const SGGeod& g);

151
test_suite/unit_tests/Instrumentation/test_gps.cxx
View file

@ -29,10 +29,42 @@
#include <Navaids/NavDataCache.hxx>
#include <Navaids/navrecord.hxx>
#include <Navaids/navlist.hxx>
#include <Navaids/FlightPlan.hxx>
#include <Instrumentation/gps.hxx>
#include <Instrumentation/navradio.hxx>
#include <Autopilot/route_mgr.hxx>
using namespace flightgear;
/////////////////////////////////////////////////////////////////////////////
class TestFPDelegate : public FlightPlan::Delegate
{
public:
FlightPlanRef thePlan;
int sequenceCount = 0;
void sequence() override
{
++sequenceCount;
int newIndex = thePlan->currentIndex() + 1;
if (newIndex >= thePlan->numLegs()) {
thePlan->finish();
return;
}
thePlan->setCurrentIndex(newIndex);
}
void currentWaypointChanged() override
{
}
};
/////////////////////////////////////////////////////////////////////////////
// Set up function for each test.
void GPSTests::setUp()
{
@ -75,6 +107,16 @@ GPS* GPSTests::setupStandardGPS(SGPropertyNode_ptr config,
return gps;
}
void GPSTests::setupRouteManager()
{
auto rm = globals->add_new_subsystem<FGRouteMgr>();
rm->bind();
rm->init();
rm->postinit();
}
/////////////////////////////////////////////////////////////////////////////
void GPSTests::testBasic()
{
auto gps = setupStandardGPS();
@ -187,8 +229,115 @@ void GPSTests::testNavRadioSlave()
std::unique_ptr<FGNavRadio> r(new FGNavRadio(radioConfigNode));
}
void GPSTests::testConfigAutopilotDrive()
void GPSTests::testLegMode()
{
setupRouteManager();
auto rm = globals->get_subsystem<FGRouteMgr>();
auto fp = new FlightPlan;
rm->setFlightPlan(fp);
FGTestApi::setUp::populateFP(fp, "EBBR", "07L", "EGGD", "27",
"NIK COA DVR TAWNY WOD");
// takes the place of the Nasal delegates
auto testDelegate = new TestFPDelegate;
testDelegate->thePlan = fp;
CPPUNIT_ASSERT(rm->activate());
fp->addDelegate(testDelegate);
auto gps = setupStandardGPS();
setPositionAndStabilise(gps, fp->departureRunway()->pointOnCenterline(0.0));
auto gpsNode = globals->get_props()->getNode("instrumentation/gps");
gpsNode->setStringValue("command", "leg");
CPPUNIT_ASSERT_EQUAL(std::string{"leg"}, std::string{gpsNode->getStringValue("mode")});
CPPUNIT_ASSERT_EQUAL(std::string{"EBBR-07L"}, std::string{gpsNode->getStringValue("wp/wp[1]/ID")});
CPPUNIT_ASSERT_DOUBLES_EQUAL(65.0, gpsNode->getDoubleValue("wp/wp[1]/bearing-true-deg"), 0.5);
CPPUNIT_ASSERT_DOUBLES_EQUAL(65.0, gpsNode->getDoubleValue("desired-course-deg"), 0.5);
auto pilot = SGSharedPtr<FGTestApi::TestPilot>(new FGTestApi::TestPilot);
pilot->setSpeedKts(200);
pilot->setCourseTrue(65.0);
pilot->setTargetAltitudeFtMSL(6000);
FGTestApi::runForTime(60.0);
// check we sequenced to NIK
CPPUNIT_ASSERT_EQUAL(1, testDelegate->sequenceCount);
CPPUNIT_ASSERT_EQUAL(1, fp->currentIndex());
CPPUNIT_ASSERT_EQUAL(std::string{"EBBR-07L"}, std::string{gpsNode->getStringValue("wp/wp[0]/ID")});
CPPUNIT_ASSERT_EQUAL(std::string{"NIK"}, std::string{gpsNode->getStringValue("wp/wp[1]/ID")});
// reposition along the leg, closer to NIK
// and fly to COA
SGGeod nikPos = fp->currentLeg()->waypoint()->position();
SGGeod p2 = SGGeodesy::direct(nikPos, 90, 5 * SG_NM_TO_METER); // due east of NIK
setPositionAndStabilise(gps, p2);
CPPUNIT_ASSERT_DOUBLES_EQUAL(270, gpsNode->getDoubleValue("wp/wp[1]/bearing-true-deg"), 0.5);
auto depRwy = fp->departureRunway();
CPPUNIT_ASSERT_DOUBLES_EQUAL(SGGeodesy::distanceNm(nikPos, depRwy->end()), gpsNode->getDoubleValue("wp/leg-distance-nm"), 0.1);
const double legCourse = SGGeodesy::courseDeg(depRwy->end(), nikPos);
CPPUNIT_ASSERT_DOUBLES_EQUAL(legCourse, gpsNode->getDoubleValue("wp/leg-true-course-deg"), 0.5);
//CPPUNIT_ASSERT_DOUBLES_EQUAL(legCourse - 270, gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5);
pilot->setSpeedKts(200);
pilot->setCourseTrue(270);
FGTestApi::runForTime(120.0);
CPPUNIT_ASSERT_EQUAL(2, testDelegate->sequenceCount);
CPPUNIT_ASSERT_EQUAL(2, fp->currentIndex());
CPPUNIT_ASSERT_EQUAL(std::string{"NIK"}, std::string{gpsNode->getStringValue("wp/wp[0]/ID")});
CPPUNIT_ASSERT_EQUAL(std::string{"COA"}, std::string{gpsNode->getStringValue("wp/wp[1]/ID")});
SGGeod coaPos = fp->currentLeg()->waypoint()->position();
CPPUNIT_ASSERT_DOUBLES_EQUAL(SGGeodesy::distanceNm(nikPos, coaPos), gpsNode->getDoubleValue("wp/leg-distance-nm"), 0.1);
CPPUNIT_ASSERT_DOUBLES_EQUAL(SGGeodesy::courseDeg(nikPos, coaPos), gpsNode->getDoubleValue("wp/leg-true-course-deg"), 0.5);
// check manual sequencing
fp->setCurrentIndex(3);
CPPUNIT_ASSERT_EQUAL(std::string{"COA"}, std::string{gpsNode->getStringValue("wp/wp[0]/ID")});
CPPUNIT_ASSERT_EQUAL(std::string{"DVR"}, std::string{gpsNode->getStringValue("wp/wp[1]/ID")});
// check course deviation / cross-track error
SGGeod doverPos = fp->currentLeg()->waypoint()->position();
double course = SGGeodesy::courseDeg(coaPos, doverPos);
CPPUNIT_ASSERT_DOUBLES_EQUAL(course, gpsNode->getDoubleValue("wp/leg-true-course-deg"), 0.5);
SGGeod off1 = SGGeodesy::direct(coaPos, course + 5.0, 8 * SG_NM_TO_METER);
setPositionAndStabilise(gps, off1);
double courseToDover = SGGeodesy::courseDeg(off1, doverPos);
CPPUNIT_ASSERT_DOUBLES_EQUAL(courseToDover, gpsNode->getDoubleValue("wp/wp[1]/bearing-true-deg"), 0.5);
// right of the desired course, negative sign, ho hum
CPPUNIT_ASSERT_DOUBLES_EQUAL(-5.0, gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5);
CPPUNIT_ASSERT_EQUAL(true, gpsNode->getBoolValue("wp/wp[1]/to-flag"));
CPPUNIT_ASSERT_EQUAL(false, gpsNode->getBoolValue("wp/wp[1]/from-flag"));
SGGeod off2 = SGGeodesy::direct(doverPos, course - 5.0, -18 * SG_NM_TO_METER);
setPositionAndStabilise(gps, off2);
courseToDover = SGGeodesy::courseDeg(off2, doverPos);
CPPUNIT_ASSERT_DOUBLES_EQUAL(courseToDover, gpsNode->getDoubleValue("wp/wp[1]/bearing-true-deg"), 0.5);
// disabled becuase GPS course deviation is using from the FROM wp, when
// it should be using the TO point (DVR in this case)
#if 0
CPPUNIT_ASSERT_DOUBLES_EQUAL(5.0, gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5);
#endif
CPPUNIT_ASSERT_EQUAL(true, gpsNode->getBoolValue("wp/wp[1]/to-flag"));
CPPUNIT_ASSERT_EQUAL(false, gpsNode->getBoolValue("wp/wp[1]/from-flag"));
}
void GPSTests::testTurnAnticipation()

7
test_suite/unit_tests/Instrumentation/test_gps.hxx
View file

@ -39,10 +39,10 @@ class GPSTests : public CppUnit::TestFixture
CPPUNIT_TEST_SUITE(GPSTests);
CPPUNIT_TEST(testBasic);
CPPUNIT_TEST(testNavRadioSlave);
CPPUNIT_TEST(testConfigAutopilotDrive);
CPPUNIT_TEST(testTurnAnticipation);
CPPUNIT_TEST(testOBSMode);
CPPUNIT_TEST(testDirectTo);
CPPUNIT_TEST(testLegMode);
CPPUNIT_TEST_SUITE_END();
@ -50,7 +50,8 @@ class GPSTests : public CppUnit::TestFixture
GPS* setupStandardGPS(SGPropertyNode_ptr config = {},
const std::string name = "gps", const int index = 0);
void setupRouteManager();
public:
// Set up function for each test.
void setUp();
@ -61,10 +62,10 @@ public:
// The tests.
void testBasic();
void testNavRadioSlave();
void testConfigAutopilotDrive();
void testTurnAnticipation();
void testOBSMode();
void testDirectTo();
void testLegMode();
};
#endif // _FG_GPS_UNIT_TESTS_HXX

75
test_suite/unit_tests/Navaids/test_flightplan.cxx
View file

@ -33,25 +33,12 @@ void FlightplanTests::tearDown()
FGTestApi::tearDown::shutdownTestGlobals();
}
FlightPlanRef makeTestFP(const std::string& depICAO, const std::string& depRunway,
static FlightPlanRef makeTestFP(const std::string& depICAO, const std::string& depRunway,
const std::string& destICAO, const std::string& destRunway,
const std::string& waypoints)
{
FlightPlanRef f = new FlightPlan;
FGAirportRef depApt = FGAirport::getByIdent(depICAO);
f->setDeparture(depApt->getRunwayByIdent(depRunway));
FGAirportRef destApt = FGAirport::getByIdent(destICAO);
f->setDestination(destApt->getRunwayByIdent(destRunway));
for (auto ws : simgear::strutils::split(waypoints)) {
WayptRef wpt = f->waypointFromString(ws);
f->insertWayptAtIndex(wpt, -1);
}
FGTestApi::setUp::populateFPWithoutNasal(f, depICAO, depRunway, destICAO, destRunway, waypoints);
return f;
}
@ -67,13 +54,18 @@ void FlightplanTests::testBasic()
CPPUNIT_ASSERT(fp1->destinationAirport()->ident() == "EHAM");
CPPUNIT_ASSERT(fp1->destinationRunway()->ident() == "24");
CPPUNIT_ASSERT_EQUAL(fp1->numLegs(), 2);
CPPUNIT_ASSERT_EQUAL(fp1->numLegs(), 5);
CPPUNIT_ASSERT(fp1->legAtIndex(0)->waypoint()->source()->ident() == "TNT");
CPPUNIT_ASSERT(fp1->legAtIndex(0)->waypoint()->source()->name() == "TRENT VOR-DME");
CPPUNIT_ASSERT(fp1->legAtIndex(0)->waypoint()->source()->ident() == "23L");
CPPUNIT_ASSERT(fp1->legAtIndex(1)->waypoint()->source()->ident() == "TNT");
CPPUNIT_ASSERT(fp1->legAtIndex(1)->waypoint()->source()->name() == "TRENT VOR-DME");
CPPUNIT_ASSERT(fp1->legAtIndex(2)->waypoint()->source()->ident() == "CLN");
CPPUNIT_ASSERT(fp1->legAtIndex(2)->waypoint()->source()->name() == "CLACTON VOR-DME");
CPPUNIT_ASSERT(fp1->legAtIndex(4)->waypoint()->source()->ident() == "24");
CPPUNIT_ASSERT(fp1->legAtIndex(1)->waypoint()->source()->ident() == "CLN");
CPPUNIT_ASSERT(fp1->legAtIndex(1)->waypoint()->source()->name() == "CLACTON VOR-DME");
}
void FlightplanTests::testRoutePathBasic()
@ -103,8 +95,8 @@ void FlightplanTests::testRoutePathBasic()
double distM = SGGeodesy::distanceM(bne->geod(), civ->geod());
double trackDeg = SGGeodesy::courseDeg(bne->geod(), civ->geod());
CPPUNIT_ASSERT_DOUBLES_EQUAL(trackDeg, rtepath.trackForIndex(3), 0.5);
CPPUNIT_ASSERT_DOUBLES_EQUAL(distM, rtepath.distanceForIndex(3), 2000); // 2km precision, allow for turns
CPPUNIT_ASSERT_DOUBLES_EQUAL(trackDeg, rtepath.trackForIndex(4), 0.5);
CPPUNIT_ASSERT_DOUBLES_EQUAL(distM, rtepath.distanceForIndex(4), 2000); // 2km precision, allow for turns
}
@ -120,16 +112,16 @@ void FlightplanTests::testRoutePathSkipped()
RoutePath rtepath(fp1);
// skipped point uses inbound track
CPPUNIT_ASSERT_EQUAL(rtepath.trackForIndex(3), rtepath.trackForIndex(4));
CPPUNIT_ASSERT_EQUAL(rtepath.trackForIndex(4), rtepath.trackForIndex(5));
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, rtepath.distanceForIndex(4), 1e-9);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, rtepath.distanceForIndex(5), 1e-9);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, rtepath.distanceForIndex(6), 1e-9);
CPPUNIT_ASSERT_DOUBLES_EQUAL(101000, rtepath.distanceForIndex(6), 1000);
CPPUNIT_ASSERT_DOUBLES_EQUAL(101000, rtepath.distanceForIndex(7), 1000);
// this tests skipping two preceeding points works as it should
SGGeodVec vec = rtepath.pathForIndex(6);
SGGeodVec vec = rtepath.pathForIndex(7);
CPPUNIT_ASSERT(vec.size() == 9);
}
@ -147,7 +139,33 @@ void FlightplanTests::testRoutePathTrivialFlightPlan()
rtepath.distanceForIndex(leg);
}
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, fp1->totalDistanceNm(), 1e-9);
CPPUNIT_ASSERT_DOUBLES_EQUAL(19.68, fp1->totalDistanceNm(), 0.1);
}
void FlightplanTests::testRoutePathVec()
{
FlightPlanRef fp1 = makeTestFP("KNUQ", "14L", "PHNL", "22R",
"ROKME WOVAB");
RoutePath rtepath(fp1);
SGGeodVec vec = rtepath.pathForIndex(0);
FGAirportRef ksfo = FGAirport::findByIdent("KSFO");
FGFixRef rokme = fgpositioned_cast<FGFix>(FGPositioned::findClosestWithIdent("ROKME", ksfo->geod()));
auto depRwy = fp1->departureRunway();
CPPUNIT_ASSERT_DOUBLES_EQUAL(depRwy->geod().getLongitudeDeg(), vec.front().getLongitudeDeg(), 0.01);
CPPUNIT_ASSERT_DOUBLES_EQUAL(depRwy->geod().getLatitudeDeg(), vec.front().getLatitudeDeg(), 0.01);
SGGeodVec vec1 = rtepath.pathForIndex(1);
CPPUNIT_ASSERT_DOUBLES_EQUAL(rokme->geod().getLongitudeDeg(), vec1.back().getLongitudeDeg(), 0.01);
CPPUNIT_ASSERT_DOUBLES_EQUAL(rokme->geod().getLatitudeDeg(), vec1.back().getLatitudeDeg(), 0.01);
SGGeodVec vec2 = rtepath.pathForIndex(2);
CPPUNIT_ASSERT_DOUBLES_EQUAL(rokme->geod().getLongitudeDeg(), vec2.front().getLongitudeDeg(), 0.01);
CPPUNIT_ASSERT_DOUBLES_EQUAL(rokme->geod().getLatitudeDeg(), vec2.front().getLatitudeDeg(), 0.01);
//CPPUNIT_ASSERT(vec.front()
}
void FlightplanTests::testRoutPathWpt0Midflight()
@ -157,6 +175,9 @@ void FlightplanTests::testRoutPathWpt0Midflight()
FlightPlanRef fp1 = makeTestFP("KNUQ", "14L", "PHNL", "22R",
"ROKME WOVAB");
// actually delete leg 0 so we start at ROKME
fp1->deleteIndex(0);
RoutePath rtepath(fp1);
SGGeodVec vec = rtepath.pathForIndex(0);

2
test_suite/unit_tests/Navaids/test_flightplan.hxx
View file

@ -39,6 +39,7 @@ class FlightplanTests : public CppUnit::TestFixture
CPPUNIT_TEST(testAirwayNetworkRoute);
CPPUNIT_TEST(testBug1814);
CPPUNIT_TEST(testRoutPathWpt0Midflight);
CPPUNIT_TEST(testRoutePathVec);
// CPPUNIT_TEST(testParseICAORoute);
// CPPUNIT_TEST(testParseICANLowLevelRoute);
@ -62,6 +63,7 @@ public:
void testParseICANLowLevelRoute();
void testBug1814();
void testRoutPathWpt0Midflight();
void testRoutePathVec();
};
#endif // FG_FLIGHTPLAN_UNIT_TESTS_HXX