fly/flightgear
1
0
Fork 0
Code Activity

GPS/RNAV: configurable max-flyBy-angle

Browse source 
Make the max-flyBy angle configuable, since for exmaple AIrbus uses 90
degrees. Expose this via a new gps/config property, and extend the tests
to verify that angles greater than the fly-by angle behav as fly-over
waypoints.

Extend the RoutePath code to share this configuration, so that route
visualisations match the configured angle.

SF-ID: https://sourceforge.net/p/flightgear/codetickets/2694/
This commit is contained in:
James Turner 2022-01-18 16:07:54 +00:00
parent 29ef5210a7
commit 08a70297ff
9 changed files with 324 additions and 206 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

13
src/Instrumentation/gps.cxx
View file

@ -91,6 +91,14 @@ void GPS::Config::bind(GPS* aOwner, SGPropertyNode* aCfg)
aOwner->tie(aCfg, "over-flight-arm-distance-nm", SGRawValuePointer<double>(&_overflightArmDistance));
aOwner->tie(aCfg, "over-flight-arm-angle-deg", SGRawValuePointer<double>(&_overflightArmAngle));
aOwner->tie(aCfg, "delegate-sequencing", SGRawValuePointer<bool>(&_delegateSequencing));
aOwner->tie(aCfg, "max-fly-by-turn-angle-deg", SGRawValueMethods<GPS, double>(*aOwner, &GPS::maxFlyByTurnAngleDeg, &GPS::setFlyByMaxTurnAngle));
}
void GPS::setFlyByMaxTurnAngle(double maxAngle)
{
_config.setMaxFlyByTurnAngle(maxAngle);
// keep the FlightPlan in sync, so RoutePath matches
_route->setMaxFlyByTurnAngle(maxAngle);
}
////////////////////////////////////////////////////////////////////////////
@ -481,6 +489,11 @@ double GPS::selectedMagCourse()
return _selectedCourse;
}
double GPS::maxFlyByTurnAngleDeg() const
{
return _config.maxFlyByTurnAngleDeg();
}
simgear::optional<double> GPS::nextLegTrack()
{
auto next = _route->nextLeg();

15
src/Instrumentation/gps.hxx
View file

@ -86,7 +86,8 @@ public:
double overflightArmDistanceM() override;
double overflightArmAngleDeg() override;
bool canFlyBy() const override;
double maxFlyByTurnAngleDeg() const override;
simgear::optional<LegData> previousLegData() override;
simgear::optional<double> nextLegTrack() override;
@ -95,6 +96,8 @@ public:
private:
friend class SearchFilter;
void setFlyByMaxTurnAngle(double maxAngle);
/**
* Configuration manager, track data relating to aircraft installation
*/
@ -158,6 +161,14 @@ private:
bool delegateDoesSequencing() const { return _delegateSequencing; }
double maxFlyByTurnAngleDeg() const { return _maxFlyByTurnAngle; }
void setMaxFlyByTurnAngle(double deg)
{
_maxFlyByTurnAngle = deg;
}
private:
bool _enableTurnAnticipation;
@ -194,6 +205,8 @@ private:
// do we handle waypoint sequencing ourselves, or let the delegate do it?
// default is we do it, for backwards compatability
bool _delegateSequencing = false;
double _maxFlyByTurnAngle = 90.0;
};
class SearchFilter : public FGPositioned::Filter

10
src/Instrumentation/rnav_waypt_controller.cxx
View file

