RNAV / GPS hold support
The GPS/RNAV system can now fly holds, both left and right-handed. The correct entry is selected, and hold count can be selected. Also there's lots of lovely test cases.
This commit is contained in:
James Turner
parent
15c101efe4
commit
5ce8a942bf
11 changed files with 1656 additions and 73 deletions
|
|
@ -330,6 +330,8 @@ GPS::update (double delta_time_sec)
|
|||
|
||||
_desiredCourse = getLegMagCourse();
|
||||
|
||||
_gpsNode->setStringValue("rnav-controller-status", _wayptController->status());
|
||||
|
||||
updateTurn();
|
||||
updateRouteData();
|
||||
}
|
||||
|
|
@ -877,6 +879,12 @@ void GPS::wp1Changed()
|
|||
return;
|
||||
if (_mode == "leg") {
|
||||
_wayptController.reset(WayptController::createForWaypt(this, _currentWaypt));
|
||||
if (_currentWaypt->type() == "hold") {
|
||||
// pass the hold count through
|
||||
auto leg = _route->currentLeg();
|
||||
auto holdCtl = static_cast<flightgear::HoldCtl*>(_wayptController.get());
|
||||
holdCtl->setHoldCount(leg->holdCount());
|
||||
}
|
||||
} else if (_mode == "obs") {
|
||||
_wayptController.reset(new OBSController(this, _currentWaypt));
|
||||
} else if (_mode == "dto") {
|
||||
|
|
@ -884,11 +892,14 @@ void GPS::wp1Changed()
|
|||
}
|
||||
|
||||
_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
|
||||
|
|
@ -1158,7 +1169,9 @@ void GPS::setCommand(const char* aCmd)
|
|||
// use the current waypoint if one exists
|
||||
selectOBSMode(isScratchPositionValid() ? nullptr : _currentWaypt);
|
||||
} else if (!strcmp(aCmd, "leg")) {
|
||||
selectLegMode();
|
||||
selectLegMode();
|
||||
} else if (!strcmp(aCmd, "exit-hold")) {
|
||||
commandExitHold();
|
||||
#if FG_210_COMPAT
|
||||
} else if (!strcmp(aCmd, "load-route-wpt")) {
|
||||
loadRouteWaypoint();
|
||||
|
|
@ -1641,6 +1654,16 @@ void GPS::tieSGGeodReadOnly(SGPropertyNode* aNode, SGGeod& aRef,
|
|||
}
|
||||
}
|
||||
|
||||
void GPS::commandExitHold()
|
||||
{
|
||||
if (_currentWaypt && (_currentWaypt->type() == "hold")) {
|
||||
auto holdCtl = static_cast<flightgear::HoldCtl*>(_wayptController.get());
|
||||
holdCtl->exitHold();
|
||||
} else {
|
||||
SG_LOG(SG_INSTR, SG_WARN, "GPS:exit hold requested, but not currently in a hold");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Register the subsystem.
|
||||
#if 0
|
||||
|
|
|
|||
|
|
@ -251,7 +251,8 @@ private:
|
|||
void defineWaypoint();
|
||||
void insertWaypointAtIndex(int aIndex);
|
||||
void removeWaypointAtIndex(int aIndex);
|
||||
|
||||
void commandExitHold();
|
||||
|
||||
// tied-property getter/setters
|
||||
double getScratchDistance() const;
|
||||
double getScratchMagBearing() const;
|
||||
|
|
|
|||
|
|
@ -71,6 +71,15 @@ WayptController::~WayptController()
|
|||
void WayptController::init()
|
||||
{
|
||||
}
|
||||
|
||||
bool WayptController::isDone() const
|
||||
{
|
||||
if (_subController) {
|
||||
return _subController->isDone();
|
||||
}
|
||||
|
||||
return _isDone;
|
||||
}
|
||||
|
||||
void WayptController::setDone()
|
||||
{
|
||||
|
|
@ -92,6 +101,53 @@ double WayptController::timeToWaypt() const
|
|||
return (distanceToWayptM() / gs);
|
||||
}
|
||||
|
||||
std::string WayptController::status() const
|
||||
{
|
||||
return {};
|
||||
}
|
||||
|
||||
void WayptController::setSubController(WayptController* sub)
|
||||
{
|
||||
_subController.reset(sub);
|
||||
if (_subController) {
|
||||
// seems like a good idea to check this
|
||||
assert(_subController->_rnav == _rnav);
|
||||
_subController->init();
|
||||
}
|
||||
}
|
||||
|
||||
double WayptController::trueBearingDeg() const
|
||||
{
|
||||
if (_subController)
|
||||
return _subController->trueBearingDeg();
|
||||
|
||||
return _targetTrack;
|
||||
}
|
||||
|
||||
double WayptController::targetTrackDeg() const
|
||||
{
|
||||
if (_subController)
|
||||
return _subController->targetTrackDeg();
|
||||
|
||||
return _targetTrack;
|
||||
}
|
||||
|
||||
double WayptController::xtrackErrorNm() const
|
||||
{
|
||||
if (_subController)
|
||||
return _subController->xtrackErrorNm();
|
||||
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
double WayptController::courseDeviationDeg() const
|
||||
{
|
||||
if (_subController)
|
||||
return _subController->courseDeviationDeg();
|
||||
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
//////////////
|
||||
|
||||
class BasicWayptCtl : public WayptController
|
||||
|
|
@ -624,62 +680,346 @@ private:
|
|||
double _distanceNm;
|
||||
};
|
||||
|
||||
class HoldCtl : public WayptController
|
||||
|
||||
HoldCtl::HoldCtl(RNAV* aRNAV, const WayptRef& aWpt) :
|
||||
WayptController(aRNAV, aWpt)
|
||||
|
||||
{
|
||||
public:
|
||||
HoldCtl(RNAV* aRNAV, const WayptRef& aWpt) :
|
||||
WayptController(aRNAV, aWpt)
|
||||
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
virtual void init()
|
||||
{
|
||||
}
|
||||
|
||||
virtual void update(double)
|
||||
{
|
||||
// fly inbound / outbound sides, or execute the turn
|
||||
#if 0
|
||||
if (inTurn) {
|
||||
|
||||
targetTrack += dt * turnRateSec * turnDirection;
|
||||
if (inbound) {
|
||||
if .. targetTrack has passed inbound radial, doen with this turn
|
||||
} else {
|
||||
if target track has passed reciprocal radial done with turn
|
||||
}
|
||||
} else {
|
||||
check time / distance elapsed
|
||||
|
||||
if (sideDone) {
|
||||
inTurn = true;
|
||||
inbound = !inbound;
|
||||
nextHeading = inbound;
|
||||
if (!inbound) {
|
||||
nextHeading += 180.0;
|
||||
SG_NORMALIZE_RANGE(nextHeading, 0.0, 360.0);
|
||||
}
|
||||
}
|
||||
|
||||
// find published hold for aWpt
|
||||
// do we have this?!
|
||||
if (aWpt->type() == "hold") {
|
||||
const auto hold = static_cast<Hold*>(aWpt.ptr());
|
||||
_holdCourse = hold->inboundRadial();
|
||||
if (hold->isDistance())
|
||||
_holdLegDistance = hold->timeOrDistance();
|
||||
else
|
||||
_holdLegTime = hold->timeOrDistance();
|
||||
_leftHandTurns = hold->isLeftHanded();
|
||||
}
|
||||
}
|
||||
|
||||
void HoldCtl::init()
|
||||
{
|
||||
_segmentEnd = _waypt->position();
|
||||
_state = LEG_TO_HOLD;
|
||||
|
||||
#endif
|
||||
// use leg controller to fly us to the hold point - this also gives
|
||||
// the normal legl behaviour if the hold is not enabled
|
||||
setSubController(new LegWayptCtl(_rnav, _waypt));
|
||||
}
|
||||
|
||||
void HoldCtl::computeEntry()
|
||||
{
|
||||
const double entryCourse = SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
|
||||
const double diff = SGMiscd::normalizePeriodic( -180.0, 180.0, _holdCourse - entryCourse);
|
||||
|
||||
if (_leftHandTurns) {
|
||||
if ((diff > -70) && (diff < 120.0)) {
|
||||
_state = ENTRY_DIRECT;
|
||||
} else if (diff < 0.0) {
|
||||
_state = ENTRY_PARALLEL;
|
||||
} else {
|
||||
_state = ENTRY_TEARDROP;
|
||||
}
|
||||
} else {
|
||||
// right handed entry angles
|
||||
if ((diff > -120) && (diff < 70.0)) {
|
||||
_state = ENTRY_DIRECT;
|
||||
} else if (diff > 0.0) {
|
||||
_state = ENTRY_PARALLEL;
|
||||
} else {
|
||||
_state = ENTRY_TEARDROP;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void HoldCtl::update(double dt)
|
||||
{
|
||||
const auto rnavPos = _rnav->position();
|
||||
const double dEnd = SGGeodesy::distanceNm(rnavPos, _segmentEnd);
|
||||
|
||||
// fly inbound / outbound sides, or execute the turn
|
||||
switch (_state) {
|
||||
case LEG_TO_HOLD:
|
||||
// update the leg controller
|
||||
_subController->update(dt);
|
||||
break;
|
||||
|
||||
case HOLD_EXITING:
|
||||
// in the common case of a hold in a procedure, we often just fly
|
||||
// the hold waypoint as leg. Keep running the Leg sub-controller until
|
||||
// it's done (WayptController::isDone calls the subcController
|
||||
// automaticlaly)
|
||||
if (_subController) {
|
||||
_subController->update(dt);
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
if (_inTurn) {
|
||||
const double turnOffset = inLeftTurn() ? 90 : -90;
|
||||
const double bearingTurnCenter = SGGeodesy::courseDeg(rnavPos, _turnCenter);
|
||||
_targetTrack = bearingTurnCenter + turnOffset;
|
||||
SG_NORMALIZE_RANGE(_targetTrack, 0.0, 360.0);
|
||||
|
||||
const double angleToTurn = SGMiscd::normalizePeriodic( -180.0, 180.0, _targetTrack - _turnEndAngle);
|
||||
if (fabs(angleToTurn) < 0.5) {
|
||||
exitTurn();
|
||||
}
|
||||
} else if (_state == LEG_TO_HOLD) {
|
||||
checkInitialEntry(dEnd);
|
||||
} else if ((_state == HOLD_INBOUND) || (_state == ENTRY_PARALLEL_INBOUND)) {
|
||||
if (checkOverHold()) {
|
||||
// we are done
|
||||
} else {
|
||||
// straight line XTK/deviation
|
||||
// computed on-demand in xtrackErrorNm so nothing to do here
|
||||
}
|
||||
} else if ((_state == HOLD_OUTBOUND) || (_state == ENTRY_TEARDROP)) {
|
||||
if (dEnd < 0.2) {
|
||||
startInboundTurn();
|
||||
} else {
|
||||
// straight line XTK
|
||||
}
|
||||
} else if (_state == ENTRY_PARALLEL_OUTBOUND) {
|
||||
if (dEnd < 0.2) {
|
||||
startParallelEntryTurn(); // opposite direction to normal
|
||||
} else {
|
||||
// straight line XTK
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void HoldCtl::setHoldCount(int count)
|
||||
{
|
||||
_holdCount = count;
|
||||
}
|
||||
|
||||
void HoldCtl::exitHold()
|
||||
{
|
||||
_holdCount = 0;
|
||||
}
|
||||
|
||||
bool HoldCtl::checkOverHold()
|
||||
{
|
||||
// check distance to entry fix
|
||||
const double d = SGGeodesy::distanceNm(_rnav->position(), _waypt->position());
|
||||
if (d > 0.2) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (_holdCount == 0) {
|
||||
setDone();
|
||||
return true;
|
||||
}
|
||||
|
||||
startOutboundTurn();
|
||||
return true;
|
||||
}
|
||||
|
||||
void HoldCtl::checkInitialEntry(double dNm)
|
||||
{
|
||||
_turnRadius = _rnav->turnRadiusNm();
|
||||
if (dNm > _turnRadius) {
|
||||
// keep going;
|
||||
return;
|
||||
}
|
||||
|
||||
virtual double distanceToWayptM() const
|
||||
if (_holdCount == 0) {
|
||||
// we're done, but we want to keep the leg controller going until
|
||||
// we're right on top
|
||||
setDone();
|
||||
|
||||
// ensure we keep running the Leg cub-controller until it's done,
|
||||
// which happens a bit later
|
||||
_state = HOLD_EXITING;
|
||||
return;
|
||||
}
|
||||
|
||||
// clear the leg controller we were using to fly us to the hold
|
||||
setSubController(nullptr);
|
||||
computeEntry();
|
||||
|
||||
if (_state == ENTRY_DIRECT) {
|
||||
startOutboundTurn();
|
||||
} else if (_state == ENTRY_TEARDROP) {
|
||||
_targetTrack = _holdCourse + (_leftHandTurns ? -150 : 150);
|
||||
SG_NORMALIZE_RANGE(_targetTrack, 0.0, 360.0);
|
||||
_segmentEnd = outboundEndPoint();
|
||||
} else {
|
||||
// parallel entry
|
||||
assert(_state == ENTRY_PARALLEL);
|
||||
_state = ENTRY_PARALLEL_OUTBOUND;
|
||||
_targetTrack = _holdCourse + 180;
|
||||
SG_NORMALIZE_RANGE(_targetTrack, 0.0, 360.0);
|
||||
const double legLengthM = holdLegLengthNm() * SG_NM_TO_METER;
|
||||
_segmentEnd = SGGeodesy::direct(_waypt->position(), _holdCourse, -legLengthM);
|
||||
}
|
||||
}
|
||||
|
||||
void HoldCtl::startInboundTurn()
|
||||
{
|
||||
_state = HOLD_INBOUND;
|
||||
_inTurn = true;
|
||||
_turnCenter = inboundTurnCenter();
|
||||
_turnRadius = _rnav->turnRadiusNm();
|
||||
_turnEndAngle = _holdCourse;
|
||||
SG_NORMALIZE_RANGE(_turnEndAngle, 0.0, 360.0);
|
||||
}
|
||||
|
||||
void HoldCtl::startOutboundTurn()
|
||||
{
|
||||
_state = HOLD_OUTBOUND;
|
||||
_inTurn = true;
|
||||
_turnCenter = outboundTurnCenter();
|
||||
_turnRadius = _rnav->turnRadiusNm();
|
||||
_turnEndAngle = _holdCourse + 180.0;
|
||||
SG_NORMALIZE_RANGE(_turnEndAngle, 0.0, 360.0);
|
||||
}
|
||||
|
||||
void HoldCtl::startParallelEntryTurn()
|
||||
{
|
||||
_state = ENTRY_PARALLEL_INBOUND;
|
||||
_inTurn = true;
|
||||
_turnRadius = _rnav->turnRadiusNm();
|
||||
_turnCenter = inboundTurnCenter();
|
||||
_turnEndAngle = _holdCourse + (_leftHandTurns ? 45.0 : -45.0);
|
||||
SG_NORMALIZE_RANGE(_turnEndAngle, 0.0, 360.0);
|
||||
}
|
||||
|
||||
void HoldCtl::exitTurn()
|
||||
{
|
||||
_inTurn = false;
|
||||
switch (_state) {
|
||||
case HOLD_INBOUND:
|
||||
_targetTrack = _holdCourse;
|
||||
_segmentEnd = _waypt->position();
|
||||
break;
|
||||
|
||||
case ENTRY_PARALLEL_INBOUND:
|
||||
// possible improvement : fly the current track until the bearing tp
|
||||
// the hold point matches the hold radial. This would cause us to fly
|
||||
// a neater parallel entry, rather than flying to the hold point
|
||||
// direct from where we are now.
|
||||
_targetTrack = SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
|
||||
_segmentEnd = _waypt->position();
|
||||
break;
|
||||
|
||||
case HOLD_OUTBOUND:
|
||||
_targetTrack = _holdCourse + 180.0;
|
||||
SG_NORMALIZE_RANGE(_targetTrack, 0.0, 360.0);
|
||||
// start a timer for timed holds?
|
||||
_segmentEnd = outboundEndPoint();
|
||||
break;
|
||||
|
||||
default:
|
||||
SG_LOG(SG_INSTR, SG_DEV_WARN, "HoldCOntroller: bad state at exitTurn:" << _state);
|
||||
}
|
||||
}
|
||||
|
||||
SGGeod HoldCtl::outboundEndPoint()
|
||||
{
|
||||
// FIXME flip for left hand-turns
|
||||
const double turnRadiusM = _rnav->turnRadiusNm() * SG_NM_TO_METER;
|
||||
const double legLengthM = holdLegLengthNm() * SG_NM_TO_METER;
|
||||
const auto p1 = SGGeodesy::direct(_waypt->position(), _holdCourse, -legLengthM);
|
||||
const double turnOffset = _leftHandTurns ? -90 : 90;
|
||||
return SGGeodesy::direct(p1, _holdCourse + turnOffset, turnRadiusM * 2.0);
|
||||
}
|
||||
|
||||
SGGeod HoldCtl::outboundTurnCenter()
|
||||
{
|
||||
const double turnOffset = _leftHandTurns ? -90 : 90;
|
||||
return SGGeodesy::direct(_waypt->position(), _holdCourse + turnOffset, _rnav->turnRadiusNm() * SG_NM_TO_METER);
|
||||
}
|
||||
|
||||
SGGeod HoldCtl::inboundTurnCenter()
|
||||
{
|
||||
const double legLengthM = holdLegLengthNm() * SG_NM_TO_METER;
|
||||
const auto p1 = SGGeodesy::direct(_waypt->position(), _holdCourse, -legLengthM);
|
||||
const double turnOffset = _leftHandTurns ? -90 : 90;
|
||||
return SGGeodesy::direct(p1, _holdCourse + turnOffset, _rnav->turnRadiusNm() * SG_NM_TO_METER);
|
||||
}
|
||||
|
||||
double HoldCtl::distanceToWayptM() const
|
||||
{
|
||||
return -1.0;
|
||||
}
|
||||
|
||||
SGGeod HoldCtl::position() const
|
||||
{
|
||||
return _waypt->position();
|
||||
}
|
||||
|
||||
bool HoldCtl::inLeftTurn() const
|
||||
{
|
||||
return (_state == ENTRY_PARALLEL_INBOUND) ? !_leftHandTurns : _leftHandTurns;
|
||||
}
|
||||
|
||||
double HoldCtl::holdLegLengthNm() const
|
||||
{
|
||||
const double gs = _rnav->groundSpeedKts();
|
||||
if (_holdLegTime > 0.0) {
|
||||
return _holdLegTime * gs / 3600.0;
|
||||
}
|
||||
|
||||
return _holdLegDistance;
|
||||
}
|
||||
|
||||
double HoldCtl::xtrackErrorNm() const
|
||||
{
|
||||
if (_subController) {
|
||||
return _subController->xtrackErrorNm();
|
||||
}
|
||||
|
||||
if (_inTurn) {
|
||||
const double dR = SGGeodesy::distanceNm(_turnCenter, _rnav->position());
|
||||
const double xtk = dR - _turnRadius;
|
||||
return inLeftTurn() ? -xtk : xtk;
|
||||
} else {
|
||||
const double courseToSegmentEnd = SGGeodesy::courseDeg(_rnav->position(), _segmentEnd);
|
||||
const double courseDev = courseToSegmentEnd - _targetTrack;
|
||||
const auto d = SGGeodesy::distanceNm(_rnav->position(), _segmentEnd);
|
||||
return greatCircleCrossTrackError(d, courseDev);
|
||||
}
|
||||
}
|
||||
|
||||
double HoldCtl::courseDeviationDeg() const
|
||||
{
|
||||
if (_subController) {
|
||||
return _subController->courseDeviationDeg();
|
||||
}
|
||||
|
||||
// convert XTK to 'dots' deviation
|
||||
// assuming 10-degree peg to peg, this means 0.1nm of course is
|
||||
// one degree of error, feels about right for a hold
|
||||
return xtrackErrorNm() * 10.0;
|
||||
}
|
||||
|
||||
std::string HoldCtl::status() const
|
||||
{
|
||||
return -1.0;
|
||||
switch (_state) {
|
||||
case LEG_TO_HOLD: return "leg-to-hold";
|
||||
case HOLD_INIT: return "hold-init";
|
||||
case ENTRY_DIRECT: return "entry-direct";
|
||||
case ENTRY_PARALLEL:
|
||||
case ENTRY_PARALLEL_OUTBOUND:
|
||||
case ENTRY_PARALLEL_INBOUND:
|
||||
return "entry-parallel";
|
||||
case ENTRY_TEARDROP:
|
||||
return "entry-teardrop";
|
||||
|
||||
case HOLD_OUTBOUND: return "hold-outbound";
|
||||
case HOLD_INBOUND: return "hold-inbound";
|
||||
case HOLD_EXITING: return "hold-exiting";
|
||||
}
|
||||
}
|
||||
|
||||
virtual SGGeod position() const
|
||||
{
|
||||
return _waypt->position();
|
||||
}
|
||||
};
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
class VectorsCtl : public WayptController
|
||||
{
|
||||
public:
|
||||
|
|
|
|||
|
|
@ -116,18 +116,14 @@ public:
|
|||
* Bearing to the waypoint, if this value is meaningful.
|
||||
* Default implementation returns the target track
|
||||
*/
|
||||
virtual double trueBearingDeg() const
|
||||
{ return _targetTrack; }
|
||||
virtual double trueBearingDeg() const;
|
||||
|
||||
virtual double targetTrackDeg() const
|
||||
{ return _targetTrack; }
|
||||
|
||||
virtual double xtrackErrorNm() const
|
||||
{ return 0.0; }
|
||||
|
||||
virtual double courseDeviationDeg() const
|
||||
{ return 0.0; }
|
||||
virtual double targetTrackDeg() const;
|
||||
|
||||
virtual double xtrackErrorNm() const;
|
||||
|
||||
virtual double courseDeviationDeg() const;
|
||||
|
||||
/**
|
||||
* Position associated with the waypt. For static waypoints, this is
|
||||
* simply the waypoint position itself; for dynamic points, it's the
|
||||
|
|
@ -138,8 +134,7 @@ public:
|
|||
/**
|
||||
* Is this controller finished?
|
||||
*/
|
||||
bool isDone() const
|
||||
{ return _isDone; }
|
||||
bool isDone() const;
|
||||
|
||||
/**
|
||||
* to/from flag - true = to, false = from. Defaults to 'true' because
|
||||
|
|
@ -148,6 +143,13 @@ public:
|
|||
*/
|
||||
virtual bool toFlag() const
|
||||
{ return true; }
|
||||
|
||||
/**
|
||||
* Allow waypoints to indicate a status value as a string.
|
||||
* Useful for more complex controllers, which may have capture / exit
|
||||
* states
|
||||
*/
|
||||
virtual std::string status() const;
|
||||
|
||||
/**
|
||||
* Static factory method, given a waypoint, return a controller bound
|
||||
|
|
@ -167,6 +169,14 @@ protected:
|
|||
RNAV* _rnav;
|
||||
|
||||
void setDone();
|
||||
|
||||
// take asubcontroller ref (will be destroyed automatically)
|
||||
// pass nullptr to clear any activ esubcontroller
|
||||
// the subcontroller will be initialised
|
||||
void setSubController(WayptController* sub);
|
||||
|
||||
// if a subcontroller exists, we can delegate to it automatically
|
||||
std::unique_ptr<WayptController> _subController;
|
||||
private:
|
||||
bool _isDone;
|
||||
};
|
||||
|
|
@ -219,6 +229,71 @@ private:
|
|||
double _courseAircraftToTarget;
|
||||
};
|
||||
|
||||
class HoldCtl : public WayptController
|
||||
{
|
||||
enum HoldState {
|
||||
HOLD_INIT,
|
||||
LEG_TO_HOLD,
|
||||
ENTRY_DIRECT,
|
||||
ENTRY_PARALLEL, // flying the outbound part of a parallel entry
|
||||
ENTRY_PARALLEL_OUTBOUND,
|
||||
ENTRY_PARALLEL_INBOUND,
|
||||
ENTRY_TEARDROP, // flying the outbound leg of teardrop entry
|
||||
HOLD_OUTBOUND,
|
||||
HOLD_INBOUND,
|
||||
HOLD_EXITING, // we are going to exit the hold
|
||||
};
|
||||
|
||||
HoldState _state = HOLD_INIT;
|
||||
double _holdCourse = 0.0;
|
||||
double _holdLegTime = 60.0;
|
||||
double _holdLegDistance = 0.0;
|
||||
double _holdCount = 0;
|
||||
bool _leftHandTurns = false;
|
||||
|
||||
bool _inTurn = false;
|
||||
SGGeod _turnCenter;
|
||||
double _turnEndAngle, _turnRadius;
|
||||
SGGeod _segmentEnd;
|
||||
|
||||
bool checkOverHold();
|
||||
void checkInitialEntry(double dNm);
|
||||
|
||||
void startInboundTurn();
|
||||
void startOutboundTurn();
|
||||
void startParallelEntryTurn();
|
||||
void exitTurn();
|
||||
|
||||
SGGeod outboundEndPoint();
|
||||
SGGeod outboundTurnCenter();
|
||||
SGGeod inboundTurnCenter();
|
||||
|
||||
double holdLegLengthNm() const;
|
||||
|
||||
/**
|
||||
* are we turning left? This is basically the )leftHandTurns member,
|
||||
* but if we're in the inbound turn of a parallel entry, it's flipped
|
||||
*/
|
||||
bool inLeftTurn() const;
|
||||
|
||||
void computeEntry();
|
||||
public:
|
||||
HoldCtl(RNAV* aRNAV, const WayptRef& aWpt);
|
||||
|
||||
void setHoldCount(int count);
|
||||
void exitHold();
|
||||
|
||||
void init() override;
|
||||
void update(double) override;
|
||||
|
||||
double distanceToWayptM() const override;
|
||||
SGGeod position() const override;
|
||||
double xtrackErrorNm() const override;
|
||||
double courseDeviationDeg() const override;
|
||||
|
||||
std::string status() const override;
|
||||
};
|
||||
|
||||
} // of namespace flightgear
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1576,6 +1576,51 @@ double FlightPlan::Leg::distanceAlongRoute() const
|
|||
return _distanceAlongPath;
|
||||
}
|
||||
|
||||
bool FlightPlan::Leg::setHoldCount(int count)
|
||||
{
|
||||
if (count == 0) {
|
||||
_holdCount = count;
|
||||
return true;
|
||||
}
|
||||
|
||||
const auto wty = _waypt->type();
|
||||
bool fireWaypointsChanged = false;
|
||||
|
||||
if (wty != "hold") {
|
||||
// upgrade to a hold if possible
|
||||
if ((wty == "basic") || (wty == "navaid")) {
|
||||
auto hold = new Hold(_waypt->position(), _waypt->ident(), const_cast<FlightPlan*>(_parent));
|
||||
|
||||
// default to a 1 minute hold with the radial being our arrival radial
|
||||
hold->setHoldTime(60.0);
|
||||
hold->setHoldRadial(_courseDeg);
|
||||
fireWaypointsChanged = true;
|
||||
_waypt = hold; // we drop our reference to the old waypoint
|
||||
} else {
|
||||
SG_LOG(SG_INSTR, SG_WARN, "setHoldCount: cannot convert waypt " << index() << " " << _waypt->ident() << " to a hold");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
_holdCount = count;
|
||||
|
||||
if (fireWaypointsChanged) {
|
||||
SG_LOG(SG_INSTR, SG_INFO, "setHoldCount: changed waypoint type, notifying deleagtes the FP changed");
|
||||
|
||||
auto fp = owner();
|
||||
fp->lockDelegates();
|
||||
fp->_waypointsChanged = true;
|
||||
fp->unlockDelegates();
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
int FlightPlan::Leg::holdCount() const
|
||||
{
|
||||
return _holdCount;
|
||||
}
|
||||
|
||||
void FlightPlan::rebuildLegData()
|
||||
{
|
||||
_totalDistance = 0.0;
|
||||
|
|
|
|||
|
|
@ -92,6 +92,18 @@ public:
|
|||
// return the next leg after this one
|
||||
Leg* nextLeg() const;
|
||||
|
||||
/**
|
||||
* requesting holding at the waypoint upon reaching it. This will
|
||||
* convert the waypt to a Hold if not already defined as one, but
|
||||
* with default hold data.
|
||||
*
|
||||
* If the waypt is not of a type suitable for holding at, returns false
|
||||
* (eg a runway or dynamic waypoint)
|
||||
*/
|
||||
bool setHoldCount(int count);
|
||||
|
||||
int holdCount() const;
|
||||
|
||||
unsigned int index() const;
|
||||
|
||||
int altitudeFt() const;
|
||||
|
|
@ -123,6 +135,12 @@ public:
|
|||
int _speed = 0;
|
||||
int _altitudeFt = 0;
|
||||
|
||||
// if > 0, we will hold at the waypoint using
|
||||
// the published hold side/course
|
||||
// This only works if _waypt is a Hold, either defined by a procedure
|
||||
// or modified to become one
|
||||
int _holdCount = 0;
|
||||
|
||||
WayptRef _waypt;
|
||||
/// length of this leg following the flown path
|
||||
mutable double _pathDistance = -1.0;
|
||||
|
|
|
|||
|
|
@ -272,11 +272,6 @@ public:
|
|||
} else {
|
||||
pos = wpt->position();
|
||||
posValid = true;
|
||||
|
||||
if (ty == "hold") {
|
||||
legCourseTrue = wpt->headingRadialDeg() + magVarFor(pos);
|
||||
legCourseValid = true;
|
||||
}
|
||||
} // of static waypt
|
||||
}
|
||||
|
||||
|
|
@ -328,9 +323,7 @@ public:
|
|||
return;
|
||||
}
|
||||
|
||||
if ((wpt->type() == "hold") ||
|
||||
(wpt->type() == "discontinuity") ||
|
||||
(wpt->type() == "via"))
|
||||
if ((wpt->type() == "discontinuity") || (wpt->type() == "via"))
|
||||
{
|
||||
// do nothing, we can't compute a valid leg course for these types
|
||||
// we'll generate sharp turns in the path but that's no problem.
|
||||
|
|
@ -941,10 +934,6 @@ SGGeodVec RoutePath::pathForIndex(int index) const
|
|||
return pathForVia(static_cast<Via*>(d->waypoints[index].wpt.get()), index);
|
||||
}
|
||||
|
||||
if (ty== "hold") {
|
||||
return pathForHold((Hold*) d->waypoints[index].wpt.get());
|
||||
}
|
||||
|
||||
auto prevIt = d->previousValidWaypoint(index);
|
||||
if (prevIt != d->waypoints.end()) {
|
||||
prevIt->turnExitPath(r);
|
||||
|
|
@ -961,6 +950,13 @@ SGGeodVec RoutePath::pathForIndex(int index) const
|
|||
} // of have previous waypoint
|
||||
|
||||
w.turnEntryPath(r);
|
||||
|
||||
// hold is the normal leg and then the hold waypoints as well
|
||||
if (ty== "hold") {
|
||||
const auto h = static_cast<Hold*>(d->waypoints[index].wpt.get());
|
||||
const auto holdPath = pathForHold(h);
|
||||
r.insert(r.end(), holdPath.begin(), holdPath.end());
|
||||
}
|
||||
|
||||
if (ty == "runway") {
|
||||
// runways get an extra point, at the end. this is particularly
|
||||
|
|
|
|||
|
|
@ -3,6 +3,7 @@ set(TESTSUITE_SOURCES
|
|||
${CMAKE_CURRENT_SOURCE_DIR}/TestSuite.cxx
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/test_navRadio.cxx
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/test_gps.cxx
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/test_hold_controller.cxx
|
||||
PARENT_SCOPE
|
||||
)
|
||||
|
||||
|
|
@ -10,5 +11,6 @@ set(TESTSUITE_HEADERS
|
|||
${TESTSUITE_HEADERS}
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/test_navRadio.hxx
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/test_gps.hxx
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/test_hold_controller.hxx
|
||||
PARENT_SCOPE
|
||||
)
|
||||
|
|
|
|||
|
|
@ -19,9 +19,11 @@
|
|||
|
||||
#include "test_navRadio.hxx"
|
||||
#include "test_gps.hxx"
|
||||
#include "test_hold_controller.hxx"
|
||||
|
||||
// Set up the unit tests.
|
||||
CPPUNIT_TEST_SUITE_NAMED_REGISTRATION(NavRadioTests, "Unit tests");
|
||||
CPPUNIT_TEST_SUITE_NAMED_REGISTRATION(GPSTests, "Unit tests");
|
||||
CPPUNIT_TEST_SUITE_NAMED_REGISTRATION(HoldControllerTests, "Unit tests");
|
||||
|
||||
|
||||
|
|
|
|||
1003
test_suite/unit_tests/Instrumentation/test_hold_controller.cxx
Normal file
1003
test_suite/unit_tests/Instrumentation/test_hold_controller.cxx
Normal file
File diff suppressed because it is too large
Load diff
|
|
@ -0,0 +1,78 @@
|
|||
/*
|
||||
* Copyright (C) 2019 James Turner
|
||||
*
|
||||
* This file is part of the program FlightGear.
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 2 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef _FG_HOLD_CTL_UNIT_TESTS_HXX
|
||||
#define _FG_HOLD_CTL_UNIT_TESTS_HXX
|
||||
|
||||
|
||||
#include <cppunit/extensions/HelperMacros.h>
|
||||
#include <cppunit/TestFixture.h>
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include <simgear/props/props.hxx>
|
||||
|
||||
class SGGeod;
|
||||
class GPS;
|
||||
|
||||
// The flight plan unit tests.
|
||||
class HoldControllerTests : public CppUnit::TestFixture
|
||||
{
|
||||
// Set up the test suite.
|
||||
CPPUNIT_TEST_SUITE(HoldControllerTests);
|
||||
CPPUNIT_TEST(testHoldEntryDirect);
|
||||
CPPUNIT_TEST(testHoldEntryTeardrop);
|
||||
CPPUNIT_TEST(testHoldEntryParallel);
|
||||
CPPUNIT_TEST(testLeftHoldEntryDirect);
|
||||
CPPUNIT_TEST(testLeftHoldEntryTeardrop);
|
||||
CPPUNIT_TEST(testLeftHoldEntryParallel);
|
||||
CPPUNIT_TEST(testHoldNotEntered);
|
||||
CPPUNIT_TEST(testHoldEntryOffCourse);
|
||||
|
||||
CPPUNIT_TEST_SUITE_END();
|
||||
|
||||
void setPositionAndStabilise(const SGGeod& g);
|
||||
|
||||
GPS* setupStandardGPS(SGPropertyNode_ptr config = {},
|
||||
const std::string name = "gps", const int index = 0);
|
||||
void setupRouteManager();
|
||||
|
||||
public:
|
||||
// Set up function for each test.
|
||||
void setUp();
|
||||
|
||||
// Clean up after each test.
|
||||
void tearDown();
|
||||
|
||||
// The tests.
|
||||
void testHoldEntryDirect();
|
||||
void testHoldEntryTeardrop();
|
||||
void testHoldEntryParallel();
|
||||
void testLeftHoldEntryDirect();
|
||||
void testLeftHoldEntryTeardrop();
|
||||
void testLeftHoldEntryParallel();
|
||||
void testHoldNotEntered();
|
||||
void testHoldEntryOffCourse();
|
||||
private:
|
||||
GPS* m_gps = nullptr;
|
||||
SGPropertyNode_ptr m_gpsNode;
|
||||
};
|
||||
|
||||
#endif // _FG_HOLD_CTL_UNIT_TESTS_HXX
|
||||
Loading…
Reference in a new issue