fly/flightgear
1
0
Fork 0
Code Activity

RoutePath: fix pointAlongPath for fly-overs

Browse source 
FIx some bugs in computing turnExitPos for fly-over waypoints,
and computing leg course when preceeeding leg is a runway.
In both cass we had an incorrect turnExitPos, so points along
the leg were incorreclty positioned.

Extend the test cases to cover this further.
This commit is contained in:
James Turner 2022-01-20 20:33:37 +00:00
parent b50ba41a15
commit fe9aadd8bb
6 changed files with 117 additions and 22 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

59
src/Navaids/routePath.cxx
View file

@ -368,12 +368,20 @@ public:
legCourseValid = true;
return;
}
SGGeod previousPos = previous->pos;
// use correct location for runway, otherwise use threshold
// and get weird courses
if (previous->wpt->type() == "runway") {
FGRunway* rwy = static_cast<RunwayWaypt*>(previous->wpt.get())->runway();
previousPos = rwy->end();
}
if (posValid) {
legCourseTrue = SGGeodesy::courseDeg(previous->pos, pos);
legCourseTrue = SGGeodesy::courseDeg(previousPos, pos);
legCourseValid = true;
} else if (isCourseConstrained()) {
double magVar = magVarFor(previous->pos);
double magVar = magVarFor(previousPos);
legCourseTrue = wpt->headingRadialDeg() + magVar;
legCourseValid = true;
} // of pos not valid
@ -418,6 +426,7 @@ public:
turnExitAngle = next.legCourseTrue - rwy->headingDeg();
legCourseTrue = rwy->headingDeg();
flyOver = true;
// fall through
} else {
legCourseValid = true;
legCourseTrue = next.legCourseTrue;
@ -463,9 +472,14 @@ public:
theta = copysign(theta, turnExitAngle);
turnExitAngle += theta;
// move by the distance to compensate
double d = turnRadius * 2.0 * sin(theta * SG_DEGREES_TO_RADIANS);
turnExitPos = SGGeodesy::direct(turnExitPos, next.legCourseTrue, d);
double p = copysign(90, turnExitAngle);
const double offsetAngle = legCourseTrue + turnExitAngle - p;
SGGeod tc2 = SGGeodesy::direct(turnExitCenter,
offsetAngle,
turnRadius * 2.0);
turnExitPos = SGGeodesy::direct(tc2, next.legCourseTrue + p , turnRadius);
overflightCompensationAngle = -theta;
// sign of angles will differ, so compute distances seperately
@ -576,23 +590,28 @@ public:
SGGeod pointAlongExitPath(double distanceM) const
{
double theta = (distanceM / turnRadius) * SG_RADIANS_TO_DEGREES;
double p = copysign(90, turnExitAngle);
double basicTurnDistance = pathDistanceForTurnAngle(turnExitAngle);
if (distanceM > basicTurnDistance) {
assert(flyOver);
assert(overflightCompensationAngle != 0.0);
double p = copysign(90, turnExitAngle);
if (flyOver && (overflightCompensationAngle != 0.0)) {
// figure out if we're in the compensation section
if (theta > turnExitAngle) {
// compute the compensation turn center - twice the turn radius
// from turnCenter
SGGeod tc2 = SGGeodesy::direct(turnExitCenter,
legCourseTrue - overflightCompensationAngle - p,
turnRadius * 2.0);
theta = copysign(theta - turnExitAngle, overflightCompensationAngle);
return SGGeodesy::direct(tc2,
legCourseTrue - overflightCompensationAngle + theta + p, turnRadius);
}
double distanceAlongCompensationTurn = distanceM - basicTurnDistance;
double theta = (distanceAlongCompensationTurn / turnRadius) * SG_RADIANS_TO_DEGREES;
// compute the compensation turn center - twice the turn radius
// from turnCenter
SGGeod tc2 = SGGeodesy::direct(turnExitCenter,
legCourseTrue + turnExitAngle - p,
turnRadius * 2.0);
theta = copysign(theta, overflightCompensationAngle);
return SGGeodesy::direct(tc2,
legCourseTrue + turnExitAngle + theta + p, turnRadius);
}
double theta = (distanceM / turnRadius) * SG_RADIANS_TO_DEGREES;
theta = copysign(theta, turnExitAngle);
double inboundCourse = legCourseTrue + (flyOver ? 0.0 : turnExitAngle);
return pointOnExitTurnFromHeading(inboundCourse + theta);

6
test_suite/FGTestApi/TestPilot.cxx
View file

@ -273,4 +273,10 @@ bool TestPilot::isOnHeading(double heading) const
return fabs(hdgDelta) < 0.5;
}
double TestPilot::trueCourseDeg() const
{
return _trueCourseDeg;
}
} // of namespace

1
test_suite/FGTestApi/TestPilot.hxx
View file

@ -63,6 +63,7 @@ public:
bool isOnHeading(double heading) const;
double trueCourseDeg() const;
private:
enum class LateralMode
{

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

@ -1717,3 +1717,70 @@ void GPSTests::testCourseLegIntermediateWaypoint()
});
CPPUNIT_ASSERT(ok);
}
// Test to check the situation where you have two legs forming a straight line
// with the current waypoint being the third waypoint
void GPSTests::testFlyOverMaxInterceptAngle()
{
//FGTestApi::setUp::logPositionToKML("gps_fly_over_max_intercept_angle");
auto rm = globals->get_subsystem<FGRouteMgr>();
auto fp = FlightPlan::create();
rm->setFlightPlan(fp);
FGTestApi::setUp::populateFPWithoutNasal(fp, "EGAA", "25", "EGPH", "06", "LISBO BLACA");
// takes the place of the Nasal delegates
auto testDelegate = new TestFPDelegate;
testDelegate->thePlan = fp;
CPPUNIT_ASSERT(rm->activate());
fp->addDelegate(testDelegate);
FGTestApi::writeFlightPlanToKML(fp);
SGGeod initPos = fp->pointAlongRouteNorm(1, -0.15);
FGTestApi::writePointToKML("Start pos", initPos);
auto gps = setupStandardGPS();
FGTestApi::setPositionAndStabilise(initPos);
auto gpsNode = globals->get_props()->getNode("instrumentation/gps");
gpsNode->setBoolValue("config/delegate-sequencing", true);
gpsNode->setStringValue("command", "leg");
auto gpsErrorNmNode = gpsNode->getNode("wp/wp[1]/course-error-nm");
fp->setCurrentIndex(1); // BLACA
auto pilot = SGSharedPtr<FGTestApi::TestPilot>(new FGTestApi::TestPilot);
pilot->resetAtPosition(initPos);
pilot->setCourseTrue(fp->legAtIndex(1)->courseDeg());
pilot->setSpeedKts(280);
pilot->flyGPSCourse(gps);
bool ok = FGTestApi::runForTimeWithCheck(1200.0, [&fp]() {
return fp->currentIndex() == 2;
});
CPPUNIT_ASSERT(ok);
FGPositioned::TypeFilter fixFilter(FGPositioned::FIX);
auto lisbo = FGPositioned::findClosestWithIdent("LISBO", fp->departureAirport()->geod(), &fixFilter);
auto blaca = FGPositioned::findClosestWithIdent("BLACA", fp->departureAirport()->geod(), &fixFilter);
const double crsToBlaca = SGGeodesy::courseDeg(lisbo->geod(), blaca->geod());
ok = FGTestApi::runForTimeWithCheck(1200.0, [pilot, crsToBlaca, gpsErrorNmNode]()
{
const double heading = pilot->trueCourseDeg();
if (heading < (crsToBlaca - 45.0)) {
CPPUNIT_FAIL("Turned too far on intercept");
}
// check if we're near the leg
if (fabs(gpsErrorNmNode->getDoubleValue()) > 0.05) {
return false;
}
return pilot->isOnHeading(crsToBlaca);
});
CPPUNIT_ASSERT(ok);
}

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

@ -57,7 +57,8 @@ class GPSTests : public CppUnit::TestFixture
CPPUNIT_TEST(testFinalLegCourse);
CPPUNIT_TEST(testCourseLegIntermediateWaypoint);
CPPUNIT_TEST(testExceedFlyByMaxAngleTurn);
CPPUNIT_TEST(testFlyOverMaxInterceptAngle);
CPPUNIT_TEST_SUITE_END();
void setPositionAndStabilise(GPS* gps, const SGGeod& g);
@ -94,6 +95,7 @@ public:
void testFinalLegCourse();
void testCourseLegIntermediateWaypoint();
void testExceedFlyByMaxAngleTurn();
void testFlyOverMaxInterceptAngle();
};
#endif // _FG_GPS_UNIT_TESTS_HXX

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

@ -359,7 +359,7 @@ void FlightplanTests::testRoutePathTrivialFlightPlan()
rtepath.distanceForIndex(leg);
}
CPPUNIT_ASSERT_DOUBLES_EQUAL(19.68, fp1->totalDistanceNm(), 0.1);
CPPUNIT_ASSERT_DOUBLES_EQUAL(19.5, fp1->totalDistanceNm(), 0.1);
}
void FlightplanTests::testRoutePathVec()