+ double _distanceAircraftTargetMeter;
+ double _courseDev;
+};
+
+/**
+ * Controller for leg course interception.
+ * In leg mode, we want to intercept the leg between 2 waypoints(A->B).
+ * If we can't reach the the selected waypoint leg,we going direct to B.
+ */
+
+class LegWayptCtl : public WayptController
+{
+public:
+ LegWayptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
+ WayptController(aRNAV, aWpt),
+ _waypointOrigin(),
+ _distanceOriginAircraftMeter(0.0),
+ _distanceAircraftTargetMeter(0.0),
+ _courseOriginToAircraft(0.0),
+ _courseAircraftToTarget(0.0),
+ _courseDev(0.0),
+ _toFlag(true)
+ {
+ if (aWpt->flag(WPT_DYNAMIC)) {
+ throw sg_exception("LegWayptCtl doesn't work with dynamic waypoints");
+ }
+ }
+
+ virtual void init()
+ {
+ double courseOriginTarget;
+ bool isPreviousLegValid = false;
+
+ _waypointOrigin = _rnav->previousLegWaypointPosition(isPreviousLegValid);
+
+ courseOriginTarget = SGGeodesy::courseDeg(_waypointOrigin,_waypt->position());
+
+ _courseAircraftToTarget = SGGeodesy::courseDeg(_rnav->position(),_waypt->position());
+ _distanceAircraftTargetMeter = SGGeodesy::distanceM(_rnav->position(),_waypt->position());
+
+
+ // check reach the leg in 45Deg or going direct
+ bool canReachLeg = (fabs(courseOriginTarget -_courseAircraftToTarget) < 45.0);
+
+ if ( isPreviousLegValid && canReachLeg){
+ _targetTrack = courseOriginTarget;
+ }else{
+ _targetTrack = _courseAircraftToTarget;
+ _waypointOrigin = _rnav->position();
+ }
+
+ }
+
+ virtual void update()
+ {
+ _courseOriginToAircraft = SGGeodesy::courseDeg(_waypointOrigin,_rnav->position());
+ _distanceOriginAircraftMeter = SGGeodesy::distanceM(_waypointOrigin,_rnav->position());
+
+ _courseAircraftToTarget = SGGeodesy::courseDeg(_rnav->position(),_waypt->position());
+ _distanceAircraftTargetMeter = SGGeodesy::distanceM(_rnav->position(),_waypt->position());
+
+ _courseDev = -(_courseOriginToAircraft - _targetTrack);
+
+ bool isMinimumOverFlightDistanceReached = _distanceAircraftTargetMeter < _rnav->overflightDistanceM();
+ bool isOverFlightConeArmed = _distanceAircraftTargetMeter < ( _rnav->overflightArmDistanceM() + _rnav->overflightDistanceM() );
+ bool leavingOverFlightCone = (fabs(_courseAircraftToTarget - _targetTrack) > _rnav->overflightArmAngleDeg());
+
+ if( isMinimumOverFlightDistanceReached ){
+ _toFlag = false;
+ setDone();
+ }else{
+ if( isOverFlightConeArmed ){
+ _toFlag = false;
+ if ( leavingOverFlightCone ) {
+ setDone();
+ }
+ }else{
+ _toFlag = true;
+ }
+ }
+ }
+
+ virtual double distanceToWayptM() const
+ {
+ return _distanceAircraftTargetMeter;
+ }
+
+ virtual double xtrackErrorNm() const
+ {
+ return greatCircleCrossTrackError(_distanceOriginAircraftMeter * SG_METER_TO_NM, _courseDev);
+ }
+
+ virtual bool toFlag() const
+ {
+ return _toFlag;
+ }
+
+ virtual double courseDeviationDeg() const
+ {
+ return _courseDev;
+ }
+
+ virtual double trueBearingDeg() const
+ {
+ return _courseAircraftToTarget;
+ }
+
+ virtual SGGeod position() const
+ {
+ return _waypt->position();
+ }
+
+private:
+
+ /*
+ * great circle route
+ * A(from), B(to), D(position) perhaps off course
+ */
+ SGGeod _waypointOrigin;
+ double _distanceOriginAircraftMeter;
+ double _distanceAircraftTargetMeter;
+ double _courseOriginToAircraft;
+ double _courseAircraftToTarget;