From 6bb14780679249d45a265a610ad8e907e4c7c363 Mon Sep 17 00:00:00 2001 From: James Turner Date: Sat, 30 May 2020 15:55:30 +0100 Subject: [PATCH] GPS fly-by support Remove the non-functional turn-anticipation code from the GPS, and enable fly-by mode in (some of the) RNAV controllers: initially the leg controller. The new config property is gps/config/enable-fly-by, defaults to off for compatibility. --- src/Instrumentation/gps.cxx | 244 +++++------------- src/Instrumentation/gps.hxx | 48 ++-- src/Instrumentation/rnav_waypt_controller.cxx | 236 ++++++++++++++--- src/Instrumentation/rnav_waypt_controller.hxx | 43 ++- src/Navaids/route.hxx | 3 +- 5 files changed, 325 insertions(+), 249 deletions(-) diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx index fb3b240ca..9a3470680 100644 --- a/src/Instrumentation/gps.cxx +++ b/src/Instrumentation/gps.cxx @@ -60,7 +60,7 @@ static const char* makeTTWString(double TTW) // configuration helper object GPS::Config::Config() : - _enableTurnAnticipation(true), + _enableTurnAnticipation(false), _turnRate(3.0), // degrees-per-second, so 180 degree turn takes 60 seconds _overflightDistance(0.02), _overflightArmDistance(1.0), @@ -72,13 +72,12 @@ GPS::Config::Config() : _courseSelectable(false), _followLegTrackToFix(true) { - _enableTurnAnticipation = false; } void GPS::Config::bind(GPS* aOwner, SGPropertyNode* aCfg) { aOwner->tie(aCfg, "turn-rate-deg-sec", SGRawValuePointer(&_turnRate)); - aOwner->tie(aCfg, "turn-anticipation", SGRawValuePointer(&_enableTurnAnticipation)); + aOwner->tie(aCfg, "enable-fly-by", SGRawValuePointer(&_enableTurnAnticipation)); aOwner->tie(aCfg, "wpt-alert-time", SGRawValuePointer(&_waypointAlertTime)); aOwner->tie(aCfg, "hard-surface-runways-only", SGRawValuePointer(&_requireHardSurface)); aOwner->tie(aCfg, "cdi-max-deflection-nm", SGRawValuePointer(&_cdiMaxDeflectionNm)); @@ -102,9 +101,6 @@ GPS::GPS ( SGPropertyNode *node, bool defaultGPSMode) : _mode("init"), _name(node->getStringValue("name", "gps")), _num(node->getIntValue("number", 0)), - _computeTurnData(false), - _anticipateTurn(false), - _inTurn(false), _callbackFlightPlanChanged(SGPropertyChangeCallback(this,&GPS::routeManagerFlightPlanChanged, fgGetNode("/autopilot/route-manager/signals/flightplan-changed", true))), _callbackRouteActivated(SGPropertyChangeCallback(this,&GPS::routeActivated, @@ -332,7 +328,9 @@ GPS::update (double delta_time_sec) _gpsNode->setStringValue("rnav-controller-status", _wayptController->status()); - updateTurn(); + if (_wayptController->isDone()) { + doSequence(); + } updateRouteData(); } @@ -467,26 +465,47 @@ double GPS::selectedMagCourse() return _selectedCourse; } -SGGeod GPS::previousLegWaypointPosition(bool& isValid) +simgear::optional GPS::nextLegTrack() { - FlightPlan::Leg* leg = _route->previousLeg(); - if (leg){ - Waypt* waypt = leg->waypoint(); - if (waypt) { - isValid = true; - // ensure computations use runway end, not threshold - if (waypt->type() == "runway") { - RunwayWaypt* rwpt = static_cast(waypt); - return rwpt->runway()->end(); - } - - return waypt->position(); - } - } - - isValid = false; - return SGGeod(); + auto next = _route->nextLeg(); + if (!next) + return {}; + + return next->courseDeg(); } + +simgear::optional GPS::previousLegData() +{ + // if the previous controller computed valid data, + // use that. This ensures fly-by turns work out, especially + if (_wp0Data.has_value()) + return _wp0Data; + + // if we did not get data from the previous controller (eg, waypoint + // jumped or first waypoint), just compute the position + FlightPlan::Leg* leg = _route->previousLeg(); + if (!leg) { + return {}; // no data + } + + LegData legData; + Waypt* waypt = leg->waypoint(); + legData.position = waypt->position(); + + // ensure computations use runway end, not threshold + if (waypt->type() == "runway") { + RunwayWaypt* rwpt = static_cast(waypt); + legData.position = rwpt->runway()->end(); + } + + return legData; +} + +bool GPS::canFlyBy() const +{ + return _config.turnAnticipationEnabled(); +} + /////////////////////////////////////////////////////////////////////////// void @@ -560,6 +579,7 @@ GPS::updateTrackingBug() void GPS::currentWaypointChanged() { + _wp0Data.reset(); if (!_route) { return; } @@ -583,7 +603,12 @@ void GPS::currentWaypointChanged() } _currentWaypt = _route->currentLeg()->waypoint(); - wp1Changed(); // rebuild the active controller + if (_wayptController && (_wayptController->waypoint() == _prevWaypt)) { + // capture leg data form the controller, before we destroy it + _wp0Data = _wayptController->legData(); + } + + wp1Changed(); // rebuild the active controller _desiredCourse = getLegMagCourse(); _desiredCourseNode->fireValueChanged(); } @@ -661,164 +686,11 @@ void GPS::endOfFlightPlan() cleared(); } -void GPS::updateTurn() -{ - bool printProgress = false; - - if (_computeTurnData) { - if (_last_speed_kts < 10) { - // need valid leg course and sensible ground speed to compute the turn - return; - } - - computeTurnData(); - printProgress = true; - } - - if (!_anticipateTurn) { - updateOverflight(); - return; - } - - updateTurnData(); - // find bearing to turn centre - double bearing, az2, distanceM; - SGGeodesy::inverse(_indicated_pos, _turnCentre, bearing, az2, distanceM); - double progress = computeTurnProgress(bearing); - - if (printProgress) { - SG_LOG(SG_INSTR, SG_INFO,"turn progress=" << progress); - } - - if (!_inTurn && (progress > 0.0)) { - beginTurn(); - } - - if (_inTurn && !_turnSequenced && (progress > 0.5)) { - _turnSequenced = true; - SG_LOG(SG_INSTR, SG_INFO, "turn passed midpoint, sequencing"); - doSequence(); - } - - if (_inTurn && (progress >= 1.0)) { - endTurn(); - } - - if (_inTurn) { - // drive deviation and desired course - double desiredCourse = bearing - copysign(90, _turnAngle); - SG_NORMALIZE_RANGE(desiredCourse, 0.0, 360.0); - double deviationNm = (distanceM * SG_METER_TO_NM) - _turnRadius; - double deviationDeg = desiredCourse - getMagTrack(); - deviationNm = copysign(deviationNm, deviationDeg); - // FIXME - //_wp1_course_deviation_node->setDoubleValue(deviationDeg); - //_wp1_course_error_nm_node->setDoubleValue(deviationNm); - //_cdiDeflectionNode->setDoubleValue(deviationDeg); - } -} - -void GPS::updateOverflight() -{ - if (_wayptController->isDone()) { - doSequence(); - _computeTurnData = true; - } -} - -void GPS::beginTurn() -{ - _inTurn = true; - _turnSequenced = false; - SG_LOG(SG_INSTR, SG_INFO, "beginning turn"); -} - -void GPS::endTurn() -{ - _inTurn = false; - SG_LOG(SG_INSTR, SG_INFO, "ending turn"); - _computeTurnData = true; -} - -double GPS::computeTurnProgress(double aBearing) const -{ - double startBearing = _turnStartBearing + copysign(90, _turnAngle); - return (aBearing - startBearing) / _turnAngle; -} - -void GPS::computeTurnData() -{ - _computeTurnData = false; - if ((_mode != "leg") || !_route->nextLeg()) { - _anticipateTurn = false; - return; - } - - WayptRef next = _route->nextLeg()->waypoint(); - if (next->flag(WPT_DYNAMIC) || - !_config.turnAnticipationEnabled() || - next->flag(WPT_OVERFLIGHT)) - { - _anticipateTurn = false; - return; - } - - _turnStartBearing = _desiredCourse; -// compute next leg course - RoutePath path(_route); - double crs = path.trackForIndex(_route->currentIndex() + 1); - -// compute offset bearing - _turnAngle = crs - _turnStartBearing; - SG_NORMALIZE_RANGE(_turnAngle, -180.0, 180.0); - double median = _turnStartBearing + (_turnAngle * 0.5); - double offsetBearing = median + copysign(90, _turnAngle); - SG_NORMALIZE_RANGE(offsetBearing, 0.0, 360.0); - - SG_LOG(SG_INSTR, SG_INFO, "GPS computeTurnData: in=" << _turnStartBearing << - ", out=" << crs << "; turnAngle=" << _turnAngle << ", median=" << median - << ", offset=" << offsetBearing); - - SG_LOG(SG_INSTR, SG_INFO, "next leg is now:" << _currentWaypt->ident() << "->" << next->ident()); - - _turnPt = _currentWaypt->position(); - _anticipateTurn = true; -} - -void GPS::updateTurnData() -{ - // depends on ground speed, so needs to be updated per-frame - _turnRadius = computeTurnRadiusNm(_last_speed_kts); - - // compute the turn centre, based on the turn radius. - // key thing is to understand that we're working a right-angle triangle, - // where the right-angle is the point we start the turn. From that point, - // one side is the inbound course to the turn pt, and the other is the - // perpendicular line, of length 'r', to the turn centre. - // the triangle's hypotenuse, which we need to find, is the distance from the - // turn pt to the turn center (in the direction of the offset bearing) - // note that d - _turnRadius tell us how much we're 'cutting' the corner. - - double halfTurnAngle = fabs(_turnAngle * 0.5) * SG_DEGREES_TO_RADIANS; - double d = _turnRadius / cos(halfTurnAngle); - - // SG_LOG(SG_INSTR, SG_INFO, "turnRadius=" << _turnRadius << ", d=" << d - // << " (cut distance=" << d - _turnRadius << ")"); - - double median = _turnStartBearing + (_turnAngle * 0.5); - double offsetBearing = median + copysign(90, _turnAngle); - SG_NORMALIZE_RANGE(offsetBearing, 0.0, 360.0); - - double az2; - SGGeodesy::direct(_turnPt, offsetBearing, d * SG_NM_TO_METER, _turnCentre, az2); -} - double GPS::turnRadiusNm(double groundSpeedKts) { return computeTurnRadiusNm(groundSpeedKts); } - double GPS::computeTurnRadiusNm(double aGroundSpeedKts) const { // turn time is seconds to execute a 360 turn. @@ -877,6 +749,7 @@ void GPS::wp1Changed() { if (!_currentWaypt) return; + if (_mode == "leg") { _wayptController.reset(WayptController::createForWaypt(this, _currentWaypt)); if (_currentWaypt->type() == "hold") { @@ -890,16 +763,16 @@ void GPS::wp1Changed() } else if (_mode == "dto") { _wayptController.reset(new DirectToController(this, _currentWaypt, _wp0_position)); } - + _wayptController->init(); _gpsNode->setStringValue("rnav-controller-status", _wayptController->status()); - + if (_mode == "obs") { _legDistanceNm = -1.0; } else { _wayptController->update(0.0); _gpsNode->setStringValue("rnav-controller-status", _wayptController->status()); - + _legDistanceNm = _wayptController->distanceToWayptM() * SG_METER_TO_NM; // synchronise these properties immediately @@ -907,7 +780,7 @@ void GPS::wp1Changed() _currentWayptNode->setDoubleValue("longitude-deg", p.getLongitudeDeg()); _currentWayptNode->setDoubleValue("latitude-deg", p.getLatitudeDeg()); _currentWayptNode->setDoubleValue("altitude-ft", p.getElevationFt()); - + _desiredCourse = getLegMagCourse(); } } @@ -1308,6 +1181,11 @@ void GPS::selectLegMode() _mode = "leg"; +// clear any previous leg data which might be hanging around +// note this means you can mess up fly-by by toggling into and out LEG +// mode, but this seems reasonable + _wp0Data.reset(); + // depending on the situation, this will either get over-written // in routeManagerSequenced or not; either way it does no harm to // set it here. diff --git a/src/Instrumentation/gps.hxx b/src/Instrumentation/gps.hxx index 889f0bf45..200499eb7 100644 --- a/src/Instrumentation/gps.hxx +++ b/src/Instrumentation/gps.hxx @@ -76,15 +76,19 @@ public: // RNAV interface SGGeod position() override; - virtual double trackDeg(); - virtual double groundSpeedKts(); - virtual double vspeedFPM(); - virtual double magvarDeg(); - virtual double selectedMagCourse(); - virtual double overflightDistanceM(); - virtual double overflightArmDistanceM(); - virtual double overflightArmAngleDeg(); - virtual SGGeod previousLegWaypointPosition(bool& isValid); + double trackDeg() override; + double groundSpeedKts() override; + double vspeedFPM() override; + double magvarDeg() override; + double selectedMagCourse() override; + double overflightDistanceM() override; + double overflightArmDistanceM() override; + double overflightArmAngleDeg() override; + bool canFlyBy() const override; + + simgear::optional previousLegData() override; + + simgear::optional nextLegTrack() override; double turnRadiusNm(double groundSpeedKnots) override; private: @@ -208,17 +212,7 @@ private: void updateTrackingBug(); void updateRouteData(); void driveAutopilot(); - - void updateTurn(); - void updateOverflight(); - void beginTurn(); - void endTurn(); - - double computeTurnProgress(double aBearing) const; - void computeTurnData(); - void updateTurnData(); - double computeTurnRadiusNm(double aGroundSpeedKts) const; - + /** Update one-shot things when WP1 / leg data change */ void wp1Changed(); @@ -307,6 +301,7 @@ private: // true-bearing-error and mag-bearing-error + double computeTurnRadiusNm(double aGroundSpeedKts) const; /** * Tied-properties helper, record nodes which are tied for easy un-tie-ing @@ -403,17 +398,8 @@ private: bool _searchNames; ///< set if we're searching names instead of idents #endif - // turn data - bool _computeTurnData; ///< do we need to update the turn data? - bool _anticipateTurn; ///< are we anticipating the next turn or not? - bool _inTurn; // is a turn in progress? - bool _turnSequenced; // have we sequenced the new leg? - double _turnAngle; // angle to turn through, in degrees - double _turnStartBearing; // bearing of inbound leg - double _turnRadius; // radius of turn in nm - SGGeod _turnPt; - SGGeod _turnCentre; - + simgear::optional _wp0Data; + std::unique_ptr _wayptController; flightgear::WayptRef _prevWaypt; diff --git a/src/Instrumentation/rnav_waypt_controller.cxx b/src/Instrumentation/rnav_waypt_controller.cxx index 80e0b5a14..55d893f11 100644 --- a/src/Instrumentation/rnav_waypt_controller.cxx +++ b/src/Instrumentation/rnav_waypt_controller.cxx @@ -28,6 +28,8 @@ #include #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 @@ -243,64 +245,213 @@ public: throw sg_exception("LegWayptCtl doesn't work with dynamic waypoints"); } } - + virtual void init() { - bool isPreviousLegValid = false; - - _waypointOrigin = _rnav->previousLegWaypointPosition(isPreviousLegValid); - - _initialLegCourse = SGGeodesy::courseDeg(_waypointOrigin,_waypt->position()); - _courseAircraftToTarget = SGGeodesy::courseDeg(_rnav->position(),_waypt->position()); - _distanceAircraftTargetMetre = SGGeodesy::distanceM(_rnav->position(),_waypt->position()); - - - // check reach the leg in 45Deg or going direct - bool canReachLeg = (fabs(_initialLegCourse -_courseAircraftToTarget) < 45.0); - - if (isPreviousLegValid && canReachLeg) { + auto previousLeg = _rnav->previousLegData(); + if (previousLeg) { + _waypointOrigin = previousLeg.value().position; + _initialLegCourse = SGGeodesy::courseDeg(_waypointOrigin,_waypt->position()); + } else { + // capture current position + _waypointOrigin = _rnav->position(); + } + + _courseAircraftToTarget = SGGeodesy::courseDeg(_rnav->position(),_waypt->position()); + _distanceAircraftTargetMetre = SGGeodesy::distanceM(_rnav->position(),_waypt->position()); + + + // check reach the leg in 45Deg or going direct + bool canReachLeg = (fabs(_initialLegCourse -_courseAircraftToTarget) < 45.0); + + if (previousLeg && canReachLeg) { _targetTrack = _initialLegCourse; - } else { + } else { // can't reach the leg with out a crazy turn, just go direct to the // destination waypt _targetTrack = _courseAircraftToTarget; _initialLegCourse = _courseAircraftToTarget; _waypointOrigin = _rnav->position(); - } + } + + // turn angle depends on final leg course, not initial + // do this here so we have a chance of doing a fly-by at the end of the leg + _finalLegCourse = SGGeodesy::courseDeg(_waypt->position(), _waypointOrigin) + 180; + SG_NORMALIZE_RANGE(_finalLegCourse, 0.0, 360.0); + + // turn-in logic + if (previousLeg.has_value() && previousLeg.value().didFlyBy) { + _flyByTurnCenter = previousLeg.value().flyByTurnCenter; + _flyByTurnAngle = previousLeg.value().turnAngle; + _flyByTurnRadius = previousLeg.value().flyByRadius; + _entryFlyByActive = true; + SG_NORMALIZE_RANGE(_flyByTurnAngle, -180.0, 180.0); + } + } + + void computeTurnAnticipation() + { + _didComputeTurn = true; + + if (_waypt->flag(WPT_OVERFLIGHT)) { + return; // can't fly-by + } + + if (!_rnav->canFlyBy()) + return; + + auto nextLegTrack = _rnav->nextLegTrack(); + + if (!nextLegTrack.has_value()) { + return; + } + + _flyByTurnAngle = nextLegTrack.value() - _finalLegCourse; + SG_NORMALIZE_RANGE(_flyByTurnAngle, -180.0, 180.0); + + if (fabs(_flyByTurnAngle) > 120.0) { + // too sharp, don't anticipate + return; + } + + _flyByTurnRadius = _rnav->turnRadiusNm() * SG_NM_TO_METER; + + // this is the heading half way through the turn. Perpendicular to + // this is our turn ceenter + const auto halfTurnHeading = _finalLegCourse + (_flyByTurnAngle * 0.5); + double p = copysign(90.0, _flyByTurnAngle); + double h = halfTurnHeading + p; + SG_NORMALIZE_RANGE(h, 0.0, 360.0); + + const double tcOffset = _flyByTurnRadius / cos(_flyByTurnAngle * 0.5 * SG_DEGREES_TO_RADIANS); + _flyByTurnCenter = SGGeodesy::direct(_waypt->position(), h, tcOffset); + _doFlyBy = true; + } + + bool updateInTurn() + { + auto dl = FGTestApi::DataLogger::instance(); + + // find bearing to turn center + // when it hits 90 off our track + + auto bearingToTurnCenter = SGGeodesy::courseDeg(_rnav->position(), _flyByTurnCenter); + auto distToTurnCenter = SGGeodesy::distanceM(_rnav->position(), _flyByTurnCenter); + + if (!_flyByStarted) { + // check for the turn center being 90 degrees off one wing (start the turn) + auto a = bearingToTurnCenter - _finalLegCourse; + SG_NORMALIZE_RANGE(a, -180.0, 180.0); + if (fabs(a) < 90.0) { + dl->recordSamplePoint("turn-entry-bearing", a); + return false; // keep flying normal leg + } + + _flyByStarted = true; + } + + // check for us passing the half-way point; that's when we should + // sequence to the next WP + const auto halfPointAngle = (_finalLegCourse + (_flyByTurnAngle * 0.5)); + auto b = bearingToTurnCenter - halfPointAngle; + SG_NORMALIZE_RANGE(b, -180.0, 180.0); + + dl->recordSamplePoint("turn-exit-bearing", b); + + if (fabs(b) >= 90.0) { + _toFlag = false; + setDone(); + } + + // in the actual turn, our desired track is always pependicular to the + // bearing to the turn center we computed + _targetTrack = bearingToTurnCenter - copysign(90, _flyByTurnAngle); + + SG_NORMALIZE_RANGE(_targetTrack, 0.0, 360.0); + + _crossTrackError = (distToTurnCenter - _flyByTurnRadius) * SG_METER_TO_NM; + _courseDev = _crossTrackError * 10.0; // arbitrary guess for now + + return true; + } + + void updateInEntryTurn() + { + auto dl = FGTestApi::DataLogger::instance(); + + + auto bearingToTurnCenter = SGGeodesy::courseDeg(_rnav->position(), _flyByTurnCenter); + auto distToTurnCenter = SGGeodesy::distanceM(_rnav->position(), _flyByTurnCenter); + + auto b = bearingToTurnCenter - _initialLegCourse; + dl->recordSamplePoint("turn-entry-bearing", b); + + SG_NORMALIZE_RANGE(b, -180.0, 180.0); + if (fabs(b) >= 90.0) { + _entryFlyByActive = false; + return; // we're done with the entry turn + } + + _targetTrack = bearingToTurnCenter - copysign(90, _flyByTurnAngle); + + SG_NORMALIZE_RANGE(_targetTrack, 0.0, 360.0); + _crossTrackError = (distToTurnCenter - _flyByTurnRadius) * SG_METER_TO_NM; + _courseDev = _crossTrackError * 10.0; // arbitrary guess for now } - virtual void update(double) + void update(double) override { + auto dl = FGTestApi::DataLogger::instance(); + _courseOriginToAircraft = SGGeodesy::courseDeg(_waypointOrigin,_rnav->position()); _distanceOriginAircraftMetre = SGGeodesy::distanceM(_waypointOrigin,_rnav->position()); _courseAircraftToTarget = SGGeodesy::courseDeg(_rnav->position(),_waypt->position()); _distanceAircraftTargetMetre = SGGeodesy::distanceM(_rnav->position(),_waypt->position()); + if (_entryFlyByActive) { + updateInEntryTurn(); + return; + } + + const auto turnComputeDist = SG_NM_TO_METER * 4.0; + if (!_didComputeTurn && (_distanceAircraftTargetMetre < turnComputeDist)) { + computeTurnAnticipation(); + } + + if (_didComputeTurn && _doFlyBy) { + bool ok = updateInTurn(); + if (ok) { + return; + } + + // otherwise we fall through + } + // from the Aviation Formulary #if 0 - Suppose you are proceeding on a great circle route from A to B (course =crs_AB) and end up at D, - perhaps off course. (We presume that A is ot a pole!) You can calculate the course from A to D - (crs_AD) and the distance from A to D (dist_AD) using the formulae above. In terms of these the - cross track error, XTD, (distance off course) is given by - - XTD =asin(sin(dist_AD)*sin(crs_AD-crs_AB)) - (positive XTD means right of course, negative means left) - (If the point A is the N. or S. Pole replace crs_AD-crs_AB with - lon_D-lon_B or lon_B-lon_D, respectively.) + Suppose you are proceeding on a great circle route from A to B (course =crs_AB) and end up at D, + perhaps off course. (We presume that A is ot a pole!) You can calculate the course from A to D + (crs_AD) and the distance from A to D (dist_AD) using the formulae above. In terms of these the + cross track error, XTD, (distance off course) is given by + + XTD =asin(sin(dist_AD)*sin(crs_AD-crs_AB)) + (positive XTD means right of course, negative means left) + (If the point A is the N. or S. Pole replace crs_AD-crs_AB with + lon_D-lon_B or lon_B-lon_D, respectively.) #endif // however, just for fun, our convention for polarity of the cross-track // sign is opposite, so we add a -ve to the computation below. - + double distOriginAircraftRad = _distanceOriginAircraftMetre * SG_METER_TO_NM * SG_NM_TO_RAD; double xtkRad = asin(sin(distOriginAircraftRad) * sin((_courseOriginToAircraft - _initialLegCourse) * SG_DEGREES_TO_RADIANS)); - + // convert to NM and flip sign for consistency with existing code. // since we derive the abeam point and course-deviation from this, and // thus the GPS cdi-deflection, if we don't fix this here, the sign of // all of those comes out backwards. _crossTrackError = -(xtkRad * SG_RAD_TO_NM); - + /* The "along track distance", ATD, the distance from A along the course towards B to the point abeam D is given by: @@ -324,6 +475,7 @@ public: _courseDev = _courseAircraftToTarget - _targetTrack; SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0); + bool isMinimumOverFlightDistanceReached = _distanceAircraftTargetMetre < _rnav->overflightDistanceM(); bool isOverFlightConeArmed = _distanceAircraftTargetMetre < ( _rnav->overflightArmDistanceM() + _rnav->overflightDistanceM() ); bool leavingOverFlightCone = (fabs(_courseDev) > _rnav->overflightArmAngleDeg()); @@ -343,6 +495,22 @@ public: } } + simgear::optional legData() const override + { + RNAV::LegData r; + r.position = _waypt->position(); + if (_doFlyBy) { + // copy all the fly by paramters, so the next controller can + // smoothly link up + r.didFlyBy = true; + r.flyByRadius = _flyByTurnRadius; + r.flyByTurnCenter = _flyByTurnCenter; + r.turnAngle = _flyByTurnAngle; + } + + return r; + } + virtual double distanceToWayptM() const { return _distanceAircraftTargetMetre; @@ -381,6 +549,7 @@ private: */ SGGeod _waypointOrigin; double _initialLegCourse; + double _finalLegCourse; double _distanceOriginAircraftMetre; double _distanceAircraftTargetMetre; double _courseOriginToAircraft; @@ -388,7 +557,14 @@ private: double _courseDev; bool _toFlag; double _crossTrackError; - + + bool _didComputeTurn = false; + bool _doFlyBy = false; + SGGeod _flyByTurnCenter; + double _flyByTurnAngle; + double _flyByTurnRadius; + bool _entryFlyByActive = false; + bool _flyByStarted = false; }; /** diff --git a/src/Instrumentation/rnav_waypt_controller.hxx b/src/Instrumentation/rnav_waypt_controller.hxx index e71edaa20..763170b5f 100644 --- a/src/Instrumentation/rnav_waypt_controller.hxx +++ b/src/Instrumentation/rnav_waypt_controller.hxx @@ -21,6 +21,7 @@ #define FG_WAYPT_CONTROLLER_HXX #include +#include namespace flightgear { @@ -58,7 +59,12 @@ public: * device selected course (eg, from autopilot / MCP / OBS) in degrees */ virtual double selectedMagCourse() = 0; - + + virtual bool canFlyBy() const + { + return false; + } + /** * minimum distance to switch next waypoint. */ @@ -72,15 +78,31 @@ public: */ virtual double overflightArmAngleDeg() = 0; + struct LegData { + SGGeod position; + bool didFlyBy = false; + SGGeod flyByTurnCenter; + double flyByRadius = 0.0; + double turnAngle = 0.0; + }; + /** * device leg previous waypoint position(eg, from route manager) */ - virtual SGGeod previousLegWaypointPosition(bool& isValid)= 0; - + virtual simgear::optional previousLegData() + { + return simgear::optional(); + } + + virtual simgear::optional nextLegTrack() + { + return simgear::optional{}; + } + /** * @brief compute turn radius based on current ground-speed */ - + double turnRadiusNm() { return turnRadiusNm(groundSpeedKts()); @@ -151,11 +173,24 @@ public: */ virtual std::string status() const; + virtual simgear::optional legData() const + { + // defer to our subcontroller if it exists + if (_subController) + return _subController->legData(); + + return simgear::optional(); + } + /** * Static factory method, given a waypoint, return a controller bound * to it, of the appropriate type */ static WayptController* createForWaypt(RNAV* rnav, const WayptRef& aWpt); + + WayptRef waypoint() const + { return _waypt; } + protected: WayptController(RNAV* aRNAV, const WayptRef& aWpt) : _waypt(aWpt), diff --git a/src/Navaids/route.hxx b/src/Navaids/route.hxx index f6587dc50..283456f5d 100644 --- a/src/Navaids/route.hxx +++ b/src/Navaids/route.hxx @@ -78,7 +78,8 @@ typedef enum { /// waypoint prodcued by expanding a VIA segment WPT_VIA = 1 << 12, - // waypoint should be hidden from maps, etc + /// waypoint should not be shown in UI displays, etc + /// this is used to implement FMSs which delete waypoints after passing them WPT_HIDDEN = 1 << 13 } WayptFlag;