GPS fly-by support

Remove the non-functional turn-anticipation code from the GPS, and
enable fly-by mode in (some of the) RNAV controllers: initially the
leg controller.

The new config property is gps/config/enable-fly-by, defaults to off
for compatibility.

src/Instrumentation/gps.cxx

@@ -60,7 +60,7 @@ static const char* makeTTWString(double TTW)
 // configuration helper object
 
 GPS::Config::Config() :
-  _enableTurnAnticipation(true),
+  _enableTurnAnticipation(false),
   _turnRate(3.0), // degrees-per-second, so 180 degree turn takes 60 seconds
   _overflightDistance(0.02),
   _overflightArmDistance(1.0),
@@ -72,13 +72,12 @@ GPS::Config::Config() :
   _courseSelectable(false),
   _followLegTrackToFix(true)
 {
-  _enableTurnAnticipation = false;
 }
 
 void GPS::Config::bind(GPS* aOwner, SGPropertyNode* aCfg)
 {
   aOwner->tie(aCfg, "turn-rate-deg-sec", SGRawValuePointer<double>(&_turnRate));
-  aOwner->tie(aCfg, "turn-anticipation", SGRawValuePointer<bool>(&_enableTurnAnticipation));
+  aOwner->tie(aCfg, "enable-fly-by", SGRawValuePointer<bool>(&_enableTurnAnticipation));
   aOwner->tie(aCfg, "wpt-alert-time", SGRawValuePointer<double>(&_waypointAlertTime));
   aOwner->tie(aCfg, "hard-surface-runways-only", SGRawValuePointer<bool>(&_requireHardSurface));
   aOwner->tie(aCfg, "cdi-max-deflection-nm", SGRawValuePointer<double>(&_cdiMaxDeflectionNm));
@@ -102,9 +101,6 @@ GPS::GPS ( SGPropertyNode *node, bool defaultGPSMode) :
   _mode("init"),
   _name(node->getStringValue("name", "gps")),
   _num(node->getIntValue("number", 0)),
-  _computeTurnData(false),
-  _anticipateTurn(false),
-  _inTurn(false),
   _callbackFlightPlanChanged(SGPropertyChangeCallback<GPS>(this,&GPS::routeManagerFlightPlanChanged,
                                                            fgGetNode("/autopilot/route-manager/signals/flightplan-changed", true))),
   _callbackRouteActivated(SGPropertyChangeCallback<GPS>(this,&GPS::routeActivated,
@@ -332,7 +328,9 @@ GPS::update (double delta_time_sec)
       
       _gpsNode->setStringValue("rnav-controller-status", _wayptController->status());
         
-      updateTurn();
+      if (_wayptController->isDone()) {
+        doSequence();
+      }
       updateRouteData();
     }
 
@@ -467,26 +465,47 @@ double GPS::selectedMagCourse()
   return _selectedCourse;
 }
 
-SGGeod GPS::previousLegWaypointPosition(bool& isValid)
+simgear::optional<double> GPS::nextLegTrack()
 {
-	FlightPlan::Leg* leg = _route->previousLeg();
+    auto next = _route->nextLeg();
-	if (leg){
+    if (!next)
-		Waypt* waypt = leg->waypoint();
+        return {};
-		if (waypt) {
+    
-            isValid = true;
+    return next->courseDeg();
-            // ensure computations use runway end, not threshold
-            if (waypt->type() == "runway") {
-                RunwayWaypt* rwpt = static_cast<RunwayWaypt*>(waypt);
-                return rwpt->runway()->end();
-            }
-
-			return waypt->position();
-		}
-	}
-
-	isValid = false;
-    return SGGeod();
 }
+
+simgear::optional<RNAV::LegData> GPS::previousLegData()
+{
+  // if the previous controller computed valid data,
+  // use that. This ensures fly-by turns work out, especially
+  if (_wp0Data.has_value())
+    return _wp0Data;
+  
+  // if we did not get data from the previous controller (eg, waypoint
+  // jumped or first waypoint), just compute the position
+  FlightPlan::Leg* leg = _route->previousLeg();
+  if (!leg) {
+    return {}; // no data
+  }
+  
+  LegData legData;
+  Waypt* waypt = leg->waypoint();
+  legData.position = waypt->position();
+  
+   // ensure computations use runway end, not threshold
+   if (waypt->type() == "runway") {
+       RunwayWaypt* rwpt = static_cast<RunwayWaypt*>(waypt);
+       legData.position =  rwpt->runway()->end();
+   }
+
+  return legData;
+}
+
+bool GPS::canFlyBy() const
+{
+    return _config.turnAnticipationEnabled();
+}
+
 ///////////////////////////////////////////////////////////////////////////
 
 void
@@ -560,6 +579,7 @@ GPS::updateTrackingBug()
 
 void GPS::currentWaypointChanged()
 {
+  _wp0Data.reset();
   if (!_route) {
     return;
   }
@@ -583,7 +603,12 @@ void GPS::currentWaypointChanged()
   }
   
   _currentWaypt = _route->currentLeg()->waypoint();
-    wp1Changed(); // rebuild the active controller
+  if (_wayptController && (_wayptController->waypoint() == _prevWaypt)) {
+    // capture leg data form the controller, before we destroy it
+    _wp0Data = _wayptController->legData();
+  }
+  
+  wp1Changed(); // rebuild the active controller
   _desiredCourse = getLegMagCourse();
   _desiredCourseNode->fireValueChanged();
 }
