fly/flightgear
1
0
Fork 0
Code Activity

Fix DME intercept handling in route/GPS

Browse source 
Both the route-path cade and the RNAV code were using some bad logic
to compute the intersection point. All fixed now, but requires a
new helper in Simgear.
This commit is contained in:
James Turner 2020-05-30 15:50:09 +01:00
parent e51a0c55b5
commit 03563a1601
3 changed files with 7 additions and 17 deletions
src
Instrumentation
rnav_waypt_controller.cxx
Navaids
routePath.cxx
test_suite/FGTestApi
testGlobals.cxx

10
src/Instrumentation/rnav_waypt_controller.cxx
View file

@ -28,8 +28,6 @@
#include <Airports/runways.hxx>
#include "Main/util.hxx" // for fgLowPass
#include "test_suite/FGTestApi/TestDataLogger.hxx"
extern double pointsKnownDistanceFromGC(const SGGeoc& a, const SGGeoc&b, const SGGeoc& d, double dist);
namespace flightgear
@ -757,9 +755,7 @@ public:
// try with a backwards offset from the waypt pos, in case the
// procedure waypt location is too close. (eg, KSFO OCEAN SID)
SGGeoc navidAdjusted;
SGGeodesy::advanceRadM(geocWayptPos, _trueRadial, SG_NM_TO_METER * -10, navidAdjusted);
SGGeoc navidAdjusted = SGGeodesy::advanceDegM(geocWayptPos, _trueRadial, -10 * SG_NM_TO_METER);
ok = geocRadialIntersection(geocPos, _targetTrack,
navidAdjusted, _trueRadial, c);
if (!ok) {
@ -916,9 +912,7 @@ public:
double distRad = _dme->dmeDistanceNm() * SG_NM_TO_RAD;
// compute radial GC course
SGGeoc bPt;
SGGeodesy::advanceRadM(geocPos, _targetTrack, 100 * SG_NM_TO_RAD, bPt);
SGGeoc bPt = SGGeodesy::advanceDegM(geocPos, _targetTrack, 1e5);
double dNm = pointsKnownDistanceFromGC(geocPos, bPt, navid, distRad);
if (dNm < 0.0) {
SG_LOG(SG_AUTOPILOT, SG_WARN, "DMEInterceptCtl::position failed");

6
src/Navaids/routePath.cxx
View file

@ -662,10 +662,9 @@ public:
double track = i->courseDegMagnetic() + magVar;
bool ok = geocRadialIntersection(prevGc, track, navid, radial, rGc);
if (!ok) {
SGGeoc navidAdjusted;
// try pulling backward along the radial in case we're too close.
// suggests bad procedure construction if this is happening!
SGGeodesy::advanceRadM(navid, radial, SG_NM_TO_METER * -10, navidAdjusted);
SGGeoc navidAdjusted = SGGeodesy::advanceDegM(navid, radial, -10 * SG_NM_TO_METER);
// try again
ok = geocRadialIntersection(prevGc, track, navidAdjusted, radial, rGc);
@ -691,9 +690,8 @@ public:
double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD;
SGGeoc rGc;
SGGeoc bPt;
double crs = di->courseDegMagnetic() + magVarFor(wpt->position());
SGGeodesy::advanceRadM(prevGc, crs, 100 * SG_NM_TO_RAD, bPt);
SGGeoc bPt = SGGeodesy::advanceDegM(prevGc, crs, 1e5);
double dNm = pointsKnownDistanceFromGC(prevGc, bPt, navid, distRad);
if (dNm < 0.0) {

8
test_suite/FGTestApi/testGlobals.cxx
View file

@ -329,17 +329,15 @@ void writeFlightPlanToKML(flightgear::FlightPlanRef fp)
RoutePath rpath(fp);
SGGeodVec fullPath;
for (int i=0; i<fp->numLegs(); ++i) {
SGGeodVec legPath = rpath.pathForIndex(i);
fullPath.insert(fullPath.end(), legPath.begin(), legPath.end());
auto wp = fp->legAtIndex(i)->waypoint();
writeGeodsToKML("FP-leg-" + wp->ident(), legPath);
SGGeod legWPPosition = wp->position();
writePointToKML("WP " + wp->ident(), legWPPosition);
}
writeGeodsToKML("FlightPlan", fullPath);
}
void writeGeodsToKML(const std::string &label, const flightgear::SGGeodVec& geods)