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)
162 if (aWpt->flag(WPT_DYNAMIC)) {
163 throw sg_exception("BasicWayptCtrl doesn't work with dynamic waypoints");
169 _targetTrack = SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
172 virtual void update()
175 SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM);
176 _courseDev = brg - _targetTrack;
177 SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
179 if ((fabs(_courseDev) > 90.0) && (_distanceM < _rnav->overflightArmDistanceM())) {
184 virtual double distanceToWayptM() const
189 virtual double xtrackErrorNm() const
191 double x = sin(courseDeviationDeg() * SG_DEGREES_TO_RADIANS) * _distanceM;
192 return x * SG_METER_TO_NM;
195 virtual bool toFlag() const
197 return (fabs(_courseDev) < 90.0);
200 virtual double courseDeviationDeg() const
205 virtual double trueBearingDeg() const
207 return SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
210 virtual SGGeod position() const
212 return _waypt->position();
221 * Special controller for runways. For runways, we want very narrow deviation
222 * contraints, and to understand that any point along the paved area is
223 * equivalent to being 'at' the runway.
225 class RunwayCtl : public WayptController
228 RunwayCtl(RNAV* aRNAV, const WayptRef& aWpt) :
229 WayptController(aRNAV, aWpt)
235 _runway = static_cast<RunwayWaypt*>(_waypt.get())->runway();
236 _targetTrack = _runway->headingDeg();
239 virtual void update()
242 // use the far end of the runway for course deviation calculations.
243 // this should do the correct thing both for takeoffs (including entering
244 // the runway at a taxiway after the threshold) and also landings.
245 // seperately compute the distance to the threshold for timeToWaypt calc
246 SGGeodesy::inverse(_rnav->position(), _runway->end(), brg, az2, _distanceM);
247 double _courseDev = brg - _targetTrack;
248 SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
250 if (fabs(_courseDev) > 90.0) {
255 virtual double distanceToWayptM() const
257 return SGGeodesy::distanceM(_rnav->position(), _runway->threshold());
260 virtual double xtrackErrorNm() const
262 double x = sin(_courseDev * SG_RADIANS_TO_DEGREES) * _distanceM;
263 return x * SG_METER_TO_NM;
266 virtual double courseDeviationDeg() const
271 virtual double trueBearingDeg() const
273 // as in update(), use runway->end here, so the value remains
274 // sensible whether taking off or landing.
275 return SGGeodesy::courseDeg(_rnav->position(), _runway->end());
278 virtual SGGeod position() const
280 return _runway->threshold();
288 class ConstHdgToAltCtl : public WayptController
291 ConstHdgToAltCtl(RNAV* aRNAV, const WayptRef& aWpt) :
292 WayptController(aRNAV, aWpt)
295 if (_waypt->type() != "hdgToAlt") {
296 throw sg_exception("invalid waypoint type", "ConstHdgToAltCtl ctor");
299 if (_waypt->altitudeRestriction() == RESTRICT_NONE) {
300 throw sg_exception("invalid waypoint alt restriction", "ConstHdgToAltCtl ctor");
306 HeadingToAltitude* w = (HeadingToAltitude*) _waypt.get();
307 _targetTrack = w->headingDegMagnetic() + _rnav->magvarDeg();
310 virtual void update()
312 double curAlt = _rnav->position().getElevationFt();
314 switch (_waypt->altitudeRestriction()) {
316 double d = curAlt - _waypt->altitudeFt();
317 if (fabs(d) < 50.0) {
318 SG_LOG(SG_GENERAL, SG_INFO, "ConstHdgToAltCtl, reached target altitude " << _waypt->altitudeFt());
324 if (curAlt >= _waypt->altitudeFt()) {
325 SG_LOG(SG_GENERAL, SG_INFO, "ConstHdgToAltCtl, above target altitude " << _waypt->altitudeFt());
331 if (curAlt <= _waypt->altitudeFt()) {
332 SG_LOG(SG_GENERAL, SG_INFO, "ConstHdgToAltCtl, below target altitude " << _waypt->altitudeFt());
343 virtual double timeToWaypt() const
345 double d = fabs(_rnav->position().getElevationFt() - _waypt->altitudeFt());
346 return (d / _rnav->vspeedFPM()) * 60.0; // low pass filter here, probably
349 virtual double distanceToWayptM() const
351 double gsMsec = _rnav->groundSpeedKts() * KNOTS_TO_METRES_PER_SECOND;
352 return timeToWaypt() * gsMsec;
355 virtual SGGeod position() const
359 SGGeodesy::direct(_rnav->position(), _targetTrack, distanceToWayptM(), p, az2);
364 class InterceptCtl : public WayptController
367 InterceptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
368 WayptController(aRNAV, aWpt)
371 if (_waypt->type() != "radialIntercept") {
372 throw sg_exception("invalid waypoint type", "InterceptCtl ctor");
378 RadialIntercept* w = (RadialIntercept*) _waypt.get();
379 _trueRadial = w->radialDegMagnetic() + _rnav->magvarDeg();
380 _targetTrack = w->courseDegMagnetic() + _rnav->magvarDeg();
383 virtual void update()
385 // note we want the outbound radial from the waypt, hence the ordering
386 // of arguments to courseDeg
387 double r = SGGeodesy::courseDeg(_waypt->position(), _rnav->position());
388 SG_LOG(SG_AUTOPILOT, SG_INFO, "current radial=" << r);
389 if (fabs(r - _trueRadial) < 0.5) {
390 SG_LOG(SG_GENERAL, SG_INFO, "InterceptCtl, intercepted radial " << _trueRadial);
395 virtual double distanceToWayptM() const
397 return SGGeodesy::distanceM(_rnav->position(), position());
400 virtual SGGeod position() const
403 geocRadialIntersection(SGGeoc::fromGeod(_rnav->position()), _rnav->trackDeg(),
404 SGGeoc::fromGeod(_waypt->position()), _trueRadial, c);
405 return SGGeod::fromGeoc(c);
411 class DMEInterceptCtl : public WayptController
414 DMEInterceptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
415 WayptController(aRNAV, aWpt)
418 if (_waypt->type() != "dmeIntercept") {
419 throw sg_exception("invalid waypoint type", "DMEInterceptCtl ctor");
425 _dme = (DMEIntercept*) _waypt.get();
426 _targetTrack = _dme->courseDegMagnetic() + _rnav->magvarDeg();
429 virtual void update()
431 _distanceNm = SGGeodesy::distanceNm(_rnav->position(), _dme->position());
432 double d = fabs(_distanceNm - _dme->dmeDistanceNm());
434 SG_LOG(SG_GENERAL, SG_INFO, "DMEInterceptCtl, intercepted DME " << _dme->dmeDistanceNm());
439 virtual double distanceToWayptM() const
441 return fabs(_distanceNm - _dme->dmeDistanceNm()) * SG_NM_TO_METER;
444 virtual SGGeod position() const
448 SGGeodesy::direct(_rnav->position(), _targetTrack, distanceToWayptM(), p, az2);
457 class HoldCtl : public WayptController
460 HoldCtl(RNAV* aRNAV, const WayptRef& aWpt) :
461 WayptController(aRNAV, aWpt)
471 virtual void update()
473 // fly inbound / outbound sides, or execute the turn
477 targetTrack += dt * turnRateSec * turnDirection;
479 if .. targetTrack has passed inbound radial, doen with this turn
481 if target track has passed reciprocal radial done with turn
484 check time / distance elapsed
489 nextHeading = inbound;
491 nextHeading += 180.0;
492 SG_NORMALIZE_RANGE(nextHeading, 0.0, 360.0);
502 virtual double distanceToWayptM() const
507 virtual SGGeod position() const
509 return _waypt->position();
513 class VectorsCtl : public WayptController
516 VectorsCtl(RNAV* aRNAV, const WayptRef& aWpt) :
517 WayptController(aRNAV, aWpt)
527 virtual void update()
532 virtual double distanceToWayptM() const
537 virtual SGGeod position() const
539 return _waypt->position();
545 ///////////////////////////////////////////////////////////////////////////////
547 DirectToController::DirectToController(RNAV* aRNAV, const WayptRef& aWpt, const SGGeod& aOrigin) :
548 WayptController(aRNAV, aWpt),
553 void DirectToController::init()
555 if (_waypt->flag(WPT_DYNAMIC)) {
556 throw sg_exception("can't direct-to a dynamic waypoint");
559 _targetTrack = SGGeodesy::courseDeg(_origin, _waypt->position());
562 void DirectToController::update()
565 SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM);
566 _courseDev = brg - _targetTrack;
567 SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
569 if ((fabs(_courseDev) > 90.0) && (_distanceM < _rnav->overflightArmDistanceM())) {
574 double DirectToController::distanceToWayptM() const
579 double DirectToController::xtrackErrorNm() const
581 double x = sin(courseDeviationDeg() * SG_DEGREES_TO_RADIANS) * _distanceM;
582 return x * SG_METER_TO_NM;
585 double DirectToController::courseDeviationDeg() const
590 double DirectToController::trueBearingDeg() const
592 return SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
595 SGGeod DirectToController::position() const
597 return _waypt->position();
600 ///////////////////////////////////////////////////////////////////////////////
602 OBSController::OBSController(RNAV* aRNAV, const WayptRef& aWpt) :
603 WayptController(aRNAV, aWpt)
607 void OBSController::init()
609 if (_waypt->flag(WPT_DYNAMIC)) {
610 throw sg_exception("can't use a dynamic waypoint for OBS mode");
613 _targetTrack = _rnav->selectedMagCourse() + _rnav->magvarDeg();
616 void OBSController::update()
618 _targetTrack = _rnav->selectedMagCourse() + _rnav->magvarDeg();
620 SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM);
621 _courseDev = brg - _targetTrack;
622 SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
625 bool OBSController::toFlag() const
627 return (fabs(_courseDev) < 90.0);
630 double OBSController::distanceToWayptM() const
635 double OBSController::xtrackErrorNm() const
637 double x = sin(_courseDev * SG_DEGREES_TO_RADIANS) * _distanceM;
638 return x * SG_METER_TO_NM;
641 double OBSController::courseDeviationDeg() const
643 // if (fabs(_courseDev) > 90.0) {
644 // double d = -_courseDev;
645 // SG_NORMALIZE_RANGE(d, -90.0, 90.0);
652 double OBSController::trueBearingDeg() const
654 return SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
657 SGGeod OBSController::position() const
659 return _waypt->position();
662 ///////////////////////////////////////////////////////////////////////////////
664 WayptController* WayptController::createForWaypt(RNAV* aRNAV, const WayptRef& aWpt)
667 throw sg_exception("Passed null waypt", "WayptController::createForWaypt");
670 const std::string& wty(aWpt->type());
671 if (wty == "runway") {
672 return new RunwayCtl(aRNAV, aWpt);
675 if (wty == "radialIntercept") {
676 return new InterceptCtl(aRNAV, aWpt);
679 if (wty == "dmeIntercept") {
680 return new DMEInterceptCtl(aRNAV, aWpt);
683 if (wty == "hdgToAlt") {
684 return new ConstHdgToAltCtl(aRNAV, aWpt);
687 if (wty == "vectors") {
688 return new VectorsCtl(aRNAV, aWpt);
692 return new HoldCtl(aRNAV, aWpt);
695 return new BasicWayptCtl(aRNAV, aWpt);
698 } // of namespace flightgear