@ -325,12 +325,12 @@ public:
_flyByTurnAngle = nextLegTrack.value() - _finalLegCourse;
SG_NORMALIZE_RANGE(_flyByTurnAngle, -180.0, 180.0);
if (fabs(_flyByTurnAngle) > 120.0) {
// too sharp, don't anticipate
return;
if (fabs(_flyByTurnAngle) > _rnav->maxFlyByTurnAngleDeg()) {
// too sharp, don't anticipate
return;
}
_flyByTurnRadius = _rnav->turnRadiusNm() * SG_NM_TO_METER;
_flyByTurnCenter = computeTurnCenter(_flyByTurnRadius, _waypt->position(), _finalLegCourse, _flyByTurnAngle);
_doFlyBy = true;

12
src/Instrumentation/rnav_waypt_controller.hxx
View file

@ -64,7 +64,17 @@ public:
{
return false;
}
/**
* maximum angle in degrees where flyBy is permitted. Turns larger
* than this value will always be executed as fly-over to avoid
* a very large cut.
*/
virtual double maxFlyByTurnAngleDeg() const
{
return 120.0;
}
/**
* minimum distance to switch next waypoint.
*/

10
src/Navaids/FlightPlan.cxx
View file

@ -1908,6 +1908,16 @@ bool FlightPlan::followLegTrackToFixes() const
return _followLegTrackToFix;
}
void FlightPlan::setMaxFlyByTurnAngle(double deg)
{
_maxFlyByTurnAngle = deg;
}
double FlightPlan::maxFlyByTurnAngle() const
{
return _maxFlyByTurnAngle;
}
std::string FlightPlan::icaoAircraftCategory() const
{
std::string r;

28
src/Navaids/FlightPlan.hxx
View file

@ -76,6 +76,9 @@ public:
void setFollowLegTrackToFixes(bool tf);
bool followLegTrackToFixes() const;
void setMaxFlyByTurnAngle(double deg);
double maxFlyByTurnAngle() const;
// aircraft approach category as per CFR 97.3, etc
// http://www.flightsimaviation.com/data/FARS/part_97-3.html
std::string icaoAircraftCategory() const;
@ -482,22 +485,23 @@ private:
int _cruiseFlightLevel = 0;
int _cruiseAltitudeFt = 0;
int _estimatedDuration = 0;
double _maxFlyByTurnAngle = 90.0;
FGAirportRef _departure, _destination;
FGAirportRef _alternate;
FGRunway* _departureRunway, *_destinationRunway;
SGSharedPtr<SID> _sid;
SGSharedPtr<STAR> _star;
SGSharedPtr<Approach> _approach;
std::string _sidTransition, _starTransition, _approachTransition;
FGAirportRef _departure, _destination;
FGAirportRef _alternate;
FGRunway *_departureRunway, *_destinationRunway;
SGSharedPtr<SID> _sid;
SGSharedPtr<STAR> _star;
SGSharedPtr<Approach> _approach;
std::string _sidTransition, _starTransition, _approachTransition;
double _totalDistance;
void rebuildLegData();
double _totalDistance;
void rebuildLegData();
using LegVec = std::vector<LegRef>;
LegVec _legs;
using LegVec = std::vector<LegRef>;
LegVec _legs;
std::vector<Delegate*> _delegates;
std::vector<Delegate*> _delegates;
};
} // of namespace flightgear

350
src/Navaids/routePath.cxx
View file

