From 5ce8a942bf81831eeb808b1dbefafe255c814058 Mon Sep 17 00:00:00 2001 From: James Turner Date: Wed, 18 Sep 2019 16:13:10 +0100 Subject: [PATCH] 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. --- src/Instrumentation/gps.cxx | 25 +- src/Instrumentation/gps.hxx | 3 +- src/Instrumentation/rnav_waypt_controller.cxx | 434 ++++++- src/Instrumentation/rnav_waypt_controller.hxx | 99 +- src/Navaids/FlightPlan.cxx | 45 + src/Navaids/FlightPlan.hxx | 18 + src/Navaids/routePath.cxx | 20 +- .../unit_tests/Instrumentation/CMakeLists.txt | 2 + .../unit_tests/Instrumentation/TestSuite.cxx | 2 + .../Instrumentation/test_hold_controller.cxx | 1003 +++++++++++++++++ .../Instrumentation/test_hold_controller.hxx | 78 ++ 11 files changed, 1656 insertions(+), 73 deletions(-) create mode 100644 test_suite/unit_tests/Instrumentation/test_hold_controller.cxx create mode 100644 test_suite/unit_tests/Instrumentation/test_hold_controller.hxx diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx index ec286d756..b30dfe448 100644 --- a/src/Instrumentation/gps.cxx +++ b/src/Instrumentation/gps.cxx @@ -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(_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(_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 diff --git a/src/Instrumentation/gps.hxx b/src/Instrumentation/gps.hxx index 1edf04f28..889f0bf45 100644 --- a/src/Instrumentation/gps.hxx +++ b/src/Instrumentation/gps.hxx @@ -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; diff --git a/src/Instrumentation/rnav_waypt_controller.cxx b/src/Instrumentation/rnav_waypt_controller.cxx index a5e471601..3e9891f92 100644 --- a/src/Instrumentation/rnav_waypt_controller.cxx +++ b/src/Instrumentation/rnav_waypt_controller.cxx @@ -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(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: diff --git a/src/Instrumentation/rnav_waypt_controller.hxx b/src/Instrumentation/rnav_waypt_controller.hxx index 41f8dc602..e71edaa20 100644 --- a/src/Instrumentation/rnav_waypt_controller.hxx +++ b/src/Instrumentation/rnav_waypt_controller.hxx @@ -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 _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 diff --git a/src/Navaids/FlightPlan.cxx b/src/Navaids/FlightPlan.cxx index 2c4ef1c75..9f27ffff8 100644 --- a/src/Navaids/FlightPlan.cxx +++ b/src/Navaids/FlightPlan.cxx @@ -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(_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; diff --git a/src/Navaids/FlightPlan.hxx b/src/Navaids/FlightPlan.hxx index 01b0a5cb7..7d9bd6696 100644 --- a/src/Navaids/FlightPlan.hxx +++ b/src/Navaids/FlightPlan.hxx @@ -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; diff --git a/src/Navaids/routePath.cxx b/src/Navaids/routePath.cxx index 0a68da87a..8aee6051e 100644 --- a/src/Navaids/routePath.cxx +++ b/src/Navaids/routePath.cxx @@ -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(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(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 diff --git a/test_suite/unit_tests/Instrumentation/CMakeLists.txt b/test_suite/unit_tests/Instrumentation/CMakeLists.txt index ee8078269..75e8d9884 100644 --- a/test_suite/unit_tests/Instrumentation/CMakeLists.txt +++ b/test_suite/unit_tests/Instrumentation/CMakeLists.txt @@ -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 ) diff --git a/test_suite/unit_tests/Instrumentation/TestSuite.cxx b/test_suite/unit_tests/Instrumentation/TestSuite.cxx index 8a77785df..476cb8bfc 100644 --- a/test_suite/unit_tests/Instrumentation/TestSuite.cxx +++ b/test_suite/unit_tests/Instrumentation/TestSuite.cxx @@ -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"); diff --git a/test_suite/unit_tests/Instrumentation/test_hold_controller.cxx b/test_suite/unit_tests/Instrumentation/test_hold_controller.cxx new file mode 100644 index 000000000..736df7cd3 --- /dev/null +++ b/test_suite/unit_tests/Instrumentation/test_hold_controller.cxx @@ -0,0 +1,1003 @@ +/* + * 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 . + */ + +#include "test_hold_controller.hxx" + +#include +#include + +#include "test_suite/FGTestApi/globals.hxx" +#include "test_suite/FGTestApi/NavDataCache.hxx" +#include "test_suite/FGTestApi/TestPilot.hxx" + +#include +#include +#include +#include + +#include +#include + +#include + +using namespace flightgear; + +///////////////////////////////////////////////////////////////////////////// + +class TestFPDelegate : public FlightPlan::Delegate +{ +public: + FlightPlanRef thePlan; + int sequenceCount = 0; + + void sequence() override + { + + ++sequenceCount; + int newIndex = thePlan->currentIndex() + 1; + if (newIndex >= thePlan->numLegs()) { + thePlan->finish(); + return; + } + + thePlan->setCurrentIndex(newIndex); + } + + void currentWaypointChanged() override + { + } + +}; + +///////////////////////////////////////////////////////////////////////////// + +// Set up function for each test. +void HoldControllerTests::setUp() +{ + FGTestApi::setUp::initTestGlobals("hold-ctl"); + FGTestApi::setUp::initNavDataCache(); + + setupRouteManager(); +} + +// Clean up after each test. +void HoldControllerTests::tearDown() +{ + FGTestApi::tearDown::shutdownTestGlobals(); +} + +void HoldControllerTests::setPositionAndStabilise(const SGGeod& g) +{ + FGTestApi::setPosition(g); + for (int i=0; i<60; ++i) { + m_gps->update(0.015); + } +} + +GPS* HoldControllerTests::setupStandardGPS(SGPropertyNode_ptr config, + const std::string name, const int index) +{ + SGPropertyNode_ptr configNode(config.valid() ? config + : SGPropertyNode_ptr{new SGPropertyNode}); + configNode->setStringValue("name", name); + configNode->setIntValue("number", index); + + GPS* gps(new GPS(configNode)); + m_gps = gps; + + m_gpsNode = globals->get_props()->getNode("instrumentation", true)->getChild(name, index, true); + m_gpsNode->setBoolValue("serviceable", true); + globals->get_props()->setDoubleValue("systems/electrical/outputs/gps", 6.0); + + gps->bind(); + gps->init(); + + globals->add_subsystem("gps", gps, SGSubsystemMgr::POST_FDM); + return gps; +} + +void HoldControllerTests::setupRouteManager() +{ + auto rm = globals->add_new_subsystem(); + rm->bind(); + rm->init(); + rm->postinit(); +} + +///////////////////////////////////////////////////////////////////////////// + +void HoldControllerTests::testHoldEntryDirect() +{ + FGTestApi::setUp::logPositionToKML("hold_direct_entry"); + + auto rm = globals->get_subsystem(); + auto fp = new FlightPlan; + rm->setFlightPlan(fp); + FGTestApi::setUp::populateFPWithoutNasal(fp, "EBBR", "07L", "EGGD", "27", + "NIK COA DVR TAWNY WOD"); + FGTestApi::writeFlightPlanToKML(fp); + + auto testDelegate = new TestFPDelegate; + testDelegate->thePlan = fp; + CPPUNIT_ASSERT(rm->activate()); + fp->addDelegate(testDelegate); + + setupStandardGPS(); + m_gpsNode->setStringValue("command", "leg"); + setPositionAndStabilise(fp->departureRunway()->pointOnCenterline(0.0)); + + CPPUNIT_ASSERT_EQUAL(std::string{"leg"}, std::string{m_gpsNode->getStringValue("mode")}); + CPPUNIT_ASSERT_EQUAL(std::string{"EBBR-07L"}, std::string{m_gpsNode->getStringValue("wp/wp[1]/ID")}); + + auto leg = fp->legAtIndex(3); // DVR vor + leg->setHoldCount(3); + CPPUNIT_ASSERT_EQUAL(std::string{"hold"}, leg->waypoint()->type()); + + // sequence to that waypoint + SGGeod nearDover = fp->pointAlongRoute(3, -6.0); + setPositionAndStabilise(nearDover); + + // check course deviation / cross-track error - we should still be at zero + auto doverVORPos = fp->legAtIndex(3)->waypoint()->position(); + const double crsToDover = SGGeodesy::courseDeg(globals->get_aircraft_position(), doverVORPos); + const double legCrs = SGGeodesy::courseDeg(nearDover, doverVORPos); + + // verify hold entry course + auto pilot = SGSharedPtr(new FGTestApi::TestPilot); + pilot->resetAtPosition(nearDover); + pilot->setCourseTrue(crsToDover); // initial course is correct + pilot->setSpeedKts(250); + pilot->flyGPSCourse(m_gps); + + // ensure GPS speed is plausible + FGTestApi::runForTime(1.0); + fp->setCurrentIndex(3); + + CPPUNIT_ASSERT_EQUAL(std::string{"COA"}, std::string{m_gpsNode->getStringValue("wp/wp[0]/ID")}); + CPPUNIT_ASSERT_EQUAL(std::string{"DVR"}, std::string{m_gpsNode->getStringValue("wp/wp[1]/ID")}); + + CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToDover, m_gpsNode->getDoubleValue("wp/wp[1]/bearing-true-deg"), 0.5); + CPPUNIT_ASSERT_DOUBLES_EQUAL(legCrs, m_gpsNode->getDoubleValue("wp/leg-true-course-deg"), 0.5); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.05); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5); + + auto statusNode = m_gpsNode->getNode("rnav-controller-status"); + CPPUNIT_ASSERT_EQUAL(std::string{"leg-to-hold"}, std::string{statusNode->getStringValue()}); + + bool ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "hold-outbound") return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + + // first outbound leg + ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "hold-inbound") return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + // check where we are! + + + ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "hold-outbound") return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + FGTestApi::runForTime(45.0); + + // reqeust hold exit + m_gpsNode->setStringValue("command", "exit-hold"); + + ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "hold-inbound") return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + + // and now we should exit the hold and sequence to TAWNY + + ok = FGTestApi::runForTimeWithCheck(600.0, [fp]() { + if (fp->currentIndex() == 4) return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + CPPUNIT_ASSERT_EQUAL(std::string{"leg"}, std::string{m_gpsNode->getStringValue("mode")}); + + // check we get back enroute okay + FGTestApi::runForTime(45.0); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.05); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5); +} + + +void HoldControllerTests::testHoldEntryTeardrop() +{ + FGTestApi::setUp::logPositionToKML("hold_teardrop_entry"); + + auto rm = globals->get_subsystem(); + auto fp = new FlightPlan; + rm->setFlightPlan(fp); + FGTestApi::setUp::populateFPWithoutNasal(fp, "EBBR", "07L", "EGGD", "27", + "NIK COA DVR TAWNY WOD"); + FGTestApi::writeFlightPlanToKML(fp); + + auto testDelegate = new TestFPDelegate; + testDelegate->thePlan = fp; + CPPUNIT_ASSERT(rm->activate()); + fp->addDelegate(testDelegate); + + setupStandardGPS(); + m_gpsNode->setStringValue("command", "leg"); + setPositionAndStabilise(fp->departureRunway()->pointOnCenterline(0.0)); + + CPPUNIT_ASSERT_EQUAL(std::string{"leg"}, std::string{m_gpsNode->getStringValue("mode")}); + CPPUNIT_ASSERT_EQUAL(std::string{"EBBR-07L"}, std::string{m_gpsNode->getStringValue("wp/wp[1]/ID")}); + + auto leg = fp->legAtIndex(3); // DVR vor + leg->setHoldCount(3); + CPPUNIT_ASSERT_EQUAL(std::string{"hold"}, leg->waypoint()->type()); + + flightgear::Hold* holdWpt = static_cast(leg->waypoint()); + holdWpt->setHoldRadial(120.0); + + // sequence to that waypoint + SGGeod nearDover = fp->pointAlongRoute(3, -8.0); + setPositionAndStabilise(nearDover); + + // check course deviation / cross-track error - we should still be at zero + auto doverVORPos = fp->legAtIndex(3)->waypoint()->position(); + const double crsToDover = SGGeodesy::courseDeg(globals->get_aircraft_position(), doverVORPos); + const double legCrs = SGGeodesy::courseDeg(nearDover, doverVORPos); + + // verify hold entry course + auto pilot = SGSharedPtr(new FGTestApi::TestPilot); + pilot->resetAtPosition(nearDover); + pilot->setCourseTrue(crsToDover); // initial course is correct + pilot->setSpeedKts(200); + pilot->flyGPSCourse(m_gps); + + // ensure GPS speed is plausible + FGTestApi::runForTime(5.0); + + // trigger entry to the hold + fp->setCurrentIndex(3); + + CPPUNIT_ASSERT_EQUAL(std::string{"DVR"}, std::string{m_gpsNode->getStringValue("wp/wp[1]/ID")}); + CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToDover, m_gpsNode->getDoubleValue("wp/wp[1]/bearing-true-deg"), 0.5); + CPPUNIT_ASSERT_DOUBLES_EQUAL(legCrs, m_gpsNode->getDoubleValue("wp/leg-true-course-deg"), 0.5); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.05); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5); + + + auto statusNode = m_gpsNode->getNode("rnav-controller-status"); + CPPUNIT_ASSERT_EQUAL(std::string{"leg-to-hold"}, std::string{statusNode->getStringValue()}); + + bool ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "entry-teardrop") return true; + return false; + }); + CPPUNIT_ASSERT(ok); + + ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "hold-inbound") return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + // check where we are + + ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "hold-outbound") return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + + // outbound leg + ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "hold-inbound") return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + FGTestApi::runForTime(80.0); // run inbound a bit + m_gpsNode->setStringValue("command", "exit-hold"); + + // check we exit ok + ok = FGTestApi::runForTimeWithCheck(600.0, [fp]() { + if (fp->currentIndex() == 4) return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + CPPUNIT_ASSERT_EQUAL(std::string{"leg"}, std::string{m_gpsNode->getStringValue("mode")}); + + // check we get back enroute okay - will take a long time since we're pointing totally + // the wrong way + FGTestApi::runForTime(240.0); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.05); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5); +} + + +void HoldControllerTests::testHoldEntryParallel() +{ + FGTestApi::setUp::logPositionToKML("hold_parallel_entry"); + + auto rm = globals->get_subsystem(); + auto fp = new FlightPlan; + rm->setFlightPlan(fp); + FGTestApi::setUp::populateFPWithoutNasal(fp, "EBBR", "07L", "EGGD", "27", + "NIK COA DVR TAWNY WOD"); + FGTestApi::writeFlightPlanToKML(fp); + + auto testDelegate = new TestFPDelegate; + testDelegate->thePlan = fp; + CPPUNIT_ASSERT(rm->activate()); + fp->addDelegate(testDelegate); + + setupStandardGPS(); + m_gpsNode->setStringValue("command", "leg"); + setPositionAndStabilise(fp->departureRunway()->pointOnCenterline(0.0)); + + CPPUNIT_ASSERT_EQUAL(std::string{"leg"}, std::string{m_gpsNode->getStringValue("mode")}); + CPPUNIT_ASSERT_EQUAL(std::string{"EBBR-07L"}, std::string{m_gpsNode->getStringValue("wp/wp[1]/ID")}); + + auto leg = fp->legAtIndex(3); // DVR vor + leg->setHoldCount(3); + CPPUNIT_ASSERT_EQUAL(std::string{"hold"}, leg->waypoint()->type()); + + flightgear::Hold* holdWpt = static_cast(leg->waypoint()); + holdWpt->setHoldRadial(45.0); + + // sequence to that waypoint + SGGeod nearDover = fp->pointAlongRoute(3, -5.0); + setPositionAndStabilise(nearDover); + + // trigger entry to the hold + fp->setCurrentIndex(3); + + CPPUNIT_ASSERT_EQUAL(std::string{"DVR"}, std::string{m_gpsNode->getStringValue("wp/wp[1]/ID")}); + + // check course deviation / cross-track error - we should still be at zero + auto doverVORPos = fp->legAtIndex(3)->waypoint()->position(); + const double crsToDover = SGGeodesy::courseDeg(globals->get_aircraft_position(), doverVORPos); + const double legCrs = SGGeodesy::courseDeg(nearDover, doverVORPos); + + CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToDover, m_gpsNode->getDoubleValue("wp/wp[1]/bearing-true-deg"), 0.5); + CPPUNIT_ASSERT_DOUBLES_EQUAL(legCrs, m_gpsNode->getDoubleValue("wp/leg-true-course-deg"), 0.5); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.05); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5); + + // verify hold entry course + auto pilot = SGSharedPtr(new FGTestApi::TestPilot); + pilot->resetAtPosition(nearDover); + pilot->setCourseTrue(crsToDover); // initial course is correct + pilot->setSpeedKts(250); + pilot->flyGPSCourse(m_gps); + + auto statusNode = m_gpsNode->getNode("rnav-controller-status"); + CPPUNIT_ASSERT_EQUAL(std::string{"leg-to-hold"}, std::string{statusNode->getStringValue()}); + + bool ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "entry-parallel") return true; + return false; + }); + CPPUNIT_ASSERT(ok); + + ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "hold-outbound") return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + // check where we are + + // outbound leg + ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "hold-inbound") return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + FGTestApi::runForTime(80.0); // run inbound a bit + m_gpsNode->setStringValue("command", "exit-hold"); + + // check we exit ok + ok = FGTestApi::runForTimeWithCheck(600.0, [fp]() { + if (fp->currentIndex() == 4) return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + CPPUNIT_ASSERT_EQUAL(std::string{"leg"}, std::string{m_gpsNode->getStringValue("mode")}); + + // check we get back enroute okay - will take a long time since we're pointing totally + // the wrong way + FGTestApi::runForTime(240.0); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.05); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5); +} + +///////////////////////////////////////////////////////////////////////////// + +void HoldControllerTests::testLeftHoldEntryDirect() +{ + FGTestApi::setUp::logPositionToKML("hold_left_direct_entry"); + + auto rm = globals->get_subsystem(); + auto fp = new FlightPlan; + rm->setFlightPlan(fp); + FGTestApi::setUp::populateFPWithoutNasal(fp, "EBBR", "07L", "EGGD", "27", + "NIK COA DVR TAWNY WOD"); + FGTestApi::writeFlightPlanToKML(fp); + + auto testDelegate = new TestFPDelegate; + testDelegate->thePlan = fp; + CPPUNIT_ASSERT(rm->activate()); + fp->addDelegate(testDelegate); + + setupStandardGPS(); + m_gpsNode->setStringValue("command", "leg"); + setPositionAndStabilise(fp->departureRunway()->pointOnCenterline(0.0)); + + CPPUNIT_ASSERT_EQUAL(std::string{"leg"}, std::string{m_gpsNode->getStringValue("mode")}); + CPPUNIT_ASSERT_EQUAL(std::string{"EBBR-07L"}, std::string{m_gpsNode->getStringValue("wp/wp[1]/ID")}); + + auto leg = fp->legAtIndex(3); // DVR vor + leg->setHoldCount(3); + CPPUNIT_ASSERT_EQUAL(std::string{"hold"}, leg->waypoint()->type()); + flightgear::Hold* holdWpt = static_cast(leg->waypoint()); + + // puts us near the 110-degree limit for a direct entry on a left-handed-hold + holdWpt->setHoldRadial(15.0); + holdWpt->setLeftHanded(); + + // sequence to that waypoint + SGGeod nearDover = fp->pointAlongRoute(3, -4.0); + setPositionAndStabilise(nearDover); + fp->setCurrentIndex(3); + + CPPUNIT_ASSERT_EQUAL(std::string{"COA"}, std::string{m_gpsNode->getStringValue("wp/wp[0]/ID")}); + CPPUNIT_ASSERT_EQUAL(std::string{"DVR"}, std::string{m_gpsNode->getStringValue("wp/wp[1]/ID")}); + + // check course deviation / cross-track error - we should still be at zero + auto doverVORPos = fp->legAtIndex(3)->waypoint()->position(); + const double crsToDover = SGGeodesy::courseDeg(globals->get_aircraft_position(), doverVORPos); + const double legCrs = SGGeodesy::courseDeg(nearDover, doverVORPos); + + CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToDover, m_gpsNode->getDoubleValue("wp/wp[1]/bearing-true-deg"), 0.5); + CPPUNIT_ASSERT_DOUBLES_EQUAL(legCrs, m_gpsNode->getDoubleValue("wp/leg-true-course-deg"), 0.5); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.05); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5); + + // verify hold entry course + auto pilot = SGSharedPtr(new FGTestApi::TestPilot); + pilot->resetAtPosition(nearDover); + pilot->setCourseTrue(crsToDover); // initial course is correct + pilot->setSpeedKts(300); + pilot->flyGPSCourse(m_gps); + + auto statusNode = m_gpsNode->getNode("rnav-controller-status"); + CPPUNIT_ASSERT_EQUAL(std::string{"leg-to-hold"}, std::string{statusNode->getStringValue()}); + + bool ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "hold-outbound") return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + + // first outbound leg + ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "hold-inbound") return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + // check where we are! + + + ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "hold-outbound") return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + FGTestApi::runForTime(45.0); + + // reqeust hold exit + m_gpsNode->setStringValue("command", "exit-hold"); + + ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "hold-inbound") return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + + // and now we should exit the hold and sequence to TAWNY + + ok = FGTestApi::runForTimeWithCheck(600.0, [fp]() { + if (fp->currentIndex() == 4) return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + CPPUNIT_ASSERT_EQUAL(std::string{"leg"}, std::string{m_gpsNode->getStringValue("mode")}); + + // check we get back enroute okay + FGTestApi::runForTime(120.0); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.05); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5); +} + +void HoldControllerTests::testLeftHoldEntryTeardrop() +{ + FGTestApi::setUp::logPositionToKML("hold_left_teardrop_entry"); + + auto rm = globals->get_subsystem(); + auto fp = new FlightPlan; + rm->setFlightPlan(fp); + FGTestApi::setUp::populateFPWithoutNasal(fp, "EBBR", "07L", "EGGD", "27", + "NIK COA DVR TAWNY WOD"); + FGTestApi::writeFlightPlanToKML(fp); + + auto testDelegate = new TestFPDelegate; + testDelegate->thePlan = fp; + CPPUNIT_ASSERT(rm->activate()); + fp->addDelegate(testDelegate); + + setupStandardGPS(); + m_gpsNode->setStringValue("command", "leg"); + setPositionAndStabilise(fp->departureRunway()->pointOnCenterline(0.0)); + + CPPUNIT_ASSERT_EQUAL(std::string{"leg"}, std::string{m_gpsNode->getStringValue("mode")}); + CPPUNIT_ASSERT_EQUAL(std::string{"EBBR-07L"}, std::string{m_gpsNode->getStringValue("wp/wp[1]/ID")}); + + auto leg = fp->legAtIndex(3); // DVR vor + leg->setHoldCount(3); + CPPUNIT_ASSERT_EQUAL(std::string{"hold"}, leg->waypoint()->type()); + + flightgear::Hold* holdWpt = static_cast(leg->waypoint()); + holdWpt->setHoldRadial(25.0); + holdWpt->setLeftHanded(); + + // sequence to that waypoint + SGGeod nearDover = fp->pointAlongRoute(3, -5.0); + setPositionAndStabilise(nearDover); + + // trigger entry to the hold + fp->setCurrentIndex(3); + + CPPUNIT_ASSERT_EQUAL(std::string{"DVR"}, std::string{m_gpsNode->getStringValue("wp/wp[1]/ID")}); + + // check course deviation / cross-track error - we should still be at zero + auto doverVORPos = fp->legAtIndex(3)->waypoint()->position(); + const double crsToDover = SGGeodesy::courseDeg(globals->get_aircraft_position(), doverVORPos); + const double legCrs = SGGeodesy::courseDeg(nearDover, doverVORPos); + + CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToDover, m_gpsNode->getDoubleValue("wp/wp[1]/bearing-true-deg"), 0.5); + CPPUNIT_ASSERT_DOUBLES_EQUAL(legCrs, m_gpsNode->getDoubleValue("wp/leg-true-course-deg"), 0.5); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.05); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5); + + // verify hold entry course + auto pilot = SGSharedPtr(new FGTestApi::TestPilot); + pilot->resetAtPosition(nearDover); + pilot->setCourseTrue(crsToDover); // initial course is correct + pilot->setSpeedKts(250); + pilot->flyGPSCourse(m_gps); + + auto statusNode = m_gpsNode->getNode("rnav-controller-status"); + CPPUNIT_ASSERT_EQUAL(std::string{"leg-to-hold"}, std::string{statusNode->getStringValue()}); + + bool ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "entry-teardrop") return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + + ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "hold-inbound") return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + // check where we are + + ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "hold-outbound") return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + + // outbound leg + ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "hold-inbound") return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + FGTestApi::runForTime(80.0); // run inbound a bit + m_gpsNode->setStringValue("command", "exit-hold"); + + // check we exit ok + ok = FGTestApi::runForTimeWithCheck(600.0, [fp]() { + if (fp->currentIndex() == 4) return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + CPPUNIT_ASSERT_EQUAL(std::string{"leg"}, std::string{m_gpsNode->getStringValue("mode")}); + + // check we get back enroute okay - will take a long time since we're pointing totally + // the wrong way + FGTestApi::runForTime(240.0); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.05); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5); +} + +void HoldControllerTests::testLeftHoldEntryParallel() +{ + FGTestApi::setUp::logPositionToKML("hold_left_parallel_entry"); + + auto rm = globals->get_subsystem(); + auto fp = new FlightPlan; + rm->setFlightPlan(fp); + FGTestApi::setUp::populateFPWithoutNasal(fp, "EBBR", "07L", "EGGD", "27", + "NIK COA DVR TAWNY WOD"); + FGTestApi::writeFlightPlanToKML(fp); + + auto testDelegate = new TestFPDelegate; + testDelegate->thePlan = fp; + CPPUNIT_ASSERT(rm->activate()); + fp->addDelegate(testDelegate); + + setupStandardGPS(); + m_gpsNode->setStringValue("command", "leg"); + setPositionAndStabilise(fp->departureRunway()->pointOnCenterline(0.0)); + + CPPUNIT_ASSERT_EQUAL(std::string{"leg"}, std::string{m_gpsNode->getStringValue("mode")}); + CPPUNIT_ASSERT_EQUAL(std::string{"EBBR-07L"}, std::string{m_gpsNode->getStringValue("wp/wp[1]/ID")}); + + auto leg = fp->legAtIndex(3); // DVR vor + leg->setHoldCount(3); + CPPUNIT_ASSERT_EQUAL(std::string{"hold"}, leg->waypoint()->type()); + + flightgear::Hold* holdWpt = static_cast(leg->waypoint()); + holdWpt->setHoldRadial(160); + holdWpt->setLeftHanded(); + + // sequence to that waypoint + SGGeod nearDover = fp->pointAlongRoute(3, -5.0); + setPositionAndStabilise(nearDover); + + // trigger entry to the hold + fp->setCurrentIndex(3); + + CPPUNIT_ASSERT_EQUAL(std::string{"DVR"}, std::string{m_gpsNode->getStringValue("wp/wp[1]/ID")}); + + // check course deviation / cross-track error - we should still be at zero + auto doverVORPos = fp->legAtIndex(3)->waypoint()->position(); + const double crsToDover = SGGeodesy::courseDeg(globals->get_aircraft_position(), doverVORPos); + const double legCrs = SGGeodesy::courseDeg(nearDover, doverVORPos); + + CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToDover, m_gpsNode->getDoubleValue("wp/wp[1]/bearing-true-deg"), 0.5); + CPPUNIT_ASSERT_DOUBLES_EQUAL(legCrs, m_gpsNode->getDoubleValue("wp/leg-true-course-deg"), 0.5); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.05); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5); + + // verify hold entry course + auto pilot = SGSharedPtr(new FGTestApi::TestPilot); + pilot->resetAtPosition(nearDover); + pilot->setCourseTrue(crsToDover); // initial course is correct + pilot->setSpeedKts(190); + pilot->flyGPSCourse(m_gps); + + auto statusNode = m_gpsNode->getNode("rnav-controller-status"); + CPPUNIT_ASSERT_EQUAL(std::string{"leg-to-hold"}, std::string{statusNode->getStringValue()}); + + bool ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "entry-parallel") return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + + ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "hold-outbound") return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + // check where we are + + // outbound leg + ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "hold-inbound") return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + FGTestApi::runForTime(80.0); // run inbound a bit + m_gpsNode->setStringValue("command", "exit-hold"); + + // check we exit ok + ok = FGTestApi::runForTimeWithCheck(600.0, [fp]() { + if (fp->currentIndex() == 4) return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + CPPUNIT_ASSERT_EQUAL(std::string{"leg"}, std::string{m_gpsNode->getStringValue("mode")}); + + // check we get back enroute okay - will take a long time since we're pointing totally + // the wrong way + FGTestApi::runForTime(240.0); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.05); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5); +} + +///////////////////////////////////////////////////////////////////////////// + +void HoldControllerTests::testHoldNotEntered() +{ + FGTestApi::setUp::logPositionToKML("hold_no_entry"); + + auto rm = globals->get_subsystem(); + auto fp = new FlightPlan; + rm->setFlightPlan(fp); + FGTestApi::setUp::populateFPWithoutNasal(fp, "EBBR", "07L", "EGGD", "27", + "NIK COA DVR TAWNY WOD"); + FGTestApi::writeFlightPlanToKML(fp); + + auto testDelegate = new TestFPDelegate; + testDelegate->thePlan = fp; + CPPUNIT_ASSERT(rm->activate()); + fp->addDelegate(testDelegate); + + setupStandardGPS(); + m_gpsNode->setStringValue("command", "leg"); + setPositionAndStabilise(fp->departureRunway()->pointOnCenterline(0.0)); + + CPPUNIT_ASSERT_EQUAL(std::string{"leg"}, std::string{m_gpsNode->getStringValue("mode")}); + CPPUNIT_ASSERT_EQUAL(std::string{"EBBR-07L"}, std::string{m_gpsNode->getStringValue("wp/wp[1]/ID")}); + + auto leg = fp->legAtIndex(3); // DVR vor + leg->setHoldCount(3); + CPPUNIT_ASSERT_EQUAL(std::string{"hold"}, leg->waypoint()->type()); + flightgear::Hold* holdWpt = static_cast(leg->waypoint()); + + holdWpt->setHoldRadial(15.0); + + // sequence to that waypoint + SGGeod nearDover = fp->pointAlongRoute(3, -4.0); + setPositionAndStabilise(nearDover); + fp->setCurrentIndex(3); + + CPPUNIT_ASSERT_EQUAL(std::string{"COA"}, std::string{m_gpsNode->getStringValue("wp/wp[0]/ID")}); + CPPUNIT_ASSERT_EQUAL(std::string{"DVR"}, std::string{m_gpsNode->getStringValue("wp/wp[1]/ID")}); + + // check course deviation / cross-track error - we should still be at zero + auto doverVORPos = fp->legAtIndex(3)->waypoint()->position(); + const double crsToDover = SGGeodesy::courseDeg(globals->get_aircraft_position(), doverVORPos); + const double legCrs = SGGeodesy::courseDeg(nearDover, doverVORPos); + + CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToDover, m_gpsNode->getDoubleValue("wp/wp[1]/bearing-true-deg"), 0.5); + CPPUNIT_ASSERT_DOUBLES_EQUAL(legCrs, m_gpsNode->getDoubleValue("wp/leg-true-course-deg"), 0.5); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.05); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5); + + // verify hold entry course + auto pilot = SGSharedPtr(new FGTestApi::TestPilot); + pilot->resetAtPosition(nearDover); + pilot->setCourseTrue(crsToDover); // initial course is correct + pilot->setSpeedKts(300); + pilot->flyGPSCourse(m_gps); + + auto statusNode = m_gpsNode->getNode("rnav-controller-status"); + CPPUNIT_ASSERT_EQUAL(std::string{"leg-to-hold"}, std::string{statusNode->getStringValue()}); + + // now we reset the hold count, so we don't enter + m_gpsNode->setStringValue("command", "exit-hold"); + + // this should not do anything for now + CPPUNIT_ASSERT_EQUAL(std::string{"leg-to-hold"}, std::string{statusNode->getStringValue()}); + + bool ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "hold-exiting") return true; + return false; + }); + + // and now we should exit the hold and sequence to TAWNY + ok = FGTestApi::runForTimeWithCheck(600.0, [fp]() { + if (fp->currentIndex() == 4) return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + CPPUNIT_ASSERT_EQUAL(std::string{"leg"}, std::string{m_gpsNode->getStringValue("mode")}); + + // check we get back enroute okay + FGTestApi::runForTime(120.0); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.05); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5); +} + +///////////////////////////////////////////////////////////////////////////// +// test entering the hold from wildly off the leg's track + +void HoldControllerTests::testHoldEntryOffCourse() +{ + FGTestApi::setUp::logPositionToKML("hold_entry_off-course"); + + auto rm = globals->get_subsystem(); + auto fp = new FlightPlan; + rm->setFlightPlan(fp); + FGTestApi::setUp::populateFPWithoutNasal(fp, "EBBR", "07L", "EGGD", "27", + "NIK COA DVR TAWNY WOD"); + FGTestApi::writeFlightPlanToKML(fp); + + auto testDelegate = new TestFPDelegate; + testDelegate->thePlan = fp; + CPPUNIT_ASSERT(rm->activate()); + fp->addDelegate(testDelegate); + + setupStandardGPS(); + m_gpsNode->setStringValue("command", "leg"); + setPositionAndStabilise(fp->departureRunway()->pointOnCenterline(0.0)); + + CPPUNIT_ASSERT_EQUAL(std::string{"leg"}, std::string{m_gpsNode->getStringValue("mode")}); + CPPUNIT_ASSERT_EQUAL(std::string{"EBBR-07L"}, std::string{m_gpsNode->getStringValue("wp/wp[1]/ID")}); + + auto leg = fp->legAtIndex(3); // DVR vor + leg->setHoldCount(3); + CPPUNIT_ASSERT_EQUAL(std::string{"hold"}, leg->waypoint()->type()); + + flightgear::Hold* holdWpt = static_cast(leg->waypoint()); + // this should be a direct entry, but now we're going to go off course! + holdWpt->setHoldRadial(330); + + // sequence to that waypoint + auto doverVORPos = fp->legAtIndex(3)->waypoint()->position(); + SGGeod nearDover = fp->pointAlongRoute(3, -6.0); + SGGeod offCoursePos = SGGeodesy::direct(doverVORPos, 20, 3.0 * SG_NM_TO_METER); + setPositionAndStabilise(nearDover); + + // check course deviation / cross-track error - we should still be at zero + const double legCrs = SGGeodesy::courseDeg(nearDover, doverVORPos); + +// sequence to the COA -> DVR leg + fp->setCurrentIndex(3); + +// assume we got wildly off course somehow + setPositionAndStabilise(offCoursePos); + + // verify hold entry course + auto pilot = SGSharedPtr(new FGTestApi::TestPilot); + pilot->setCourseTrue(legCrs); // initial course is correct + pilot->setSpeedKts(250); + pilot->flyGPSCourse(m_gps); + pilot->resetAtPosition(offCoursePos); + const double crsToDover = SGGeodesy::courseDeg(globals->get_aircraft_position(), doverVORPos); + pilot->setCourseTrue(crsToDover); // initial course is correct + + CPPUNIT_ASSERT_EQUAL(std::string{"COA"}, std::string{m_gpsNode->getStringValue("wp/wp[0]/ID")}); + CPPUNIT_ASSERT_EQUAL(std::string{"DVR"}, std::string{m_gpsNode->getStringValue("wp/wp[1]/ID")}); + + CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToDover, m_gpsNode->getDoubleValue("wp/wp[1]/bearing-true-deg"), 0.5); + CPPUNIT_ASSERT_DOUBLES_EQUAL(legCrs, m_gpsNode->getDoubleValue("wp/leg-true-course-deg"), 0.5); + // CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.05); + CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToDover - legCrs, m_gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5); + + auto statusNode = m_gpsNode->getNode("rnav-controller-status"); + CPPUNIT_ASSERT_EQUAL(std::string{"leg-to-hold"}, std::string{statusNode->getStringValue()}); + + // we should select parallel entry due to being off course + bool ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "entry-parallel") return true; + return false; + }); + CPPUNIT_ASSERT(ok); + + ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "hold-outbound") return true; + return false; + }); + + + CPPUNIT_ASSERT(ok); + // first outbound leg + ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "hold-inbound") return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + + ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "hold-outbound") return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + FGTestApi::runForTime(45.0); + + // reqeust hold exit + m_gpsNode->setStringValue("command", "exit-hold"); + + ok = FGTestApi::runForTimeWithCheck(600.0, [statusNode]() { + std::string s = statusNode->getStringValue(); + if (s == "hold-inbound") return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + + // and now we should exit the hold and sequence to TAWNY + + ok = FGTestApi::runForTimeWithCheck(600.0, [fp]() { + if (fp->currentIndex() == 4) return true; + return false; + }); + + CPPUNIT_ASSERT(ok); + CPPUNIT_ASSERT_EQUAL(std::string{"leg"}, std::string{m_gpsNode->getStringValue("mode")}); + + // check we get back enroute okay + FGTestApi::runForTime(45.0); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.05); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, m_gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5); +} diff --git a/test_suite/unit_tests/Instrumentation/test_hold_controller.hxx b/test_suite/unit_tests/Instrumentation/test_hold_controller.hxx new file mode 100644 index 000000000..4434a18f9 --- /dev/null +++ b/test_suite/unit_tests/Instrumentation/test_hold_controller.hxx @@ -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 . + */ + + +#ifndef _FG_HOLD_CTL_UNIT_TESTS_HXX +#define _FG_HOLD_CTL_UNIT_TESTS_HXX + + +#include +#include + +#include + +#include + +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