fly/flightgear
1
0
Fork 0
Code Activity

FlightPlan: DISCON works with pointAlongRoute

Browse source 
Add some tests for the default GPS / route-manager handling of
DISCON in flight-plans. Especially check behaviour when sequencing
a DISCON, and resuming with a Direct-to after one. 

This requires a corresponding FGData update to the default GPS Nasal.
This commit is contained in:
James Turner 2020-04-19 16:44:49 +01:00
parent 1a925b152d
commit 17774dbe4c
4 changed files with 166 additions and 60 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

131
src/Navaids/routePath.cxx
View file

@ -632,7 +632,7 @@ public:
void computeDynamicPosition(int index)
{
auto previous(previousValidWaypoint(index));
if ((previous == waypoints.end()) || !previous->posValid);
if ((previous == waypoints.end()) || !previous->posValid)
{
SG_LOG(SG_NAVAID, SG_WARN, "couldn't compute position for dynamic waypoint: no preceeding valid waypoint");
return;
@ -1165,69 +1165,80 @@ double RoutePath::distanceBetweenIndices(int from, int to) const
SGGeod RoutePath::positionForDistanceFrom(int index, double distanceM) const
{
int sz = (int) d->waypoints.size();
if (index < 0) {
index = sz - 1; // map negative values to end of the route
int sz = (int) d->waypoints.size();
if (index < 0) {
index = sz - 1; // map negative values to end of the route
}
if ((index < 0) || (index >= sz)) {
throw sg_range_exception("waypt index out of range",
"RoutePath::positionForDistanceFrom");
}
// find the actual leg we're within
if (distanceM < 0.0) {
// scan backwards
while ((index > 0) && (distanceM < 0.0)) {
// we are looking at index n, say 4, but with a negative distance.
// we want to look at index n-1 (so, 3), and see if this makes
// distance positive. We need to offset by distance from 3 -> 4,
// which is waypoint 4's path distance.
// note pathDistanceM is 0 for skipped waypoints, so this works out
distanceM += d->waypoints[index].pathDistanceM;
--index;
}
if ((index < 0) || (index >= sz)) {
throw sg_range_exception("waypt index out of range",
"RoutePath::positionForDistanceFrom");
}
// find the actual leg we're within
if (distanceM < 0.0) {
// scan backwards
while ((index > 0) && (distanceM < 0.0)) {
// we are looking at index n, say 4, but with a negative distance.
// we want to look at index n-1 (so, 3), and see if this makes
// distance positive. We need to offset by distance from 3 -> 4,
// which is waypoint 4's path distance.
distanceM += d->waypoints[index].pathDistanceM;
--index;
}
if (distanceM < 0.0) {
// still negative, return route start
return d->waypoints[0].pos;
}
} else {
// scan forwards
int nextIndex = index + 1;
while ((nextIndex < sz) && (d->waypoints[nextIndex].pathDistanceM < distanceM)) {
distanceM -= d->waypoints[nextIndex].pathDistanceM;
index = nextIndex++;
}
// still negative, return route start
return d->waypoints[0].pos;
}
if ((index + 1) >= sz) {
// past route end, just return final position
return d->waypoints[sz - 1].pos;
} else {
// scan forwards
int nextIndex = index + 1;
while ((nextIndex < sz) && (d->waypoints[nextIndex].pathDistanceM < distanceM)) {
distanceM -= d->waypoints[nextIndex].pathDistanceM;
index = nextIndex++;
}
const WayptData& wpt(d->waypoints[index]);
const WayptData& next(d->waypoints[index+1]);
if (next.wpt->type() == "via") {
return positionAlongVia(static_cast<Via*>(next.wpt.get()), index, distanceM);
}
if (wpt.turnPathDistanceM > distanceM) {
// on the exit path of current wpt
return wpt.pointAlongExitPath(distanceM);
} else {
distanceM -= wpt.turnPathDistanceM;
}
double corePathDistance = next.pathDistanceM - next.turnPathDistanceM;
if (next.hasEntry && (distanceM > corePathDistance)) {
// on the entry path of next waypoint
return next.pointAlongEntryPath(distanceM - corePathDistance);
}
// linear between turn exit and turn entry points
return SGGeodesy::direct(wpt.turnExitPos, next.legCourseTrue, distanceM);
}
auto nextIt = d->nextValidWaypoint(index);
if (nextIt == d->waypoints.end()) {
// past route end, just return final position
return d->waypoints[sz - 1].pos;
}
// this is important so we start from a valid WP if we're
// working either side of a DISCON
auto curIt = d->previousValidWaypoint(nextIt);
if (curIt == d->waypoints.end()) {
SG_LOG(SG_NAVAID, SG_WARN, "Couldn't find valid preceeding waypoint " << index);
return nextIt->pos;
}
const WayptData& wpt = *curIt;
const WayptData& next = *nextIt;
if (next.wpt->type() == "via") {
return positionAlongVia(static_cast<Via*>(next.wpt.get()), index, distanceM);
}
if (wpt.turnPathDistanceM > distanceM) {
// on the exit path of current wpt
return wpt.pointAlongExitPath(distanceM);
} else {
distanceM -= wpt.turnPathDistanceM;
}
double corePathDistance = next.pathDistanceM - next.turnPathDistanceM;
if (next.hasEntry && (distanceM > corePathDistance)) {
// on the entry path of next waypoint
return next.pointAlongEntryPath(distanceM - corePathDistance);
}
// linear between turn exit and turn entry points
return SGGeodesy::direct(wpt.turnExitPos, next.legCourseTrue, distanceM);
}
SGGeod RoutePath::positionAlongVia(Via* via, int previousIndex, double distanceM) const

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

@ -405,6 +405,7 @@ void FlightplanTests::testBasicDiscontinuity()
const SGGeod balenPos = fp1->legAtIndex(3)->waypoint()->position();
const SGGeod murenPos = fp1->legAtIndex(4)->waypoint()->position();
const auto crs = SGGeodesy::courseDeg(balenPos, murenPos);
// total distance should not change
fp1->insertWayptAtIndex(new Discontinuity(fp1), 4); // betwee BALEN and MUREN
@ -420,6 +421,20 @@ void FlightplanTests::testBasicDiscontinuity()
CPPUNIT_ASSERT_DOUBLES_EQUAL(fp1->legAtIndex(5)->distanceNm(),
SGGeodesy::distanceNm(balenPos, murenPos), 0.1);
// ensure that pointAlongRoute works correctly going into the DISCON
const auto pos1 = fp1->pointAlongRoute(3, 20.0);
const auto validP1 = SGGeodesy::direct(balenPos, crs, 20.0 * SG_NM_TO_METER);
CPPUNIT_ASSERT(SGGeodesy::distanceM(pos1, validP1) < 500.0);
const auto pos2 = fp1->pointAlongRoute(5, -10.0);
const auto crs2 = SGGeodesy::courseDeg(murenPos, balenPos);
const auto validP2 = SGGeodesy::direct(murenPos, crs2, 10.0 * SG_NM_TO_METER);
CPPUNIT_ASSERT(SGGeodesy::distanceM(pos2, validP2) < 500.0);
// remove the discontinuity
fp1->deleteIndex(4);
CPPUNIT_ASSERT_DOUBLES_EQUAL(fp1->legAtIndex(4)->courseDeg(),

77
test_suite/unit_tests/Navaids/test_routeManager.cxx
View file

@ -279,6 +279,83 @@ void RouteManagerTests::testDirectToLegOnFlightplanAndResume()
CPPUNIT_ASSERT_EQUAL(std::string{"leg"}, std::string{gpsNode->getStringValue("mode")});
}
void RouteManagerTests::testSequenceDiscontinuityAndResume()
{
FGTestApi::setUp::logPositionToKML("rm_seq_discon_resume_leg");
FlightPlanRef fp1 = makeTestFP("LIRF", "16R", "LEBL", "07L",
"GITRI BALEN MUREN TOSNU");
fp1->insertWayptAtIndex(new Discontinuity(fp1), 3);
auto rm = globals->get_subsystem<FGRouteMgr>();
rm->setFlightPlan(fp1);
auto gpsNode = globals->get_props()->getNode("instrumentation/gps", true);
// auto rmNode = globals->get_props()->getNode("autopilot/route-manager", true);
CPPUNIT_ASSERT(!strcmp("obs", gpsNode->getStringValue("mode")));
rm->activate();
CPPUNIT_ASSERT(fp1->isActive());
auto balenLeg = fp1->legAtIndex(2);
auto pos = fp1->pointAlongRoute(2, -8.0); // 8nm before BALEN
fp1->setCurrentIndex(2);
FGTestApi::setPosition(pos);
FGTestApi::runForTime(10.0); // let the GPS stabilize
auto pilot = SGSharedPtr<FGTestApi::TestPilot>(new FGTestApi::TestPilot);
pilot->resetAtPosition(pos);
pilot->setCourseTrue(270);
pilot->setSpeedKts(250);
pilot->flyGPSCourse(m_gps);
bool ok = FGTestApi::runForTimeWithCheck(180.0, [gpsNode] () {
return (gpsNode->getStringValue("mode") == std::string{"obs"});
});
CPPUNIT_ASSERT(ok);
CPPUNIT_ASSERT_EQUAL(2, fp1->currentIndex()); // shouldn't sequence
FGTestApi::runForTime(30.0);
// 2nm before MUREN : this should be on the GC course from BALEN
auto pos2 = fp1->pointAlongRoute(4, -6.0);
FGTestApi::setPosition(pos2);
FGTestApi::runForTime(2.0); // let the GPS stabilize
// initiate a direct-to the next real WP
const auto murenPos = fp1->legAtIndex(4)->waypoint()->position();
gpsNode->setStringValue("scratch/ident", "MUREN");
gpsNode->setDoubleValue("scratch/longitude-deg", murenPos.getLongitudeDeg());
gpsNode->setDoubleValue("scratch/latitude-deg", murenPos.getLatitudeDeg());
gpsNode->setStringValue("command", "direct");
CPPUNIT_ASSERT_EQUAL(std::string{"dto"}, std::string{gpsNode->getStringValue("mode")});
pilot->resetAtPosition(pos2);
pilot->flyGPSCourse(m_gps);
ok = FGTestApi::runForTimeWithCheck(600.0, [fp1] () {
if (fp1->currentIndex() == 5) {
return true;
}
return false;
});
CPPUNIT_ASSERT(ok);
CPPUNIT_ASSERT_EQUAL(std::string{"leg"}, std::string{gpsNode->getStringValue("mode")});
CPPUNIT_ASSERT_EQUAL(5, fp1->currentIndex());
FGTestApi::runForTime(30.0);
}
void RouteManagerTests::testDefaultApproach()
{

3
test_suite/unit_tests/Navaids/test_routeManager.hxx
View file

@ -38,6 +38,8 @@ class RouteManagerTests : public CppUnit::TestFixture
CPPUNIT_TEST(testDefaultApproach);
CPPUNIT_TEST(testDirectToLegOnFlightplanAndResume);
CPPUNIT_TEST(testHoldFromNasal);
CPPUNIT_TEST(testSequenceDiscontinuityAndResume);
CPPUNIT_TEST_SUITE_END();
// void setPositionAndStabilise(FGNavRadio* r, const SGGeod& g);
@ -57,6 +59,7 @@ public:
void testDefaultApproach();
void testDirectToLegOnFlightplanAndResume();
void testHoldFromNasal();
void testSequenceDiscontinuityAndResume();
private:
GPS* m_gps = nullptr;
};