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>
32 const double KNOTS_TO_METRES_PER_SECOND = SG_NM_TO_METER / 3600.0;
34 double pmod(double x, double y)
44 // http://williams.best.vwh.net/avform.htm#Intersection
45 bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result)
47 double crs13 = r1 * SG_DEGREES_TO_RADIANS;
48 double crs23 = r2 * SG_DEGREES_TO_RADIANS;
49 double dst12 = SGGeodesy::distanceRad(a, b);
52 // crs12=acos((sin(lat2)-sin(lat1)*cos(dst12))/(sin(dst12)*cos(lat1)))
53 // crs21=2.*pi-acos((sin(lat1)-sin(lat2)*cos(dst12))/(sin(dst12)*cos(lat2)))
55 // crs12=2.*pi-acos((sin(lat2)-sin(lat1)*cos(dst12))/(sin(dst12)*cos(lat1)))
56 // crs21=acos((sin(lat1)-sin(lat2)*cos(dst12))/(sin(dst12)*cos(lat2)))
60 // double diffLon = b.getLongitudeRad() - a.getLongitudeRad();
62 double sinLat1 = sin(a.getLatitudeRad());
63 double cosLat1 = cos(a.getLatitudeRad());
64 // double sinLat2 = sin(b.getLatitudeRad());
65 //double cosLat2 = cos(b.getLatitudeRad());
66 double sinDst12 = sin(dst12);
67 double cosDst12 = cos(dst12);
69 double crs12 = SGGeodesy::courseRad(a, b),
70 crs21 = SGGeodesy::courseRad(b, a);
72 //double degCrs12 = crs12 * SG_RADIANS_TO_DEGREES;
73 //double degCrs21 = crs21 * SG_RADIANS_TO_DEGREES;
76 if (sin(diffLon) < 0.0) {
77 crs12 = acos((sinLat2 - sinLat1 * cosDst12) / (sinDst12 * cosLat1));
78 crs21 = SGMiscd::twopi() - acos((sinLat1 - sinLat2*cosDst12)/(sinDst12*cosLat2));
80 crs12 = SGMiscd::twopi() - acos((sinLat2 - sinLat1 * cosDst12)/(sinDst12 * cosLat1));
81 crs21 = acos((sinLat1 - sinLat2 * cosDst12)/(sinDst12 * cosLat2));
85 double ang1 = SGMiscd::normalizeAngle2(crs13-crs12);
86 double ang2 = SGMiscd::normalizeAngle2(crs21-crs23);
88 if ((sin(ang1) == 0.0) && (sin(ang2) == 0.0)) {
89 SG_LOG(SG_GENERAL, SG_WARN, "geocRadialIntersection: infinity of intersections");
93 if ((sin(ang1)*sin(ang2))<0.0) {
94 SG_LOG(SG_GENERAL, SG_WARN, "geocRadialIntersection: intersection ambiguous");
101 //ang3=acos(-cos(ang1)*cos(ang2)+sin(ang1)*sin(ang2)*cos(dst12))
102 //dst13=atan2(sin(dst12)*sin(ang1)*sin(ang2),cos(ang2)+cos(ang1)*cos(ang3))
103 //lat3=asin(sin(lat1)*cos(dst13)+cos(lat1)*sin(dst13)*cos(crs13))
105 //lon3=mod(lon1-dlon+pi,2*pi)-pi
107 double ang3 = acos(-cos(ang1) * cos(ang2) + sin(ang1) * sin(ang2) * cosDst12);
108 double dst13 = atan2(sinDst12 * sin(ang1) * sin(ang2), cos(ang2) + cos(ang1)*cos(ang3));
111 SGGeodesy::advanceRadM(a, crs13, dst13 * SG_RAD_TO_NM * SG_NM_TO_METER, pt3);
113 double lat3 = asin(sinLat1 * cos(dst13) + cosLat1 * sin(dst13) * cos(crs13));
115 //dlon=atan2(sin(crs13)*sin(dst13)*cos(lat1),cos(dst13)-sin(lat1)*sin(lat3))
116 double dlon = atan2(sin(crs13)*sin(dst13)*cosLat1, cos(dst13)- (sinLat1 * sin(lat3)));
117 double lon3 = SGMiscd::normalizeAngle(-a.getLongitudeRad()-dlon);
119 result = SGGeoc::fromRadM(-lon3, lat3, a.getRadiusM());
124 ////////////////////////////////////////////////////////////////////////////
126 WayptController::~WayptController()
130 void WayptController::init()
134 void WayptController::setDone()
137 SG_LOG(SG_AUTOPILOT, SG_WARN, "already done @ WayptController::setDone");
143 double WayptController::timeToWaypt() const
145 double gs = _rnav->groundSpeedKts();
147 return -1.0; // stationary
150 gs*= KNOTS_TO_METRES_PER_SECOND;
151 return (distanceToWayptM() / gs);
156 class BasicWayptCtl : public WayptController
159 BasicWayptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
160 WayptController(aRNAV, aWpt),
164 if (aWpt->flag(WPT_DYNAMIC)) {
165 throw sg_exception("BasicWayptCtrl doesn't work with dynamic waypoints");
171 _targetTrack = SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
174 virtual void update()
177 SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM);
178 _courseDev = brg - _targetTrack;
179 SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
181 if ((fabs(_courseDev) > 90.0) && (_distanceM < _rnav->overflightArmDistanceM())) {
186 virtual double distanceToWayptM() const
191 virtual double xtrackErrorNm() const
193 double x = sin(courseDeviationDeg() * SG_DEGREES_TO_RADIANS) * _distanceM;
194 return x * SG_METER_TO_NM;
197 virtual bool toFlag() const
199 return (fabs(_courseDev) < 90.0);
202 virtual double courseDeviationDeg() const
207 virtual double trueBearingDeg() const
209 return SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
212 virtual SGGeod position() const
214 return _waypt->position();
223 * Special controller for runways. For runways, we want very narrow deviation
224 * constraints, and to understand that any point along the paved area is
225 * equivalent to being 'at' the runway.
227 class RunwayCtl : public WayptController
230 RunwayCtl(RNAV* aRNAV, const WayptRef& aWpt) :
231 WayptController(aRNAV, aWpt),
240 _runway = static_cast<RunwayWaypt*>(_waypt.get())->runway();
241 _targetTrack = _runway->headingDeg();
244 virtual void update()
247 // use the far end of the runway for course deviation calculations.
248 // this should do the correct thing both for takeoffs (including entering
249 // the runway at a taxiway after the threshold) and also landings.
250 // seperately compute the distance to the threshold for timeToWaypt calc
251 SGGeodesy::inverse(_rnav->position(), _runway->end(), brg, az2, _distanceM);
252 double _courseDev = brg - _targetTrack;
253 SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
255 if ((fabs(_courseDev) > 90.0) && (_distanceM < _rnav->overflightArmDistanceM())) {
260 virtual double distanceToWayptM() const
262 return SGGeodesy::distanceM(_rnav->position(), _runway->threshold());
265 virtual double xtrackErrorNm() const
267 double x = sin(_courseDev * SG_RADIANS_TO_DEGREES) * _distanceM;
268 return x * SG_METER_TO_NM;
271 virtual double courseDeviationDeg() const
276 virtual double trueBearingDeg() const
278 // as in update(), use runway->end here, so the value remains
279 // sensible whether taking off or landing.
280 return SGGeodesy::courseDeg(_rnav->position(), _runway->end());
283 virtual SGGeod position() const
285 return _runway->threshold();
293 class ConstHdgToAltCtl : public WayptController
296 ConstHdgToAltCtl(RNAV* aRNAV, const WayptRef& aWpt) :
297 WayptController(aRNAV, aWpt)
300 if (_waypt->type() != "hdgToAlt") {
301 throw sg_exception("invalid waypoint type", "ConstHdgToAltCtl ctor");
304 if (_waypt->altitudeRestriction() == RESTRICT_NONE) {
305 throw sg_exception("invalid waypoint alt restriction", "ConstHdgToAltCtl ctor");
311 HeadingToAltitude* w = (HeadingToAltitude*) _waypt.get();
312 _targetTrack = w->headingDegMagnetic() + _rnav->magvarDeg();
315 virtual void update()
317 double curAlt = _rnav->position().getElevationFt();
319 switch (_waypt->altitudeRestriction()) {
321 double d = curAlt - _waypt->altitudeFt();
322 if (fabs(d) < 50.0) {
323 SG_LOG(SG_GENERAL, SG_INFO, "ConstHdgToAltCtl, reached target altitude " << _waypt->altitudeFt());
329 if (curAlt >= _waypt->altitudeFt()) {
330 SG_LOG(SG_GENERAL, SG_INFO, "ConstHdgToAltCtl, above target altitude " << _waypt->altitudeFt());
336 if (curAlt <= _waypt->altitudeFt()) {
337 SG_LOG(SG_GENERAL, SG_INFO, "ConstHdgToAltCtl, below target altitude " << _waypt->altitudeFt());
348 virtual double timeToWaypt() const
350 double d = fabs(_rnav->position().getElevationFt() - _waypt->altitudeFt());
351 return (d / _rnav->vspeedFPM()) * 60.0; // low pass filter here, probably
354 virtual double distanceToWayptM() const
356 double gsMsec = _rnav->groundSpeedKts() * KNOTS_TO_METRES_PER_SECOND;
357 return timeToWaypt() * gsMsec;
360 virtual SGGeod position() const
364 SGGeodesy::direct(_rnav->position(), _targetTrack, distanceToWayptM(), p, az2);
369 class InterceptCtl : public WayptController
372 InterceptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
373 WayptController(aRNAV, aWpt),
376 if (_waypt->type() != "radialIntercept") {
377 throw sg_exception("invalid waypoint type", "InterceptCtl ctor");
383 RadialIntercept* w = (RadialIntercept*) _waypt.get();
384 _trueRadial = w->radialDegMagnetic() + _rnav->magvarDeg();
385 _targetTrack = w->courseDegMagnetic() + _rnav->magvarDeg();
388 virtual void update()
390 // note we want the outbound radial from the waypt, hence the ordering
391 // of arguments to courseDeg
392 double r = SGGeodesy::courseDeg(_waypt->position(), _rnav->position());
393 SG_LOG(SG_AUTOPILOT, SG_INFO, "current radial=" << r);
394 if (fabs(r - _trueRadial) < 0.5) {
395 SG_LOG(SG_GENERAL, SG_INFO, "InterceptCtl, intercepted radial " << _trueRadial);
400 virtual double distanceToWayptM() const
402 return SGGeodesy::distanceM(_rnav->position(), position());
405 virtual SGGeod position() const
408 geocRadialIntersection(SGGeoc::fromGeod(_rnav->position()), _rnav->trackDeg(),
409 SGGeoc::fromGeod(_waypt->position()), _trueRadial, c);
410 return SGGeod::fromGeoc(c);
416 class DMEInterceptCtl : public WayptController
419 DMEInterceptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
420 WayptController(aRNAV, aWpt),
424 if (_waypt->type() != "dmeIntercept") {
425 throw sg_exception("invalid waypoint type", "DMEInterceptCtl ctor");
431 _dme = (DMEIntercept*) _waypt.get();
432 _targetTrack = _dme->courseDegMagnetic() + _rnav->magvarDeg();
435 virtual void update()
437 _distanceNm = SGGeodesy::distanceNm(_rnav->position(), _dme->position());
438 double d = fabs(_distanceNm - _dme->dmeDistanceNm());
440 SG_LOG(SG_GENERAL, SG_INFO, "DMEInterceptCtl, intercepted DME " << _dme->dmeDistanceNm());
445 virtual double distanceToWayptM() const
447 return fabs(_distanceNm - _dme->dmeDistanceNm()) * SG_NM_TO_METER;
450 virtual SGGeod position() const
454 SGGeodesy::direct(_rnav->position(), _targetTrack, distanceToWayptM(), p, az2);
463 class HoldCtl : public WayptController
466 HoldCtl(RNAV* aRNAV, const WayptRef& aWpt) :
467 WayptController(aRNAV, aWpt)
477 virtual void update()
479 // fly inbound / outbound sides, or execute the turn
483 targetTrack += dt * turnRateSec * turnDirection;
485 if .. targetTrack has passed inbound radial, doen with this turn
487 if target track has passed reciprocal radial done with turn
490 check time / distance elapsed
495 nextHeading = inbound;
497 nextHeading += 180.0;
498 SG_NORMALIZE_RANGE(nextHeading, 0.0, 360.0);
508 virtual double distanceToWayptM() const
513 virtual SGGeod position() const
515 return _waypt->position();
519 class VectorsCtl : public WayptController
522 VectorsCtl(RNAV* aRNAV, const WayptRef& aWpt) :
523 WayptController(aRNAV, aWpt)
533 virtual void update()
538 virtual double distanceToWayptM() const
543 virtual SGGeod position() const
545 return _waypt->position();
551 ///////////////////////////////////////////////////////////////////////////////
553 DirectToController::DirectToController(RNAV* aRNAV, const WayptRef& aWpt, const SGGeod& aOrigin) :
554 WayptController(aRNAV, aWpt),
561 void DirectToController::init()
563 if (_waypt->flag(WPT_DYNAMIC)) {
564 throw sg_exception("can't direct-to a dynamic waypoint");
567 _targetTrack = SGGeodesy::courseDeg(_origin, _waypt->position());
570 void DirectToController::update()
573 SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM);
574 _courseDev = brg - _targetTrack;
575 SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
577 if ((fabs(_courseDev) > 90.0) && (_distanceM < _rnav->overflightArmDistanceM())) {
582 double DirectToController::distanceToWayptM() const
587 double DirectToController::xtrackErrorNm() const
589 double x = sin(courseDeviationDeg() * SG_DEGREES_TO_RADIANS) * _distanceM;
590 return x * SG_METER_TO_NM;
593 double DirectToController::courseDeviationDeg() const
598 double DirectToController::trueBearingDeg() const
600 return SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
603 SGGeod DirectToController::position() const
605 return _waypt->position();
608 ///////////////////////////////////////////////////////////////////////////////
610 OBSController::OBSController(RNAV* aRNAV, const WayptRef& aWpt) :
611 WayptController(aRNAV, aWpt),
617 void OBSController::init()
619 if (_waypt->flag(WPT_DYNAMIC)) {
620 throw sg_exception("can't use a dynamic waypoint for OBS mode");
623 _targetTrack = _rnav->selectedMagCourse() + _rnav->magvarDeg();
626 void OBSController::update()
628 _targetTrack = _rnav->selectedMagCourse() + _rnav->magvarDeg();
630 SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM);
631 _courseDev = brg - _targetTrack;
632 SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
635 bool OBSController::toFlag() const
637 return (fabs(_courseDev) < 90.0);
640 double OBSController::distanceToWayptM() const
645 double OBSController::xtrackErrorNm() const
647 double x = sin(_courseDev * SG_DEGREES_TO_RADIANS) * _distanceM;
648 return x * SG_METER_TO_NM;
651 double OBSController::courseDeviationDeg() const
653 // if (fabs(_courseDev) > 90.0) {
654 // double d = -_courseDev;
655 // SG_NORMALIZE_RANGE(d, -90.0, 90.0);
662 double OBSController::trueBearingDeg() const
664 return SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
667 SGGeod OBSController::position() const
669 return _waypt->position();
672 ///////////////////////////////////////////////////////////////////////////////
674 WayptController* WayptController::createForWaypt(RNAV* aRNAV, const WayptRef& aWpt)
677 throw sg_exception("Passed null waypt", "WayptController::createForWaypt");
680 const std::string& wty(aWpt->type());
681 if (wty == "runway") {
682 return new RunwayCtl(aRNAV, aWpt);
685 if (wty == "radialIntercept") {
686 return new InterceptCtl(aRNAV, aWpt);
689 if (wty == "dmeIntercept") {
690 return new DMEInterceptCtl(aRNAV, aWpt);
693 if (wty == "hdgToAlt") {
694 return new ConstHdgToAltCtl(aRNAV, aWpt);
697 if (wty == "vectors") {
698 return new VectorsCtl(aRNAV, aWpt);
702 return new HoldCtl(aRNAV, aWpt);
705 return new BasicWayptCtl(aRNAV, aWpt);
708 } // of namespace flightgear