@ -397,113 +397,113 @@ public:
return turnRadius * fabs(angleDeg) * SG_DEGREES_TO_RADIANS;
}
void computeTurn(double radiusM, bool constrainLegCourse, WayptData& next)
{
assert(!skipped);
assert(next.legCourseValid);
bool isRunway = (wpt->type() == "runway");
void computeTurn(double radiusM, bool constrainLegCourse, double maxFlyByTurnAngleDeg, WayptData& next)
{
assert(!skipped);
assert(next.legCourseValid);
bool isRunway = (wpt->type() == "runway");
if (legCourseValid) {
if (isRunway) {
FGRunway* rwy = static_cast<RunwayWaypt*>(wpt.get())->runway();
turnExitAngle = next.legCourseTrue - rwy->headingDeg();
} else {
turnExitAngle = next.legCourseTrue - legCourseTrue;
}
} else {
// happens for first leg
if (isRunway) {
legCourseValid = true;
FGRunway* rwy = static_cast<RunwayWaypt*>(wpt.get())->runway();
turnExitAngle = next.legCourseTrue - rwy->headingDeg();
legCourseTrue = rwy->headingDeg();
flyOver = true;
} else {
legCourseValid = true;
legCourseTrue = next.legCourseTrue;
turnExitAngle = 0.0;
turnExitPos = pos;
flyOver = true;
return;
}
}
SG_NORMALIZE_RANGE(turnExitAngle, -180.0, 180.0);
turnRadius = radiusM;
if (!flyOver && fabs(turnExitAngle) > 120.0) {
// flyBy logic blows up for sharp turns - due to the tan() term
// heading towards infinity. By converting to flyOver we do something
// closer to what was requested.
flyOver = true;
}
if (flyOver) {
if (isRunway) {
FGRunway* rwy = static_cast<RunwayWaypt*>(wpt.get())->runway();
turnExitCenter = turnCenterOverflight(rwy->end(), rwy->headingDeg(), turnExitAngle, turnRadius);
if (legCourseValid) {
if (isRunway) {
FGRunway* rwy = static_cast<RunwayWaypt*>(wpt.get())->runway();
turnExitAngle = next.legCourseTrue - rwy->headingDeg();
} else {
turnExitAngle = next.legCourseTrue - legCourseTrue;
}
} else {
turnEntryPos = pos;
turnExitCenter = turnCenterOverflight(pos, legCourseTrue, turnExitAngle, turnRadius);
// happens for first leg
if (isRunway) {
legCourseValid = true;
FGRunway* rwy = static_cast<RunwayWaypt*>(wpt.get())->runway();
turnExitAngle = next.legCourseTrue - rwy->headingDeg();
legCourseTrue = rwy->headingDeg();
flyOver = true;
} else {
legCourseValid = true;
legCourseTrue = next.legCourseTrue;
turnExitAngle = 0.0;
turnExitPos = pos;
flyOver = true;
return;
}
}
turnExitPos = pointOnExitTurnFromHeading(next.legCourseTrue);
if (!next.wpt->flag(WPT_DYNAMIC)) {
// distance perpendicular to next leg course, after turning
// through turnAngle
double xtk = turnRadius * (1 - cos(turnExitAngle * SG_DEGREES_TO_RADIANS));
if (constrainLegCourse || next.isCourseConstrained()) {
// next leg course is constrained. We need to swing back onto the
// desired course, using a compensation turn
// compensation angle to turn back on course
double theta = acos((turnRadius - (xtk * 0.5)) / turnRadius) * SG_RADIANS_TO_DEGREES;
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);
overflightCompensationAngle = -theta;
SG_NORMALIZE_RANGE(turnExitAngle, -180.0, 180.0);
turnRadius = radiusM;
// sign of angles will differ, so compute distances seperately
turnPathDistanceM = pathDistanceForTurnAngle(turnExitAngle) +
pathDistanceForTurnAngle(overflightCompensationAngle);
if (!flyOver && fabs(turnExitAngle) > maxFlyByTurnAngleDeg) {
// flyBy logic blows up for sharp turns - due to the tan() term
// heading towards infinity. By converting to flyOver we do something
// closer to what was requested. This matches logic in the RNAV
// Leg controller
flyOver = true;
}
if (flyOver) {
if (isRunway) {
FGRunway* rwy = static_cast<RunwayWaypt*>(wpt.get())->runway();
turnExitCenter = turnCenterOverflight(rwy->end(), rwy->headingDeg(), turnExitAngle, turnRadius);
} else {
turnEntryPos = pos;
turnExitCenter = turnCenterOverflight(pos, legCourseTrue, turnExitAngle, turnRadius);
}
turnExitPos = pointOnExitTurnFromHeading(next.legCourseTrue);
if (!next.wpt->flag(WPT_DYNAMIC)) {
// distance perpendicular to next leg course, after turning
// through turnAngle
double xtk = turnRadius * (1 - cos(turnExitAngle * SG_DEGREES_TO_RADIANS));
if (constrainLegCourse || next.isCourseConstrained()) {
// next leg course is constrained. We need to swing back onto the
// desired course, using a compensation turn
// compensation angle to turn back on course
double theta = acos((turnRadius - (xtk * 0.5)) / turnRadius) * SG_RADIANS_TO_DEGREES;
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);
overflightCompensationAngle = -theta;
// sign of angles will differ, so compute distances seperately
turnPathDistanceM = pathDistanceForTurnAngle(turnExitAngle) +
pathDistanceForTurnAngle(overflightCompensationAngle);
} else {
// next leg course can be adjusted. increase the turn angle
// and modify the next leg's course accordingly.
// hypotenuse of triangle, opposite edge has length turnRadius
double distAlongPath = std::min(1.0, sin(fabs(turnExitAngle) * SG_DEGREES_TO_RADIANS)) * turnRadius;
double nextLegDistance = SGGeodesy::distanceM(pos, next.pos) - distAlongPath;
double increaseAngle = atan2(xtk, nextLegDistance) * SG_RADIANS_TO_DEGREES;
increaseAngle = copysign(increaseAngle, turnExitAngle);
turnExitAngle += increaseAngle;
turnExitPos = pointOnExitTurnFromHeading(legCourseTrue + turnExitAngle);
// modify next leg course
next.legCourseTrue = SGGeodesy::courseDeg(turnExitPos, next.pos);
turnPathDistanceM = pathDistanceForTurnAngle(turnExitAngle);
} // of next leg isn't course constrained
} else {
// next point is dynamic
// no compensation needed
turnPathDistanceM = pathDistanceForTurnAngle(turnExitAngle);
}
} else {
// next leg course can be adjusted. increase the turn angle
// and modify the next leg's course accordingly.
hasEntry = true;
turnEntryCenter = turnCenterFlyBy(pos, legCourseTrue, turnExitAngle, turnRadius);
// hypotenuse of triangle, opposite edge has length turnRadius
double distAlongPath = std::min(1.0, sin(fabs(turnExitAngle) * SG_DEGREES_TO_RADIANS)) * turnRadius;
double nextLegDistance = SGGeodesy::distanceM(pos, next.pos) - distAlongPath;
double increaseAngle = atan2(xtk, nextLegDistance) * SG_RADIANS_TO_DEGREES;
increaseAngle = copysign(increaseAngle, turnExitAngle);
turnExitAngle = turnExitAngle * 0.5;
turnEntryAngle = turnExitAngle;
turnExitCenter = turnEntryCenter; // important that these match
turnExitAngle += increaseAngle;
turnExitPos = pointOnExitTurnFromHeading(legCourseTrue + turnExitAngle);
// modify next leg course
next.legCourseTrue = SGGeodesy::courseDeg(turnExitPos, next.pos);
turnPathDistanceM = pathDistanceForTurnAngle(turnExitAngle);
} // of next leg isn't course constrained
} else {
// next point is dynamic
// no compensation needed
turnPathDistanceM = pathDistanceForTurnAngle(turnExitAngle);
}
} else {
hasEntry = true;
turnEntryCenter = turnCenterFlyBy(pos, legCourseTrue, turnExitAngle, turnRadius);
turnExitAngle = turnExitAngle * 0.5;
turnEntryAngle = turnExitAngle;
turnExitCenter = turnEntryCenter; // important that these match
turnEntryPos = pointOnEntryTurnFromHeading(legCourseTrue);
turnExitPos = pointOnExitTurnFromHeading(next.legCourseTrue);
turnPathDistanceM = pathDistanceForTurnAngle(turnEntryAngle);
}
turnEntryPos = pointOnEntryTurnFromHeading(legCourseTrue);
turnExitPos = pointOnExitTurnFromHeading(next.legCourseTrue);
turnPathDistanceM = pathDistanceForTurnAngle(turnEntryAngle);
}
}
double turnDistanceM() const
@ -628,85 +628,83 @@ public:
AircraftPerformance perf;
bool constrainLegCourses;
double maxFlyByTurnAngleDeg = 90.0;
void computeDynamicPosition(int index)
{
auto previous(previousValidWaypoint(index));
if ((previous == waypoints.end()) || !previous->posValid)
void computeDynamicPosition(int index)
{
SG_LOG(SG_NAVAID, SG_WARN, "couldn't compute position for dynamic waypoint: no preceeding valid waypoint");
return;
}
WayptRef wpt = waypoints[index].wpt;
const std::string& ty(wpt->type());
if (ty == "hdgToAlt") {
HeadingToAltitude* h = (HeadingToAltitude*) wpt.get();
double altFt = computeVNAVAltitudeFt(index - 1);
double distanceM = perf.distanceNmBetween(altFt, h->altitudeFt()) * SG_NM_TO_METER;
double hdg = h->headingDegMagnetic() + magVarFor(previous->pos);
waypoints[index].pos = SGGeodesy::direct(previous->turnExitPos, hdg, distanceM);
waypoints[index].posValid = true;
} else if (ty == "radialIntercept") {
// start from previous.turnExit
RadialIntercept* i = (RadialIntercept*) wpt.get();
SGGeoc prevGc = SGGeoc::fromGeod(previous->turnExitPos);
SGGeoc navid = SGGeoc::fromGeod(wpt->position());
SGGeoc rGc;
double magVar = magVarFor(previous->pos);
double radial = i->radialDegMagnetic() + magVar;
double track = i->courseDegMagnetic() + magVar;
bool ok = geocRadialIntersection(prevGc, track, navid, radial, rGc);
if (!ok) {
// try pulling backward along the radial in case we're too close.
// suggests bad procedure construction if this is happening!
SGGeoc navidAdjusted = SGGeodesy::advanceDegM(navid, radial, -10 * SG_NM_TO_METER);
// try again
ok = geocRadialIntersection(prevGc, track, navidAdjusted, radial, rGc);
if (!ok) {
SG_LOG(SG_NAVAID, SG_WARN, "couldn't compute interception for radial:"
<< previous->turnExitPos << " / " << track << "/" << wpt->position()
<< "/" << radial);
waypoints[index].pos = wpt->position(); // horrible fallback
} else {
waypoints[index].pos = SGGeod::fromGeoc(rGc);
auto previous(previousValidWaypoint(index));
if ((previous == waypoints.end()) || !previous->posValid) {
SG_LOG(SG_NAVAID, SG_WARN, "couldn't compute position for dynamic waypoint: no preceeding valid waypoint");
return;
}
WayptRef wpt = waypoints[index].wpt;
const std::string& ty(wpt->type());
if (ty == "hdgToAlt") {
HeadingToAltitude* h = (HeadingToAltitude*)wpt.get();
double altFt = computeVNAVAltitudeFt(index - 1);
double distanceM = perf.distanceNmBetween(altFt, h->altitudeFt()) * SG_NM_TO_METER;
double hdg = h->headingDegMagnetic() + magVarFor(previous->pos);
waypoints[index].pos = SGGeodesy::direct(previous->turnExitPos, hdg, distanceM);
waypoints[index].posValid = true;
} else if (ty == "radialIntercept") {
// start from previous.turnExit
RadialIntercept* i = (RadialIntercept*)wpt.get();
SGGeoc prevGc = SGGeoc::fromGeod(previous->turnExitPos);
SGGeoc navid = SGGeoc::fromGeod(wpt->position());
SGGeoc rGc;
double magVar = magVarFor(previous->pos);
double radial = i->radialDegMagnetic() + magVar;
double track = i->courseDegMagnetic() + magVar;
bool ok = geocRadialIntersection(prevGc, track, navid, radial, rGc);
if (!ok) {
// try pulling backward along the radial in case we're too close.
// suggests bad procedure construction if this is happening!
SGGeoc navidAdjusted = SGGeodesy::advanceDegM(navid, radial, -10 * SG_NM_TO_METER);
// try again
ok = geocRadialIntersection(prevGc, track, navidAdjusted, radial, rGc);
if (!ok) {
SG_LOG(SG_NAVAID, SG_WARN, "couldn't compute interception for radial:" << previous->turnExitPos << " / " << track << "/" << wpt->position() << "/" << radial);
waypoints[index].pos = wpt->position(); // horrible fallback
} else {
waypoints[index].pos = SGGeod::fromGeoc(rGc);
}
} else {
waypoints[index].pos = SGGeod::fromGeoc(rGc);
}
waypoints[index].posValid = true;
} else if (ty == "dmeIntercept") {
DMEIntercept* di = (DMEIntercept*)wpt.get();
SGGeoc prevGc = SGGeoc::fromGeod(previous->turnExitPos);
SGGeoc navid = SGGeoc::fromGeod(wpt->position());
double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD;
SGGeoc rGc;
double crs = di->courseDegMagnetic() + magVarFor(wpt->position());
SGGeoc bPt = SGGeodesy::advanceDegM(prevGc, crs, 1e5);
double dNm = pointsKnownDistanceFromGC(prevGc, bPt, navid, distRad);
if (dNm < 0.0) {
SG_LOG(SG_NAVAID, SG_WARN, "dmeIntercept failed");
waypoints[index].pos = wpt->position(); // horrible fallback
} else {
waypoints[index].pos = SGGeodesy::direct(previous->turnExitPos, crs, dNm * SG_NM_TO_METER);
}
waypoints[index].posValid = true;
} else if (ty == "vectors") {
waypoints[index].legCourseTrue = SGGeodesy::courseDeg(previous->turnExitPos, waypoints[index].pos);
waypoints[index].legCourseValid = true;
// no turn data
}
} else {
waypoints[index].pos = SGGeod::fromGeoc(rGc);
}
waypoints[index].posValid = true;
} else if (ty == "dmeIntercept") {
DMEIntercept* di = (DMEIntercept*) wpt.get();
SGGeoc prevGc = SGGeoc::fromGeod(previous->turnExitPos);
SGGeoc navid = SGGeoc::fromGeod(wpt->position());
double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD;
SGGeoc rGc;
double crs = di->courseDegMagnetic() + magVarFor(wpt->position());
SGGeoc bPt = SGGeodesy::advanceDegM(prevGc, crs, 1e5);
double dNm = pointsKnownDistanceFromGC(prevGc, bPt, navid, distRad);
if (dNm < 0.0) {
SG_LOG(SG_NAVAID, SG_WARN, "dmeIntercept failed");
waypoints[index].pos = wpt->position(); // horrible fallback
} else {
waypoints[index].pos = SGGeodesy::direct(previous->turnExitPos, crs, dNm * SG_NM_TO_METER);
}
waypoints[index].posValid = true;
} else if (ty == "vectors") {
waypoints[index].legCourseTrue = SGGeodesy::courseDeg(previous->turnExitPos, waypoints[index].pos);
waypoints[index].legCourseValid = true;
// no turn data
}
}
double computeVNAVAltitudeFt(int index)
@ -915,7 +913,7 @@ void RoutePath::commonInit()
nextIt->computeLegCourse(&(d->waypoints[i]), radiusM);
if (nextIt->legCourseValid) {
d->waypoints[i].computeTurn(radiusM, d->constrainLegCourses, *nextIt);
d->waypoints[i].computeTurn(radiusM, d->constrainLegCourses, d->maxFlyByTurnAngleDeg, *nextIt);
} else {
// next waypoint has indeterminate course. Let's create a sharp turn
// this can happen when the following point is ATC vectors, for example.

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

@ -27,10 +27,11 @@
#include "test_suite/FGTestApi/TestPilot.hxx"
#include "test_suite/FGTestApi/TestDataLogger.hxx"
#include <Navaids/NavDataCache.hxx>
#include <Navaids/navrecord.hxx>
#include <Navaids/navlist.hxx>
#include <Navaids/FlightPlan.hxx>
#include <Navaids/NavDataCache.hxx>
#include <Navaids/fixlist.hxx>
#include <Navaids/navlist.hxx>
#include <Navaids/navrecord.hxx>
#include <Instrumentation/gps.hxx>
#include <Instrumentation/navradio.hxx>
@ -1379,6 +1380,66 @@ void GPSTests::testTurnAnticipation()
}
void GPSTests::testExceedFlyByMaxAngleTurn()
{
// FGTestApi::setUp::logPositionToKML("gps_fly_by_exceed_max_angle");
auto rm = globals->get_subsystem<FGRouteMgr>();
auto fp = FlightPlan::create();
rm->setFlightPlan(fp);
FGTestApi::setUp::populateFPWithoutNasal(fp, "LFBD", "23", "EHAM", "18L", "SOMOS ROYAN TIRAV BOBRI ADABI");
FGTestApi::writeFlightPlanToKML(fp);
// takes the place of the Nasal delegates
auto testDelegate = new TestFPDelegate;
testDelegate->thePlan = fp;
CPPUNIT_ASSERT(rm->activate());
fp->addDelegate(testDelegate);
auto gps = setupStandardGPS();
fp->setCurrentIndex(4); // BOBRI
// somehwat before BOBRI
SGGeod initPos = fp->pointAlongRouteNorm(4, -0.1);
FGTestApi::setPositionAndStabilise(initPos);
FGTestApi::writePointToKML("init point", initPos);
auto gpsNode = globals->get_props()->getNode("instrumentation/gps");
gpsNode->setBoolValue("config/delegate-sequencing", true);
gpsNode->setDoubleValue("config/max-fly-by-turn-angle-deg", 75.0);
gpsNode->setStringValue("command", "leg");
FGPositioned::TypeFilter fixFilter(FGPositioned::FIX);
auto tiravFix = FGPositioned::findFirstWithIdent("TIRAV", &fixFilter);
auto bobriFix = FGPositioned::findFirstWithIdent("BOBRI", &fixFilter);
auto adabiFix = FGPositioned::findFirstWithIdent("ADABI", &fixFilter);
auto pilot = SGSharedPtr<FGTestApi::TestPilot>(new FGTestApi::TestPilot);
pilot->resetAtPosition(initPos);
pilot->setCourseTrue(fp->legAtIndex(4)->courseDeg());
pilot->setSpeedKts(280);
pilot->flyGPSCourse(gps);
bool ok = FGTestApi::runForTimeWithCheck(1200.0, [&fp]() {
return fp->currentIndex() == 5;
});
CPPUNIT_ASSERT(ok);
// ensure we did a fly-over
CPPUNIT_ASSERT(!gps->previousLegData().value().didFlyBy);
FGTestApi::writePointToKML("seq point", globals->get_aircraft_position());
// ensure leg course is the one we want
const auto crs = SGGeodesy::courseDeg(bobriFix->geod(), adabiFix->geod());
CPPUNIT_ASSERT_DOUBLES_EQUAL(crs, gpsNode->getDoubleValue("wp/leg-true-course-deg"), 1.0);
// ensure we fly the comepnsation turns, so that end up back on the next leg's course
// exactly
FGTestApi::runForTime(60.0);
CPPUNIT_ASSERT_DOUBLES_EQUAL(crs, gpsNode->getDoubleValue("wp/leg-true-course-deg"), 1.0);
}
void GPSTests::testRadialIntercept()
{
// FGTestApi::setUp::logPositionToKML("gps_radial_intercept");
@ -1624,19 +1685,26 @@ void GPSTests::testCourseLegIntermediateWaypoint()
auto wpNode = gpsNode->getChild("wp", 0, true);
CPPUNIT_ASSERT_DOUBLES_EQUAL(56.5, legToDecel->courseDeg(), 1.0);
CPPUNIT_ASSERT_DOUBLES_EQUAL(56.5, legToBlaca->courseDeg(), 1.0);
FGPositioned::TypeFilter fixFilter(FGPositioned::FIX);
auto lisbo = FGPositioned::findClosestWithIdent("LISBO", fp->departureAirport()->geod(), &fixFilter);
auto blaca = FGPositioned::findClosestWithIdent("BLACA", fp->departureAirport()->geod(), &fixFilter);
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);
const double crsToDecel = SGGeodesy::courseDeg(lisbo->geod(), decelPos);
const double crsToBlaca = SGGeodesy::courseDeg(decelPos, blaca->geod());
CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToDecel, legToDecel->courseDeg(), 1.0);
CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToBlaca, legToBlaca->courseDeg(), 1.0);
CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToDecel, gpsNode->getDoubleValue("desired-course-deg"), 2.0);
CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToDecel, wpNode->getDoubleValue("leg-true-course-deg"), 0.5);
CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToDecel, 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);
CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToBlaca, gpsNode->getDoubleValue("desired-course-deg"), 2.0);
CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToBlaca, wpNode->getDoubleValue("leg-true-course-deg"), 0.5);
CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToBlaca, wpNode->getDoubleValue("leg-mag-course-deg"), 0.5);
auto pilot = SGSharedPtr<FGTestApi::TestPilot>(new FGTestApi::TestPilot);
pilot->resetAtPosition(initPos);

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

@ -56,6 +56,7 @@ class GPSTests : public CppUnit::TestFixture
CPPUNIT_TEST(testDMEIntercept);
CPPUNIT_TEST(testFinalLegCourse);
CPPUNIT_TEST(testCourseLegIntermediateWaypoint);
CPPUNIT_TEST(testExceedFlyByMaxAngleTurn);
CPPUNIT_TEST_SUITE_END();
@ -92,6 +93,7 @@ public:
void testDMEIntercept();
void testFinalLegCourse();
void testCourseLegIntermediateWaypoint();
void testExceedFlyByMaxAngleTurn();
};
#endif // _FG_GPS_UNIT_TESTS_HXX