diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx index 7de8d0d5f..0de9b051c 100644 --- a/src/Instrumentation/gps.cxx +++ b/src/Instrumentation/gps.cxx @@ -91,6 +91,14 @@ void GPS::Config::bind(GPS* aOwner, SGPropertyNode* aCfg) aOwner->tie(aCfg, "over-flight-arm-distance-nm", SGRawValuePointer(&_overflightArmDistance)); aOwner->tie(aCfg, "over-flight-arm-angle-deg", SGRawValuePointer(&_overflightArmAngle)); aOwner->tie(aCfg, "delegate-sequencing", SGRawValuePointer(&_delegateSequencing)); + aOwner->tie(aCfg, "max-fly-by-turn-angle-deg", SGRawValueMethods(*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 GPS::nextLegTrack() { auto next = _route->nextLeg(); diff --git a/src/Instrumentation/gps.hxx b/src/Instrumentation/gps.hxx index efcc1e0cb..68db9bfb3 100644 --- a/src/Instrumentation/gps.hxx +++ b/src/Instrumentation/gps.hxx @@ -86,7 +86,8 @@ public: double overflightArmDistanceM() override; double overflightArmAngleDeg() override; bool canFlyBy() const override; - + double maxFlyByTurnAngleDeg() const override; + simgear::optional previousLegData() override; simgear::optional 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 diff --git a/src/Instrumentation/rnav_waypt_controller.cxx b/src/Instrumentation/rnav_waypt_controller.cxx index 4a647090a..8d11081c8 100644 --- a/src/Instrumentation/rnav_waypt_controller.cxx +++ b/src/Instrumentation/rnav_waypt_controller.cxx @@ -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; diff --git a/src/Instrumentation/rnav_waypt_controller.hxx b/src/Instrumentation/rnav_waypt_controller.hxx index 6b3596ea8..8a0062039 100644 --- a/src/Instrumentation/rnav_waypt_controller.hxx +++ b/src/Instrumentation/rnav_waypt_controller.hxx @@ -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. */ diff --git a/src/Navaids/FlightPlan.cxx b/src/Navaids/FlightPlan.cxx index 21ddc8082..cb3211e5c 100644 --- a/src/Navaids/FlightPlan.cxx +++ b/src/Navaids/FlightPlan.cxx @@ -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; diff --git a/src/Navaids/FlightPlan.hxx b/src/Navaids/FlightPlan.hxx index a5cce8d8e..e86e5fb7c 100644 --- a/src/Navaids/FlightPlan.hxx +++ b/src/Navaids/FlightPlan.hxx @@ -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; - SGSharedPtr _star; - SGSharedPtr _approach; - std::string _sidTransition, _starTransition, _approachTransition; + FGAirportRef _departure, _destination; + FGAirportRef _alternate; + FGRunway *_departureRunway, *_destinationRunway; + SGSharedPtr _sid; + SGSharedPtr _star; + SGSharedPtr _approach; + std::string _sidTransition, _starTransition, _approachTransition; - double _totalDistance; - void rebuildLegData(); + double _totalDistance; + void rebuildLegData(); - using LegVec = std::vector; - LegVec _legs; + using LegVec = std::vector; + LegVec _legs; - std::vector _delegates; + std::vector _delegates; }; } // of namespace flightgear diff --git a/src/Navaids/routePath.cxx b/src/Navaids/routePath.cxx index ffcb47b36..24eaa93c6 100644 --- a/src/Navaids/routePath.cxx +++ b/src/Navaids/routePath.cxx @@ -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(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(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(wpt.get())->runway(); - turnExitCenter = turnCenterOverflight(rwy->end(), rwy->headingDeg(), turnExitAngle, turnRadius); + if (legCourseValid) { + if (isRunway) { + FGRunway* rwy = static_cast(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(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(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. diff --git a/test_suite/unit_tests/Instrumentation/test_gps.cxx b/test_suite/unit_tests/Instrumentation/test_gps.cxx index 8629e1f8d..29823c155 100644 --- a/test_suite/unit_tests/Instrumentation/test_gps.cxx +++ b/test_suite/unit_tests/Instrumentation/test_gps.cxx @@ -27,10 +27,11 @@ #include "test_suite/FGTestApi/TestPilot.hxx" #include "test_suite/FGTestApi/TestDataLogger.hxx" -#include -#include -#include #include +#include +#include +#include +#include #include #include @@ -1379,6 +1380,66 @@ void GPSTests::testTurnAnticipation() } +void GPSTests::testExceedFlyByMaxAngleTurn() +{ + // FGTestApi::setUp::logPositionToKML("gps_fly_by_exceed_max_angle"); + auto rm = globals->get_subsystem(); + 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(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(new FGTestApi::TestPilot); pilot->resetAtPosition(initPos); diff --git a/test_suite/unit_tests/Instrumentation/test_gps.hxx b/test_suite/unit_tests/Instrumentation/test_gps.hxx index d364cd714..03986d2f7 100644 --- a/test_suite/unit_tests/Instrumentation/test_gps.hxx +++ b/test_suite/unit_tests/Instrumentation/test_gps.hxx @@ -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