@@ -661,164 +686,11 @@ void GPS::endOfFlightPlan()
     cleared();
 }
 
-void GPS::updateTurn()
-{
-  bool printProgress = false;
-  
-  if (_computeTurnData) {
-    if (_last_speed_kts < 10) {
-      // need valid leg course and sensible ground speed to compute the turn
-      return;
-    }
-    
-    computeTurnData();
-    printProgress = true;
-  }
-  
-  if (!_anticipateTurn) {
-    updateOverflight();
-    return;
-  }
-
-  updateTurnData();
-  // find bearing to turn centre
-  double bearing, az2, distanceM;
-  SGGeodesy::inverse(_indicated_pos, _turnCentre, bearing, az2, distanceM);
-  double progress = computeTurnProgress(bearing);
-  
-  if (printProgress) {
-    SG_LOG(SG_INSTR, SG_INFO,"turn progress=" << progress);
-  }
-  
-  if (!_inTurn && (progress > 0.0)) {
-    beginTurn();
-  }
-  
-  if (_inTurn && !_turnSequenced && (progress > 0.5)) {
-    _turnSequenced = true;
-     SG_LOG(SG_INSTR, SG_INFO, "turn passed midpoint, sequencing");
-     doSequence();
-  }
-  
-  if (_inTurn && (progress >= 1.0)) {
-    endTurn();
-  }
-  
-  if (_inTurn) {
-    // drive deviation and desired course
-    double desiredCourse = bearing - copysign(90, _turnAngle);
-    SG_NORMALIZE_RANGE(desiredCourse, 0.0, 360.0);
-    double deviationNm = (distanceM * SG_METER_TO_NM) - _turnRadius;
-    double deviationDeg = desiredCourse - getMagTrack();
-    deviationNm = copysign(deviationNm, deviationDeg);
-    // FIXME
-    //_wp1_course_deviation_node->setDoubleValue(deviationDeg);
-    //_wp1_course_error_nm_node->setDoubleValue(deviationNm);
-    //_cdiDeflectionNode->setDoubleValue(deviationDeg);
-  }
-}
-
-void GPS::updateOverflight()
-{
-  if (_wayptController->isDone()) {
-      doSequence();
-      _computeTurnData = true;
-  }
-}
-
-void GPS::beginTurn()
-{
-  _inTurn = true;
-  _turnSequenced = false;
-  SG_LOG(SG_INSTR, SG_INFO, "beginning turn");
-}
-
-void GPS::endTurn()
-{
-  _inTurn = false;
-  SG_LOG(SG_INSTR, SG_INFO, "ending turn");
-  _computeTurnData = true;
-}
-
-double GPS::computeTurnProgress(double aBearing) const
-{
-  double startBearing = _turnStartBearing + copysign(90, _turnAngle);
-  return (aBearing - startBearing) / _turnAngle;
-}
-
-void GPS::computeTurnData()
-{
-  _computeTurnData = false;
-  if ((_mode != "leg") || !_route->nextLeg()) {
-    _anticipateTurn = false;
-    return;
-  }
-  
-  WayptRef next = _route->nextLeg()->waypoint();
-  if (next->flag(WPT_DYNAMIC) ||
-      !_config.turnAnticipationEnabled() ||
-      next->flag(WPT_OVERFLIGHT))
-  {
-    _anticipateTurn = false;
-    return;
-  }
-  
-  _turnStartBearing = _desiredCourse;
-// compute next leg course
-  RoutePath path(_route);
-  double crs = path.trackForIndex(_route->currentIndex() + 1);
-
-// compute offset bearing
-  _turnAngle = crs - _turnStartBearing;
-  SG_NORMALIZE_RANGE(_turnAngle, -180.0, 180.0);
-  double median = _turnStartBearing + (_turnAngle * 0.5);
-  double offsetBearing = median + copysign(90, _turnAngle);
-  SG_NORMALIZE_RANGE(offsetBearing, 0.0, 360.0);
-  
-  SG_LOG(SG_INSTR, SG_INFO, "GPS computeTurnData: in=" << _turnStartBearing <<
-    ", out=" << crs << "; turnAngle=" << _turnAngle << ", median=" << median 
-    << ", offset=" << offsetBearing);
-
-  SG_LOG(SG_INSTR, SG_INFO, "next leg is now:" << _currentWaypt->ident() << "->" << next->ident());
-
-  _turnPt = _currentWaypt->position();
-  _anticipateTurn = true;
-}
-
-void GPS::updateTurnData()
-{
-  // depends on ground speed, so needs to be updated per-frame
-  _turnRadius = computeTurnRadiusNm(_last_speed_kts);
-  
-  // compute the turn centre, based on the turn radius.
-  // key thing is to understand that we're working a right-angle triangle,
-  // where the right-angle is the point we start the turn. From that point,
-  // one side is the inbound course to the turn pt, and the other is the
-  // perpendicular line, of length 'r', to the turn centre.
-  // the triangle's hypotenuse, which we need to find, is the distance from the
-  // turn pt to the turn center (in the direction of the offset bearing)
-  // note that d - _turnRadius tell us how much we're 'cutting' the corner.
-  
-  double halfTurnAngle = fabs(_turnAngle * 0.5) * SG_DEGREES_TO_RADIANS;
-  double d = _turnRadius / cos(halfTurnAngle);
-  
- // SG_LOG(SG_INSTR, SG_INFO, "turnRadius=" << _turnRadius << ", d=" << d
- //   << " (cut distance=" << d - _turnRadius << ")");
-  
-  double median = _turnStartBearing + (_turnAngle * 0.5);
-  double offsetBearing = median + copysign(90, _turnAngle);
-  SG_NORMALIZE_RANGE(offsetBearing, 0.0, 360.0);
-  
-  double az2;
-  SGGeodesy::direct(_turnPt, offsetBearing, d * SG_NM_TO_METER, _turnCentre, az2); 
-}
-
 double GPS::turnRadiusNm(double groundSpeedKts)
 {
     return computeTurnRadiusNm(groundSpeedKts);
 }
 
