fly/flightgear
1
0
Fork 0
Code Activity

Fix a GPS bug identified by Jonathan Redpath

Browse source 
When in LEG mode, and within the intercept cone, but further away from
the leg waypoint than the leg origin, we were computing a bogus
abeam point and hence a bogus desired track.

Detect this situation, and invert the computed along-track-distance,
so the computed abeam point is actually near where we are, and not ahead
of us.
This commit is contained in:
James Turner 2020-12-14 21:04:50 +00:00
parent c588f90a59
commit 62cdd30810
2 changed files with 35 additions and 4 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

17
src/Instrumentation/rnav_waypt_controller.cxx
View file

@ -450,9 +450,24 @@ public:
ATD=acos(cos(dist_AD)/cos(XTD))
*/
double atd = acos(cos(distOriginAircraftRad) / cos(xtkRad));
// fix sign of ATD, if we are 'behind' the waypoint origin, rather than
// in front of it. We need to set a -ve sign on atd, otherwise we compute
// an abeamPoint ahead of the waypoint origin, potentially so far ahead
// that the computed track is backwards.
// see test-case GPSTests::testCourseLegIntermediateWaypoint
// for this happening
{
double x = _courseOriginToAircraft - _initialLegCourse;
SG_NORMALIZE_RANGE(x, -180.0, 180.0);
if (fabs(x) > 90.0) {
atd = -atd;
}
}
const double atdM = atd * SG_RAD_TO_NM * SG_NM_TO_METER;
SGGeod abeamPoint = SGGeodesy::direct(_waypointOrigin, _initialLegCourse, atdM);
// if we're close to the target point, we enter something similar to the VOR zone
// of confusion. Force target track to the final course from the origin.
if (_distanceAircraftTargetMetre < (SG_NM_TO_METER * 2.0)) {

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

@ -1600,10 +1600,9 @@ void GPSTests::testCourseLegIntermediateWaypoint()
SGGeod decelPos = fp->pointAlongRoute(2, -15.0);
fp->insertWayptAtIndex(new BasicWaypt(decelPos, "DECEL", fp), 2);
fp->setCurrentIndex(3); // BLACA
fp->setCurrentIndex(2); // DECEL
// position halfway between EGAA and EGPF
SGGeod initPos = fp->pointAlongRoute(2, -5);
SGGeod initPos = fp->pointAlongRouteNorm(1, 0.1);
FGTestApi::setPositionAndStabilise(initPos);
auto gpsNode = globals->get_props()->getNode("instrumentation/gps");
@ -1624,7 +1623,24 @@ void GPSTests::testCourseLegIntermediateWaypoint()
CPPUNIT_ASSERT_DOUBLES_EQUAL(56.5, legToBlaca->courseDeg(), 1.0);
CPPUNIT_ASSERT_DOUBLES_EQUAL(56.5, gpsNode->getDoubleValue("desired-course-deg"), 2.0);
CPPUNIT_ASSERT_DOUBLES_EQUAL(56.0, wpNode->getDoubleValue("leg-true-course-deg"), 0.5);
CPPUNIT_ASSERT_DOUBLES_EQUAL(56.0, wpNode->getDoubleValue("leg-mag-course-deg"), 0.5);
fp->setCurrentIndex(3); // BLACA
FGTestApi::runForTime(0.1);
CPPUNIT_ASSERT_DOUBLES_EQUAL(56.5, gpsNode->getDoubleValue("desired-course-deg"), 2.0);
CPPUNIT_ASSERT_DOUBLES_EQUAL(56.5, wpNode->getDoubleValue("leg-true-course-deg"), 0.5);
CPPUNIT_ASSERT_DOUBLES_EQUAL(56.5, wpNode->getDoubleValue("leg-mag-course-deg"), 0.5);
auto pilot = SGSharedPtr<FGTestApi::TestPilot>(new FGTestApi::TestPilot);
pilot->resetAtPosition(initPos);
pilot->setCourseTrue(fp->legAtIndex(2)->courseDeg());
pilot->setSpeedKts(280);
pilot->flyGPSCourse(gps);
bool ok = FGTestApi::runForTimeWithCheck(1200.0, [fp]() {
return fp->currentIndex() == 4;
});
CPPUNIT_ASSERT(ok);
}