1 // rnav_waypt_controller.cxx - Waypoint-specific behaviours for RNAV systems
2 // Written by James Turner, started 2009.
4 // Copyright (C) 2009 Curtis L. Olson
6 // This program is free software; you can redistribute it and/or
7 // modify it under the terms of the GNU General Public License as
8 // published by the Free Software Foundation; either version 2 of the
9 // License, or (at your option) any later version.
11 // This program is distributed in the hope that it will be useful, but
12 // WITHOUT ANY WARRANTY; without even the implied warranty of
13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 // General Public License for more details.
16 // You should have received a copy of the GNU General Public License
17 // along with this program; if not, write to the Free Software
18 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
20 #include "rnav_waypt_controller.hxx"
24 #include <simgear/sg_inlines.h>
25 #include <simgear/structure/exception.hxx>
27 #include <Airports/runways.hxx>
28 #include "Main/util.hxx" // for fgLowPass
30 extern double pointsKnownDistanceFromGC(const SGGeoc& a, const SGGeoc&b, const SGGeoc& d, double dist);
35 // declared in routePath.cxx
36 bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result);
39 * Helper function Cross track error:
40 * http://williams.best.vwh.net/avform.htm#XTE
42 * param distanceOriginToPosition in Nautical Miles
43 * param courseDev difference between courseOriginToAircraft(AD) and courseOriginToTarget(AB)
44 * return XTE in Nautical Miles
48 * D(aircraft) perhaps off course
56 double greatCircleCrossTrackError(double distanceOriginToPosition,double courseDev){
57 double crossTrackError = asin(
58 sin(distanceOriginToPosition * SG_NM_TO_RAD) *
59 sin(courseDev * SG_DEGREES_TO_RADIANS)
61 return crossTrackError * SG_RAD_TO_NM;
65 ////////////////////////////////////////////////////////////////////////////
67 WayptController::~WayptController()
71 void WayptController::init()
75 void WayptController::setDone()
78 SG_LOG(SG_AUTOPILOT, SG_WARN, "already done @ WayptController::setDone");
84 double WayptController::timeToWaypt() const
86 double gs = _rnav->groundSpeedKts();
88 return -1.0; // stationary
92 return (distanceToWayptM() / gs);
97 class BasicWayptCtl : public WayptController
100 BasicWayptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
101 WayptController(aRNAV, aWpt),
102 _distanceAircraftTargetMeter(0.0),
105 if (aWpt->flag(WPT_DYNAMIC)) {
106 throw sg_exception("BasicWayptCtrl doesn't work with dynamic waypoints");
112 _targetTrack = SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
115 virtual void update(double)
117 double bearingAircraftToTarget;
119 bearingAircraftToTarget = SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
120 _distanceAircraftTargetMeter = SGGeodesy::distanceM(_rnav->position(), _waypt->position());
122 _courseDev = bearingAircraftToTarget - _targetTrack;
123 SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
125 if ((fabs(_courseDev) > _rnav->overflightArmAngleDeg()) && (_distanceAircraftTargetMeter < _rnav->overflightArmDistanceM())) {
130 virtual double distanceToWayptM() const
132 return _distanceAircraftTargetMeter;
135 virtual double xtrackErrorNm() const
137 double x = sin(courseDeviationDeg() * SG_DEGREES_TO_RADIANS) * _distanceAircraftTargetMeter;
138 return x * SG_METER_TO_NM;
141 virtual bool toFlag() const
143 return (fabs(_courseDev) < _rnav->overflightArmAngleDeg());
146 virtual double courseDeviationDeg() const
151 virtual double trueBearingDeg() const
153 return SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
156 virtual SGGeod position() const
158 return _waypt->position();
162 double _distanceAircraftTargetMeter;
167 * Controller for leg course interception.
168 * In leg mode, we want to intercept the leg between 2 waypoints(A->B).
169 * If we can't reach the the selected waypoint leg,we going direct to B.
172 class LegWayptCtl : public WayptController
175 LegWayptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
176 WayptController(aRNAV, aWpt),
178 _distanceOriginAircraftMetre(0.0),
179 _distanceAircraftTargetMetre(0.0),
180 _courseOriginToAircraft(0.0),
181 _courseAircraftToTarget(0.0),
185 if (aWpt->flag(WPT_DYNAMIC)) {
186 throw sg_exception("LegWayptCtl doesn't work with dynamic waypoints");
192 double courseOriginTarget;
193 bool isPreviousLegValid = false;
195 _waypointOrigin = _rnav->previousLegWaypointPosition(isPreviousLegValid);
197 courseOriginTarget = SGGeodesy::courseDeg(_waypointOrigin,_waypt->position());
199 _courseAircraftToTarget = SGGeodesy::courseDeg(_rnav->position(),_waypt->position());
200 _distanceAircraftTargetMetre = SGGeodesy::distanceM(_rnav->position(),_waypt->position());
203 // check reach the leg in 45Deg or going direct
204 bool canReachLeg = (fabs(courseOriginTarget -_courseAircraftToTarget) < 45.0);
206 if ( isPreviousLegValid && canReachLeg){
207 _targetTrack = courseOriginTarget;
209 _targetTrack = _courseAircraftToTarget;
210 _waypointOrigin = _rnav->position();
215 virtual void update(double)
217 _courseOriginToAircraft = SGGeodesy::courseDeg(_waypointOrigin,_rnav->position());
218 _distanceOriginAircraftMetre = SGGeodesy::distanceM(_waypointOrigin,_rnav->position());
220 _courseAircraftToTarget = SGGeodesy::courseDeg(_rnav->position(),_waypt->position());
221 _distanceAircraftTargetMetre = SGGeodesy::distanceM(_rnav->position(),_waypt->position());
223 _courseDev = -(_courseOriginToAircraft - _targetTrack);
225 bool isMinimumOverFlightDistanceReached = _distanceAircraftTargetMetre < _rnav->overflightDistanceM();
226 bool isOverFlightConeArmed = _distanceAircraftTargetMetre < ( _rnav->overflightArmDistanceM() + _rnav->overflightDistanceM() );
227 bool leavingOverFlightCone = (fabs(_courseAircraftToTarget - _targetTrack) > _rnav->overflightArmAngleDeg());
229 if( isMinimumOverFlightDistanceReached ){
233 if( isOverFlightConeArmed ){
235 if ( leavingOverFlightCone ) {
244 virtual double distanceToWayptM() const
246 return _distanceAircraftTargetMetre;
249 virtual double xtrackErrorNm() const
251 return greatCircleCrossTrackError(_distanceOriginAircraftMetre * SG_METER_TO_NM, _courseDev);
254 virtual bool toFlag() const
259 virtual double courseDeviationDeg() const
264 virtual double trueBearingDeg() const
266 return _courseAircraftToTarget;
269 virtual SGGeod position() const
271 return _waypt->position();
278 * A(from), B(to), D(position) perhaps off course
280 SGGeod _waypointOrigin;
281 double _distanceOriginAircraftMetre;
282 double _distanceAircraftTargetMetre;
283 double _courseOriginToAircraft;
284 double _courseAircraftToTarget;
291 * Special controller for runways. For runways, we want very narrow deviation
292 * constraints, and to understand that any point along the paved area is
293 * equivalent to being 'at' the runway.
295 class RunwayCtl : public WayptController
298 RunwayCtl(RNAV* aRNAV, const WayptRef& aWpt) :
299 WayptController(aRNAV, aWpt),
301 _distanceAircraftRunwayEnd(0.0),
308 _runway = static_cast<RunwayWaypt*>(_waypt.get())->runway();
309 _targetTrack = _runway->headingDeg();
312 virtual void update(double)
314 double bearingAircraftRunwayEnd;
315 // use the far end of the runway for course deviation calculations.
316 // this should do the correct thing both for takeoffs (including entering
317 // the runway at a taxiway after the threshold) and also landings.
318 // seperately compute the distance to the threshold for timeToWaypt calc
320 bearingAircraftRunwayEnd = SGGeodesy::courseDeg(_rnav->position(), _runway->end());
321 _distanceAircraftRunwayEnd = SGGeodesy::distanceM(_rnav->position(), _runway->end());
323 double _courseDev = bearingAircraftRunwayEnd - _targetTrack;
324 SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
326 if ((fabs(_courseDev) > _rnav->overflightArmAngleDeg()) && (_distanceAircraftRunwayEnd < _rnav->overflightArmDistanceM())) {
331 virtual double distanceToWayptM() const
333 return SGGeodesy::distanceM(_rnav->position(), _runway->threshold());
336 virtual double xtrackErrorNm() const
338 double x = sin(_courseDev * SG_DEGREES_TO_RADIANS) * _distanceAircraftRunwayEnd;
339 return x * SG_METER_TO_NM;
342 virtual double courseDeviationDeg() const
347 virtual double trueBearingDeg() const
349 // as in update(), use runway->end here, so the value remains
350 // sensible whether taking off or landing.
351 return SGGeodesy::courseDeg(_rnav->position(), _runway->end());
354 virtual SGGeod position() const
356 return _runway->threshold();
360 double _distanceAircraftRunwayEnd;
364 class ConstHdgToAltCtl : public WayptController
367 ConstHdgToAltCtl(RNAV* aRNAV, const WayptRef& aWpt) :
368 WayptController(aRNAV, aWpt)
371 if (_waypt->type() != "hdgToAlt") {
372 throw sg_exception("invalid waypoint type", "ConstHdgToAltCtl ctor");
375 if (_waypt->altitudeRestriction() == RESTRICT_NONE) {
376 throw sg_exception("invalid waypoint alt restriction", "ConstHdgToAltCtl ctor");
382 HeadingToAltitude* w = (HeadingToAltitude*) _waypt.get();
383 _targetTrack = w->headingDegMagnetic() + _rnav->magvarDeg();
384 _filteredFPM = _lastFPM = _rnav->vspeedFPM();
387 virtual void update(double dt)
389 double curAlt = _rnav->position().getElevationFt();
390 // adjust to get a stable FPM value; bigger values mean slower
391 // response but more stable.
392 const double RESPONSIVENESS = 1.0;
394 _filteredFPM = fgGetLowPass(_lastFPM, _rnav->vspeedFPM(), dt * RESPONSIVENESS);
395 _lastFPM = _rnav->vspeedFPM();
397 switch (_waypt->altitudeRestriction()) {
399 case RESTRICT_COMPUTED:
401 double d = curAlt - _waypt->altitudeFt();
402 if (fabs(d) < 50.0) {
403 SG_LOG(SG_INSTR, SG_INFO, "ConstHdgToAltCtl, reached target altitude " << _waypt->altitudeFt());
409 if (curAlt >= _waypt->altitudeFt()) {
410 SG_LOG(SG_INSTR, SG_INFO, "ConstHdgToAltCtl, above target altitude " << _waypt->altitudeFt());
416 if (curAlt <= _waypt->altitudeFt()) {
417 SG_LOG(SG_INSTR, SG_INFO, "ConstHdgToAltCtl, below target altitude " << _waypt->altitudeFt());
427 virtual double timeToWaypt() const
429 double d = fabs(_rnav->position().getElevationFt() - _waypt->altitudeFt());
430 return (d / _filteredFPM) * 60.0;
433 virtual double distanceToWayptM() const
435 // we could filter ground speed here, but it's likely stable enough,
436 // and timeToWaypt already filters the FPM value
437 double gsMsec = _rnav->groundSpeedKts() * SG_KT_TO_MPS;
438 return timeToWaypt() * gsMsec;
441 virtual SGGeod position() const
445 SGGeodesy::direct(_rnav->position(), _targetTrack, distanceToWayptM(), p, az2);
450 double _lastFPM, _filteredFPM;
453 class InterceptCtl : public WayptController
456 InterceptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
457 WayptController(aRNAV, aWpt),
460 if (_waypt->type() != "radialIntercept") {
461 throw sg_exception("invalid waypoint type", "InterceptCtl ctor");
467 RadialIntercept* w = (RadialIntercept*) _waypt.get();
468 _trueRadial = w->radialDegMagnetic() + _rnav->magvarDeg();
469 _targetTrack = w->courseDegMagnetic() + _rnav->magvarDeg();
472 virtual void update(double)
475 geocPos = SGGeoc::fromGeod(_rnav->position()),
476 geocWayptPos = SGGeoc::fromGeod(_waypt->position());
478 bool ok = geocRadialIntersection(geocPos, _targetTrack,
479 geocWayptPos, _trueRadial, c);
481 // try with a backwards offset from the waypt pos, in case the
482 // procedure waypt location is too close. (eg, KSFO OCEAN SID)
484 SGGeoc navidAdjusted;
485 SGGeodesy::advanceRadM(geocWayptPos, _trueRadial, SG_NM_TO_METER * -10, navidAdjusted);
487 ok = geocRadialIntersection(geocPos, _targetTrack,
488 navidAdjusted, _trueRadial, c);
490 SG_LOG(SG_INSTR, SG_WARN, "InterceptCtl, bad intersection, skipping waypt");
496 _projectedPosition = SGGeod::fromGeoc(c);
499 // note we want the outbound radial from the waypt, hence the ordering
500 // of arguments to courseDeg
501 double r = SGGeodesy::courseDeg(_waypt->position(), _rnav->position());
502 double bearingDiff = r - _trueRadial;
503 SG_NORMALIZE_RANGE(bearingDiff, -180.0, 180.0);
504 if (fabs(bearingDiff) < 0.5) {
509 virtual double distanceToWayptM() const
511 return SGGeodesy::distanceM(_rnav->position(), position());
514 virtual SGGeod position() const
516 return _projectedPosition;
520 SGGeod _projectedPosition;
523 class DMEInterceptCtl : public WayptController
526 DMEInterceptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
527 WayptController(aRNAV, aWpt),
531 if (_waypt->type() != "dmeIntercept") {
532 throw sg_exception("invalid waypoint type", "DMEInterceptCtl ctor");
538 _dme = (DMEIntercept*) _waypt.get();
539 _targetTrack = _dme->courseDegMagnetic() + _rnav->magvarDeg();
542 virtual void update(double)
544 _distanceNm = SGGeodesy::distanceNm(_rnav->position(), _dme->position());
545 double d = fabs(_distanceNm - _dme->dmeDistanceNm());
551 virtual double distanceToWayptM() const
553 return fabs(_distanceNm - _dme->dmeDistanceNm()) * SG_NM_TO_METER;
556 virtual SGGeod position() const
558 SGGeoc geocPos = SGGeoc::fromGeod(_rnav->position());
559 SGGeoc navid = SGGeoc::fromGeod(_dme->position());
560 double distRad = _dme->dmeDistanceNm() * SG_NM_TO_RAD;
562 // compute radial GC course
564 SGGeodesy::advanceRadM(geocPos, _targetTrack, 100 * SG_NM_TO_RAD, bPt);
566 double dNm = pointsKnownDistanceFromGC(geocPos, bPt, navid, distRad);
568 SG_LOG(SG_AUTOPILOT, SG_WARN, "DMEInterceptCtl::position failed");
569 return _dme->position(); // horrible fallback
572 return SGGeodesy::direct(_rnav->position(), _targetTrack, dNm * SG_NM_TO_METER);
580 class HoldCtl : public WayptController
583 HoldCtl(RNAV* aRNAV, const WayptRef& aWpt) :
584 WayptController(aRNAV, aWpt)
594 virtual void update(double)
596 // fly inbound / outbound sides, or execute the turn
600 targetTrack += dt * turnRateSec * turnDirection;
602 if .. targetTrack has passed inbound radial, doen with this turn
604 if target track has passed reciprocal radial done with turn
607 check time / distance elapsed
612 nextHeading = inbound;
614 nextHeading += 180.0;
615 SG_NORMALIZE_RANGE(nextHeading, 0.0, 360.0);
625 virtual double distanceToWayptM() const
630 virtual SGGeod position() const
632 return _waypt->position();
636 class VectorsCtl : public WayptController
639 VectorsCtl(RNAV* aRNAV, const WayptRef& aWpt) :
640 WayptController(aRNAV, aWpt)
650 virtual void update(double)
655 virtual double distanceToWayptM() const
660 virtual SGGeod position() const
662 return _waypt->position();
668 ///////////////////////////////////////////////////////////////////////////////
670 DirectToController::DirectToController(RNAV* aRNAV, const WayptRef& aWpt, const SGGeod& aOrigin) :
671 WayptController(aRNAV, aWpt),
673 _distanceAircraftTargetMeter(0.0),
675 _courseAircraftToTarget(0.0)
679 void DirectToController::init()
681 if (_waypt->flag(WPT_DYNAMIC)) {
682 throw sg_exception("can't direct-to a dynamic waypoint");
685 _targetTrack = SGGeodesy::courseDeg(_origin, _waypt->position());
688 void DirectToController::update(double)
690 double courseOriginToAircraft;
692 courseOriginToAircraft = SGGeodesy::courseDeg(_origin,_rnav->position());
694 _courseAircraftToTarget = SGGeodesy::courseDeg(_rnav->position(),_waypt->position());
695 _distanceAircraftTargetMeter = SGGeodesy::distanceM(_rnav->position(),_waypt->position());
697 _courseDev = -(courseOriginToAircraft - _targetTrack);
699 SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
701 bool isMinimumOverFlightDistanceReached = _distanceAircraftTargetMeter < _rnav->overflightDistanceM();
702 bool isOverFlightConeArmed = _distanceAircraftTargetMeter < ( _rnav->overflightArmDistanceM() + _rnav->overflightDistanceM() );
703 bool leavingOverFlightCone = (fabs(_courseAircraftToTarget - _targetTrack) > _rnav->overflightArmAngleDeg());
705 if( isMinimumOverFlightDistanceReached ){
708 if( isOverFlightConeArmed && leavingOverFlightCone ){
714 double DirectToController::distanceToWayptM() const
716 return _distanceAircraftTargetMeter;
719 double DirectToController::xtrackErrorNm() const
721 return greatCircleCrossTrackError(_distanceAircraftTargetMeter * SG_METER_TO_NM, _courseDev);
724 double DirectToController::courseDeviationDeg() const
729 double DirectToController::trueBearingDeg() const
731 return _courseAircraftToTarget;
734 SGGeod DirectToController::position() const
736 return _waypt->position();
739 ///////////////////////////////////////////////////////////////////////////////
741 OBSController::OBSController(RNAV* aRNAV, const WayptRef& aWpt) :
742 WayptController(aRNAV, aWpt),
743 _distanceAircraftTargetMeter(0.0),
745 _courseAircraftToTarget(0.0)
749 void OBSController::init()
751 if (_waypt->flag(WPT_DYNAMIC)) {
752 throw sg_exception("can't use a dynamic waypoint for OBS mode");
755 _targetTrack = _rnav->selectedMagCourse() + _rnav->magvarDeg();
758 void OBSController::update(double)
760 _targetTrack = _rnav->selectedMagCourse() + _rnav->magvarDeg();
762 _courseAircraftToTarget = SGGeodesy::courseDeg(_rnav->position(),_waypt->position());
763 _distanceAircraftTargetMeter = SGGeodesy::distanceM(_rnav->position(),_waypt->position());
765 // _courseDev inverted we use point target as origin
766 _courseDev = (_courseAircraftToTarget - _targetTrack);
767 SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
770 bool OBSController::toFlag() const
772 return (fabs(_courseDev) < _rnav->overflightArmAngleDeg());
775 double OBSController::distanceToWayptM() const
777 return _distanceAircraftTargetMeter;
780 double OBSController::xtrackErrorNm() const
782 return greatCircleCrossTrackError(_distanceAircraftTargetMeter * SG_METER_TO_NM, _courseDev);
785 double OBSController::courseDeviationDeg() const
787 // if (fabs(_courseDev) > 90.0) {
788 // double d = -_courseDev;
789 // SG_NORMALIZE_RANGE(d, -90.0, 90.0);
796 double OBSController::trueBearingDeg() const
798 return _courseAircraftToTarget;
801 SGGeod OBSController::position() const
803 return _waypt->position();
806 ///////////////////////////////////////////////////////////////////////////////
808 WayptController* WayptController::createForWaypt(RNAV* aRNAV, const WayptRef& aWpt)
811 throw sg_exception("Passed null waypt", "WayptController::createForWaypt");
814 const std::string& wty(aWpt->type());
815 if (wty == "runway") {
816 return new RunwayCtl(aRNAV, aWpt);
819 if (wty == "radialIntercept") {
820 return new InterceptCtl(aRNAV, aWpt);
823 if (wty == "dmeIntercept") {
824 return new DMEInterceptCtl(aRNAV, aWpt);
827 if (wty == "hdgToAlt") {
828 return new ConstHdgToAltCtl(aRNAV, aWpt);
831 if (wty == "vectors") {
832 return new VectorsCtl(aRNAV, aWpt);
836 return new HoldCtl(aRNAV, aWpt);
839 return new LegWayptCtl(aRNAV, aWpt);
842 } // of namespace flightgear