fly/flightgear
1
0
Fork 0
Code Activity

GPS fly-by support

Browse source 
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.
This commit is contained in:
James Turner 2020-05-30 15:55:30 +01:00
parent e8bf4220a8
commit 6bb1478067
5 changed files with 325 additions and 249 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

244
src/Instrumentation/gps.cxx
View file

@ -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<double>(&_turnRate));
aOwner->tie(aCfg, "turn-anticipation", SGRawValuePointer<bool>(&_enableTurnAnticipation));
aOwner->tie(aCfg, "enable-fly-by", SGRawValuePointer<bool>(&_enableTurnAnticipation));
aOwner->tie(aCfg, "wpt-alert-time", SGRawValuePointer<double>(&_waypointAlertTime));
aOwner->tie(aCfg, "hard-surface-runways-only", SGRawValuePointer<bool>(&_requireHardSurface));
aOwner->tie(aCfg, "cdi-max-deflection-nm", SGRawValuePointer<double>(&_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<GPS>(this,&GPS::routeManagerFlightPlanChanged,
fgGetNode("/autopilot/route-manager/signals/flightplan-changed", true))),
_callbackRouteActivated(SGPropertyChangeCallback<GPS>(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<double> 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<RunwayWaypt*>(waypt);
return rwpt->runway()->end();
}
return waypt->position();
}
}
isValid = false;
return SGGeod();
auto next = _route->nextLeg();
if (!next)
return {};
return next->courseDeg();
}
simgear::optional<RNAV::LegData> 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<RunwayWaypt*>(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.

48
src/Instrumentation/gps.hxx
View file

@ -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<LegData> previousLegData() override;
simgear::optional<double> 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<RNAV::LegData> _wp0Data;
std::unique_ptr<flightgear::WayptController> _wayptController;
flightgear::WayptRef _prevWaypt;

236
src/Instrumentation/rnav_waypt_controller.cxx
View file

@ -28,6 +28,8 @@
#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
@ -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<RNAV::LegData> 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;
};
/**

43
src/Instrumentation/rnav_waypt_controller.hxx
View file

@ -21,6 +21,7 @@
#define FG_WAYPT_CONTROLLER_HXX
#include <Navaids/waypoint.hxx>
#include <simgear/misc/simgear_optional.hxx>
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<LegData> previousLegData()
{
return simgear::optional<LegData>();
}
virtual simgear::optional<double> nextLegTrack()
{
return simgear::optional<double>{};
}
/**
* @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<RNAV::LegData> legData() const
{
// defer to our subcontroller if it exists
if (_subController)
return _subController->legData();
return simgear::optional<RNAV::LegData>();
}
/**
* 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),

3
src/Navaids/route.hxx
View file

@ -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;