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

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

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;

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:
+  // find published hold for aWpt
-  HoldCtl(RNAV* aRNAV, const WayptRef& aWpt) :
+  // do we have this?!
-    WayptController(aRNAV, aWpt)
+    if (aWpt->type() == "hold") {
-    
+        const auto hold = static_cast<Hold*>(aWpt.ptr());
-  {
+        _holdCourse = hold->inboundRadial();
-
+        if (hold->isDistance())
-  }
+            _holdLegDistance = hold->timeOrDistance();
-
+        else
-  virtual void init()
+            _holdLegTime = hold->timeOrDistance();
-  {
+        _leftHandTurns = hold->isLeftHanded();
-  }
-  
-  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);
-        }
-      }
-    
     }
+}
+
+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:

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
+  virtual double trueBearingDeg() const;
-    { return _targetTrack; }
 
-  virtual double targetTrackDeg() const 
+  virtual double targetTrackDeg() const;
-    { return _targetTrack; }
-
-  virtual double xtrackErrorNm() const
-    { return 0.0; }
-
-  virtual double courseDeviationDeg() const
-    { return 0.0; }
 
+  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
+  bool isDone() const;
-    { return _isDone; }
 
   /**
    * 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

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

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;

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") ||
+      if ((wpt->type() == "discontinuity") ||  (wpt->type()  == "via"))
-              (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

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
 )

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");
 
 

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