-
 double GPS::computeTurnRadiusNm(double aGroundSpeedKts) const
 {
 	// turn time is seconds to execute a 360 turn. 
@@ -877,6 +749,7 @@ void GPS::wp1Changed()
 {
   if (!_currentWaypt)
     return;
+  
   if (_mode == "leg") {
     _wayptController.reset(WayptController::createForWaypt(this, _currentWaypt));
     if (_currentWaypt->type() == "hold") {
@@ -890,16 +763,16 @@ void GPS::wp1Changed()
   } else if (_mode == "dto") {
     _wayptController.reset(new DirectToController(this, _currentWaypt, _wp0_position));
   }
-
+  
   _wayptController->init();
   _gpsNode->setStringValue("rnav-controller-status", _wayptController->status());
-
+  
   if (_mode == "obs") {
     _legDistanceNm = -1.0;
   } else {
     _wayptController->update(0.0);
     _gpsNode->setStringValue("rnav-controller-status", _wayptController->status());
-
+    
     _legDistanceNm = _wayptController->distanceToWayptM() * SG_METER_TO_NM;
     
     // synchronise these properties immediately
@@ -907,7 +780,7 @@ void GPS::wp1Changed()
     _currentWayptNode->setDoubleValue("longitude-deg", p.getLongitudeDeg());
     _currentWayptNode->setDoubleValue("latitude-deg", p.getLatitudeDeg());
     _currentWayptNode->setDoubleValue("altitude-ft", p.getElevationFt());
-      
+    
     _desiredCourse = getLegMagCourse();
   }
 }
@@ -1308,6 +1181,11 @@ void GPS::selectLegMode()
 
   _mode = "leg";  
 
+// clear any previous leg data which might be hanging around
+// note this means you can mess up fly-by by toggling into and out LEG
+// mode, but this seems reasonable
+  _wp0Data.reset();
+
   // depending on the situation, this will either get over-written 
   // in routeManagerSequenced or not; either way it does no harm to
   // set it here.

src/Instrumentation/gps.hxx

@@ -76,15 +76,19 @@ public:
 
   // RNAV interface
     SGGeod position() override;
-    virtual double trackDeg();
+    double trackDeg() override;
-    virtual double groundSpeedKts();
+    double groundSpeedKts() override;
-    virtual double vspeedFPM();
+    double vspeedFPM() override;
-    virtual double magvarDeg();
+    double magvarDeg() override;
-    virtual double selectedMagCourse();
+    double selectedMagCourse() override;
-    virtual double overflightDistanceM();
+    double overflightDistanceM() override;
-    virtual double overflightArmDistanceM();
+    double overflightArmDistanceM() override;
-    virtual double overflightArmAngleDeg();
+    double overflightArmAngleDeg() override;
-    virtual SGGeod previousLegWaypointPosition(bool& isValid);
+    bool canFlyBy() const override;
+    
+    simgear::optional<LegData> previousLegData() override;
+    
+    simgear::optional<double> nextLegTrack() override;
 
     double turnRadiusNm(double groundSpeedKnots) override;
 private:
@@ -208,17 +212,7 @@ private:
     void updateTrackingBug();
     void updateRouteData();
     void driveAutopilot();
-
+  
-    void updateTurn();
-    void updateOverflight();
-    void beginTurn();
-    void endTurn();
-
-    double computeTurnProgress(double aBearing) const;
-    void computeTurnData();
-    void updateTurnData();
-    double computeTurnRadiusNm(double aGroundSpeedKts) const;
-
     /** Update one-shot things when WP1 / leg data change */
     void wp1Changed();
 
@@ -307,6 +301,7 @@ private:
 
     // true-bearing-error and mag-bearing-error
 
+    double computeTurnRadiusNm(double aGroundSpeedKts) const;
 
     /**
      * Tied-properties helper, record nodes which are tied for easy un-tie-ing
@@ -403,17 +398,8 @@ private:
     bool _searchNames; ///< set if we're searching names instead of idents
 #endif
 
-    // turn data
+    simgear::optional<RNAV::LegData> _wp0Data;
-    bool _computeTurnData; ///< do we need to update the turn data?
+    
-    bool _anticipateTurn; ///< are we anticipating the next turn or not?
-    bool _inTurn; // is a turn in progress?
-    bool _turnSequenced; // have we sequenced the new leg?
-    double _turnAngle; // angle to turn through, in degrees
-    double _turnStartBearing; // bearing of inbound leg
-    double _turnRadius; // radius of turn in nm
-    SGGeod _turnPt;
-    SGGeod _turnCentre;
-
     std::unique_ptr<flightgear::WayptController> _wayptController;
 
     flightgear::WayptRef _prevWaypt;

src/Instrumentation/rnav_waypt_controller.cxx

@@ -28,6 +28,8 @@
 #include <Airports/runways.hxx>
 #include "Main/util.hxx" // for fgLowPass
 
+#include "test_suite/FGTestApi/TestDataLogger.hxx"
+
 extern double pointsKnownDistanceFromGC(const SGGeoc& a, const SGGeoc&b, const SGGeoc& d, double dist);
 
 namespace flightgear
@@ -243,64 +245,213 @@ public:
       throw sg_exception("LegWayptCtl doesn't work with dynamic waypoints");
     }
   }
-
+  
   virtual void init()
   {
-	bool isPreviousLegValid = false;
+    auto previousLeg = _rnav->previousLegData();
-
+    if (previousLeg) {
-	_waypointOrigin = _rnav->previousLegWaypointPosition(isPreviousLegValid);
+      _waypointOrigin = previousLeg.value().position;
-
+      _initialLegCourse = SGGeodesy::courseDeg(_waypointOrigin,_waypt->position());
-	_initialLegCourse 				= SGGeodesy::courseDeg(_waypointOrigin,_waypt->position());
+    } else {
-	_courseAircraftToTarget			= SGGeodesy::courseDeg(_rnav->position(),_waypt->position());
+      // capture current position
-	_distanceAircraftTargetMetre 	= SGGeodesy::distanceM(_rnav->position(),_waypt->position());
+      _waypointOrigin = _rnav->position();
-
+    }
-
+    
-	// check reach the leg in 45Deg or going direct
+    _courseAircraftToTarget			= SGGeodesy::courseDeg(_rnav->position(),_waypt->position());
-	bool canReachLeg = (fabs(_initialLegCourse -_courseAircraftToTarget) < 45.0);
+    _distanceAircraftTargetMetre 	= SGGeodesy::distanceM(_rnav->position(),_waypt->position());
-
+    
-	if (isPreviousLegValid && canReachLeg) {
+    
+    // check reach the leg in 45Deg or going direct
+    bool canReachLeg = (fabs(_initialLegCourse -_courseAircraftToTarget) < 45.0);
+    
+    if (previousLeg && canReachLeg) {
       _targetTrack = _initialLegCourse;
-	} else {
+    } else {
       // can't reach the leg with out a crazy turn, just go direct to the
       // destination waypt
       _targetTrack = _courseAircraftToTarget;
       _initialLegCourse = _courseAircraftToTarget;
       _waypointOrigin = _rnav->position();
-	}
+    }
+      
+  // turn angle depends on final leg course, not initial
+  // do this here so we have a chance of doing a fly-by at the end of the leg
+  _finalLegCourse = SGGeodesy::courseDeg(_waypt->position(), _waypointOrigin) + 180;
+  SG_NORMALIZE_RANGE(_finalLegCourse, 0.0, 360.0);
+    
+    // turn-in logic
+    if (previousLeg.has_value() && previousLeg.value().didFlyBy) {
+      _flyByTurnCenter = previousLeg.value().flyByTurnCenter;
+      _flyByTurnAngle = previousLeg.value().turnAngle;
+      _flyByTurnRadius = previousLeg.value().flyByRadius;
+      _entryFlyByActive = true;
+      SG_NORMALIZE_RANGE(_flyByTurnAngle, -180.0, 180.0);
+    }
+  }
+  
+  void computeTurnAnticipation()
+  {
+    _didComputeTurn = true;
+    
+    if (_waypt->flag(WPT_OVERFLIGHT)) {
+      return; // can't fly-by
+    }
+    
+    if (!_rnav->canFlyBy())
+      return;
+    
+    auto nextLegTrack = _rnav->nextLegTrack();
+    
+    if (!nextLegTrack.has_value()) {
+      return;
+    }
+    
+    _flyByTurnAngle = nextLegTrack.value() - _finalLegCourse;
+    SG_NORMALIZE_RANGE(_flyByTurnAngle, -180.0, 180.0);
+    
+    if (fabs(_flyByTurnAngle) > 120.0) {
+      // too sharp, don't anticipate
+      return;
+    }
+    
+    _flyByTurnRadius =  _rnav->turnRadiusNm() * SG_NM_TO_METER;
+    
+    // this is the heading half way through the turn. Perpendicular to
+    // this is our turn ceenter
+    const auto halfTurnHeading = _finalLegCourse + (_flyByTurnAngle * 0.5);
+    double p = copysign(90.0, _flyByTurnAngle);
+    double h = halfTurnHeading + p;
+    SG_NORMALIZE_RANGE(h, 0.0, 360.0);
+      
+    const double tcOffset = _flyByTurnRadius / cos(_flyByTurnAngle * 0.5 * SG_DEGREES_TO_RADIANS);
+    _flyByTurnCenter = SGGeodesy::direct(_waypt->position(), h, tcOffset);
+    _doFlyBy = true;
+  }
+  
+  bool updateInTurn()
+  {
+      auto dl =  FGTestApi::DataLogger::instance();
+
+    // find bearing to turn center
+    // when it hits 90 off our track
+    
+    auto bearingToTurnCenter = SGGeodesy::courseDeg(_rnav->position(), _flyByTurnCenter);
+    auto distToTurnCenter = SGGeodesy::distanceM(_rnav->position(), _flyByTurnCenter);
+
+    if (!_flyByStarted) {
+      // check for the turn center being 90 degrees off one wing (start the turn)
+      auto a = bearingToTurnCenter - _finalLegCourse;
+      SG_NORMALIZE_RANGE(a, -180.0, 180.0);
+        if (fabs(a) < 90.0) {
+          dl->recordSamplePoint("turn-entry-bearing", a);
+        return false; // keep flying normal leg
+      }
+      
+      _flyByStarted = true;
+    }
+    
+    // check for us passing the half-way point; that's when we should
+    // sequence to the next WP
+    const auto halfPointAngle = (_finalLegCourse + (_flyByTurnAngle * 0.5));
+    auto b = bearingToTurnCenter - halfPointAngle;
+    SG_NORMALIZE_RANGE(b, -180.0, 180.0);
+      
+      dl->recordSamplePoint("turn-exit-bearing", b);
+      
+    if (fabs(b) >= 90.0) {
+      _toFlag = false;
+      setDone();
+    }
+    
+    // in the actual turn, our desired track is always pependicular to the
+    // bearing to the turn center we computed
+    _targetTrack = bearingToTurnCenter - copysign(90, _flyByTurnAngle);
+      
+    SG_NORMALIZE_RANGE(_targetTrack, 0.0, 360.0);
+
+    _crossTrackError = (distToTurnCenter - _flyByTurnRadius) * SG_METER_TO_NM;
+    _courseDev = _crossTrackError * 10.0; // arbitrary guess for now
+    
+    return true;
+  }
+  
+  void updateInEntryTurn()
+  {
+      auto dl =  FGTestApi::DataLogger::instance();
+
+      
+    auto bearingToTurnCenter = SGGeodesy::courseDeg(_rnav->position(), _flyByTurnCenter);
+    auto distToTurnCenter = SGGeodesy::distanceM(_rnav->position(), _flyByTurnCenter);
+  
+    auto b = bearingToTurnCenter - _initialLegCourse;
+      dl->recordSamplePoint("turn-entry-bearing", b);
+
+      SG_NORMALIZE_RANGE(b, -180.0, 180.0);
+    if (fabs(b) >= 90.0) {
+      _entryFlyByActive = false;
+      return; // we're done with the entry turn
+    }
+    
+    _targetTrack = bearingToTurnCenter - copysign(90, _flyByTurnAngle);
+
+    SG_NORMALIZE_RANGE(_targetTrack, 0.0, 360.0);
+    _crossTrackError = (distToTurnCenter - _flyByTurnRadius) * SG_METER_TO_NM;
+    _courseDev = _crossTrackError * 10.0; // arbitrary guess for now
   }
 
-  virtual void update(double)
+  void update(double) override
   {
+      auto dl =  FGTestApi::DataLogger::instance();
+
     _courseOriginToAircraft			= SGGeodesy::courseDeg(_waypointOrigin,_rnav->position());
     _distanceOriginAircraftMetre 	= SGGeodesy::distanceM(_waypointOrigin,_rnav->position());
 
     _courseAircraftToTarget			= SGGeodesy::courseDeg(_rnav->position(),_waypt->position());
     _distanceAircraftTargetMetre 	= SGGeodesy::distanceM(_rnav->position(),_waypt->position());
 
+    if (_entryFlyByActive) {
+      updateInEntryTurn();
+      return;
+    }
+    
+    const auto turnComputeDist = SG_NM_TO_METER * 4.0;
+    if (!_didComputeTurn && (_distanceAircraftTargetMetre < turnComputeDist)) {
+      computeTurnAnticipation();
+    }
+        
+    if (_didComputeTurn && _doFlyBy) {
+      bool ok = updateInTurn();
+      if (ok) {
+        return;
+      }
+      
+      // otherwise we fall through
+    }
+    
     // from the Aviation Formulary
 #if 0
-      Suppose you are proceeding on a great circle route from A to B (course =crs_AB) and end up at D,
+    Suppose you are proceeding on a great circle route from A to B (course =crs_AB) and end up at D,
-      perhaps off course. (We presume that A is ot a pole!) You can calculate the course from A to D
+    perhaps off course. (We presume that A is ot a pole!) You can calculate the course from A to D
-      (crs_AD) and the distance from A to D (dist_AD) using the formulae above. In terms of these the
+    (crs_AD) and the distance from A to D (dist_AD) using the formulae above. In terms of these the
-      cross track error, XTD, (distance off course) is given by
+    cross track error, XTD, (distance off course) is given by
-      
+    
-      XTD =asin(sin(dist_AD)*sin(crs_AD-crs_AB))
+    XTD =asin(sin(dist_AD)*sin(crs_AD-crs_AB))
-      (positive XTD means right of course, negative means left)
+    (positive XTD means right of course, negative means left)
-      (If the point A is the N. or S. Pole replace crs_AD-crs_AB with
+    (If the point A is the N. or S. Pole replace crs_AD-crs_AB with
-       lon_D-lon_B or lon_B-lon_D, respectively.)
+     lon_D-lon_B or lon_B-lon_D, respectively.)
 #endif
     // however, just for fun, our convention for polarity of the cross-track
     // sign is opposite, so we add a -ve to the computation below.
-      
+    
     double distOriginAircraftRad = _distanceOriginAircraftMetre * SG_METER_TO_NM * SG_NM_TO_RAD;
     double xtkRad = asin(sin(distOriginAircraftRad) * sin((_courseOriginToAircraft - _initialLegCourse) * SG_DEGREES_TO_RADIANS));
-      
+    
     // convert to NM and flip sign for consistency with existing code.
     // since we derive the abeam point and course-deviation from this, and
     // thus the GPS cdi-deflection, if we don't fix this here, the sign of
     // all of those comes out backwards.
     _crossTrackError = -(xtkRad * SG_RAD_TO_NM);
-
+    
     /*
      The "along track distance", ATD, the distance from A along the course towards B
      to the point abeam D is given by:
@@ -324,6 +475,7 @@ public:
     _courseDev = _courseAircraftToTarget - _targetTrack;
     SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
 
+    
     bool isMinimumOverFlightDistanceReached = _distanceAircraftTargetMetre < _rnav->overflightDistanceM();
     bool isOverFlightConeArmed 				= _distanceAircraftTargetMetre < ( _rnav->overflightArmDistanceM() + _rnav->overflightDistanceM() );
     bool leavingOverFlightCone 				= (fabs(_courseDev) > _rnav->overflightArmAngleDeg());
@@ -343,6 +495,22 @@ public:
     }
   }
 
+  simgear::optional<RNAV::LegData> legData() const override
+  {
+    RNAV::LegData r;
+    r.position = _waypt->position();
+    if (_doFlyBy) {
+      // copy all the fly by paramters, so the next controller can
+      // smoothly link up
+      r.didFlyBy = true;
+      r.flyByRadius = _flyByTurnRadius;
+      r.flyByTurnCenter = _flyByTurnCenter;
+      r.turnAngle = _flyByTurnAngle;
+    }
+    
+    return r;
+  }
+  
   virtual double distanceToWayptM() const
   {
     return _distanceAircraftTargetMetre;
@@ -381,6 +549,7 @@ private:
    */
   SGGeod _waypointOrigin;
   double _initialLegCourse;
+  double _finalLegCourse;
   double _distanceOriginAircraftMetre;
   double _distanceAircraftTargetMetre;
   double _courseOriginToAircraft;
@@ -388,7 +557,14 @@ private:
   double _courseDev;
   bool _toFlag;
   double _crossTrackError;
-
+  
+  bool _didComputeTurn = false;
+  bool _doFlyBy = false;
+  SGGeod _flyByTurnCenter;
+  double _flyByTurnAngle;
+  double _flyByTurnRadius;
+  bool _entryFlyByActive = false;
+  bool _flyByStarted = false;
 };
 
 /**

src/Instrumentation/rnav_waypt_controller.hxx

@@ -21,6 +21,7 @@
 #define FG_WAYPT_CONTROLLER_HXX
 
 #include <Navaids/waypoint.hxx>
+#include <simgear/misc/simgear_optional.hxx>
 
 namespace flightgear
 {
@@ -58,7 +59,12 @@ public:
    * device selected course (eg, from autopilot / MCP / OBS) in degrees
    */
   virtual double selectedMagCourse() = 0;
-
+    
+    virtual bool canFlyBy() const
+    {
+        return false;
+    }
+    
   /**
    * minimum distance to switch next waypoint.
    */
@@ -72,15 +78,31 @@ public:
      */
     virtual double overflightArmAngleDeg() = 0;
 
+    struct LegData {
+        SGGeod position;
+        bool didFlyBy = false;
+        SGGeod flyByTurnCenter;
+        double flyByRadius = 0.0;
+        double turnAngle = 0.0;
+    };
+    
   /**
    * device leg previous waypoint position(eg, from route manager)
    */
-  virtual SGGeod previousLegWaypointPosition(bool& isValid)= 0;
+  virtual simgear::optional<LegData> previousLegData()
-
+    {
+        return simgear::optional<LegData>();
+    }
+    
+    virtual simgear::optional<double> nextLegTrack()
+    {
+        return simgear::optional<double>{};
+    }
+     
   /**
    * @brief compute turn radius based on current ground-speed
    */
-  
+    
   double turnRadiusNm()
   {
     return turnRadiusNm(groundSpeedKts());
@@ -151,11 +173,24 @@ public:
    */
   virtual std::string status() const;
     
+    virtual simgear::optional<RNAV::LegData> legData() const
+    {
+        // defer to our subcontroller if it exists
+        if (_subController)
+            return _subController->legData();
+        
+        return simgear::optional<RNAV::LegData>();
+    }
+    
   /**
    * Static factory method, given a waypoint, return a controller bound
    * to it, of the appropriate type
    */
   static WayptController* createForWaypt(RNAV* rnav, const WayptRef& aWpt);
+    
+    WayptRef waypoint() const
+    { return _waypt; }
+    
 protected:
   WayptController(RNAV* aRNAV, const WayptRef& aWpt) :
     _waypt(aWpt),

src/Navaids/route.hxx

@@ -78,7 +78,8 @@ typedef enum {
   /// waypoint prodcued by expanding a VIA segment
   WPT_VIA           = 1 << 12,
     
-    // waypoint should be hidden from maps, etc
+    /// waypoint should not be shown in UI displays, etc
+    /// this is used to implement FMSs which delete waypoints after passing them
     WPT_HIDDEN      = 1 << 13
 } WayptFlag;