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_INSTR, SG_WARN, "geocRadialIntersection: infinity of intersections");
93 if ((sin(ang1)*sin(ang2))<0.0) {
94 SG_LOG(SG_INSTR, 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_INSTR, SG_INFO, "ConstHdgToAltCtl, reached target altitude " << _waypt->altitudeFt());
329 if (curAlt >= _waypt->altitudeFt()) {
330 SG_LOG(SG_INSTR, SG_INFO, "ConstHdgToAltCtl, above target altitude " << _waypt->altitudeFt());
336 if (curAlt <= _waypt->altitudeFt()) {
337 SG_LOG(SG_INSTR, SG_INFO, "ConstHdgToAltCtl, below target altitude " << _waypt->altitudeFt());
345 case SPEED_RESTRICT_MACH:
351 virtual double timeToWaypt() const
353 double d = fabs(_rnav->position().getElevationFt() - _waypt->altitudeFt());
354 return (d / _rnav->vspeedFPM()) * 60.0; // low pass filter here, probably
357 virtual double distanceToWayptM() const
359 double gsMsec = _rnav->groundSpeedKts() * KNOTS_TO_METRES_PER_SECOND;
360 return timeToWaypt() * gsMsec;
363 virtual SGGeod position() const
367 SGGeodesy::direct(_rnav->position(), _targetTrack, distanceToWayptM(), p, az2);
372 class InterceptCtl : public WayptController
375 InterceptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
376 WayptController(aRNAV, aWpt),
379 if (_waypt->type() != "radialIntercept") {
380 throw sg_exception("invalid waypoint type", "InterceptCtl ctor");
386 RadialIntercept* w = (RadialIntercept*) _waypt.get();
387 _trueRadial = w->radialDegMagnetic() + _rnav->magvarDeg();
388 _targetTrack = w->courseDegMagnetic() + _rnav->magvarDeg();
391 virtual void update()
393 // note we want the outbound radial from the waypt, hence the ordering
394 // of arguments to courseDeg
395 double r = SGGeodesy::courseDeg(_waypt->position(), _rnav->position());
396 SG_LOG(SG_AUTOPILOT, SG_INFO, "current radial=" << r);
397 if (fabs(r - _trueRadial) < 0.5) {
398 SG_LOG(SG_INSTR, SG_INFO, "InterceptCtl, intercepted radial " << _trueRadial);
403 virtual double distanceToWayptM() const
405 return SGGeodesy::distanceM(_rnav->position(), position());
408 virtual SGGeod position() const
411 geocRadialIntersection(SGGeoc::fromGeod(_rnav->position()), _rnav->trackDeg(),
412 SGGeoc::fromGeod(_waypt->position()), _trueRadial, c);
413 return SGGeod::fromGeoc(c);
419 class DMEInterceptCtl : public WayptController
422 DMEInterceptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
423 WayptController(aRNAV, aWpt),
427 if (_waypt->type() != "dmeIntercept") {
428 throw sg_exception("invalid waypoint type", "DMEInterceptCtl ctor");
434 _dme = (DMEIntercept*) _waypt.get();
435 _targetTrack = _dme->courseDegMagnetic() + _rnav->magvarDeg();
438 virtual void update()
440 _distanceNm = SGGeodesy::distanceNm(_rnav->position(), _dme->position());
441 double d = fabs(_distanceNm - _dme->dmeDistanceNm());
443 SG_LOG(SG_INSTR, SG_INFO, "DMEInterceptCtl, intercepted DME " << _dme->dmeDistanceNm());
448 virtual double distanceToWayptM() const
450 return fabs(_distanceNm - _dme->dmeDistanceNm()) * SG_NM_TO_METER;
453 virtual SGGeod position() const
457 SGGeodesy::direct(_rnav->position(), _targetTrack, distanceToWayptM(), p, az2);
466 class HoldCtl : public WayptController
469 HoldCtl(RNAV* aRNAV, const WayptRef& aWpt) :
470 WayptController(aRNAV, aWpt)
480 virtual void update()
482 // fly inbound / outbound sides, or execute the turn
486 targetTrack += dt * turnRateSec * turnDirection;
488 if .. targetTrack has passed inbound radial, doen with this turn
490 if target track has passed reciprocal radial done with turn
493 check time / distance elapsed
498 nextHeading = inbound;
500 nextHeading += 180.0;
501 SG_NORMALIZE_RANGE(nextHeading, 0.0, 360.0);
511 virtual double distanceToWayptM() const
516 virtual SGGeod position() const
518 return _waypt->position();
522 class VectorsCtl : public WayptController
525 VectorsCtl(RNAV* aRNAV, const WayptRef& aWpt) :
526 WayptController(aRNAV, aWpt)
536 virtual void update()
541 virtual double distanceToWayptM() const
546 virtual SGGeod position() const
548 return _waypt->position();
554 ///////////////////////////////////////////////////////////////////////////////
556 DirectToController::DirectToController(RNAV* aRNAV, const WayptRef& aWpt, const SGGeod& aOrigin) :
557 WayptController(aRNAV, aWpt),
564 void DirectToController::init()
566 if (_waypt->flag(WPT_DYNAMIC)) {
567 throw sg_exception("can't direct-to a dynamic waypoint");
570 _targetTrack = SGGeodesy::courseDeg(_origin, _waypt->position());
573 void DirectToController::update()
576 SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM);
577 _courseDev = brg - _targetTrack;
578 SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
580 if ((fabs(_courseDev) > 90.0) && (_distanceM < _rnav->overflightArmDistanceM())) {
585 double DirectToController::distanceToWayptM() const
590 double DirectToController::xtrackErrorNm() const
592 double x = sin(courseDeviationDeg() * SG_DEGREES_TO_RADIANS) * _distanceM;
593 return x * SG_METER_TO_NM;
596 double DirectToController::courseDeviationDeg() const
601 double DirectToController::trueBearingDeg() const
603 return SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
606 SGGeod DirectToController::position() const
608 return _waypt->position();
611 ///////////////////////////////////////////////////////////////////////////////
613 OBSController::OBSController(RNAV* aRNAV, const WayptRef& aWpt) :
614 WayptController(aRNAV, aWpt),
620 void OBSController::init()
622 if (_waypt->flag(WPT_DYNAMIC)) {
623 throw sg_exception("can't use a dynamic waypoint for OBS mode");
626 _targetTrack = _rnav->selectedMagCourse() + _rnav->magvarDeg();
629 void OBSController::update()
631 _targetTrack = _rnav->selectedMagCourse() + _rnav->magvarDeg();
633 SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM);
634 _courseDev = brg - _targetTrack;
635 SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
638 bool OBSController::toFlag() const
640 return (fabs(_courseDev) < 90.0);
643 double OBSController::distanceToWayptM() const
648 double OBSController::xtrackErrorNm() const
650 double x = sin(_courseDev * SG_DEGREES_TO_RADIANS) * _distanceM;
651 return x * SG_METER_TO_NM;
654 double OBSController::courseDeviationDeg() const
656 // if (fabs(_courseDev) > 90.0) {
657 // double d = -_courseDev;
658 // SG_NORMALIZE_RANGE(d, -90.0, 90.0);
665 double OBSController::trueBearingDeg() const
667 return SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
670 SGGeod OBSController::position() const
672 return _waypt->position();
675 ///////////////////////////////////////////////////////////////////////////////
677 WayptController* WayptController::createForWaypt(RNAV* aRNAV, const WayptRef& aWpt)
680 throw sg_exception("Passed null waypt", "WayptController::createForWaypt");
683 const std::string& wty(aWpt->type());
684 if (wty == "runway") {
685 return new RunwayCtl(aRNAV, aWpt);
688 if (wty == "radialIntercept") {
689 return new InterceptCtl(aRNAV, aWpt);
692 if (wty == "dmeIntercept") {
693 return new DMEInterceptCtl(aRNAV, aWpt);
696 if (wty == "hdgToAlt") {
697 return new ConstHdgToAltCtl(aRNAV, aWpt);
700 if (wty == "vectors") {
701 return new VectorsCtl(aRNAV, aWpt);
705 return new HoldCtl(aRNAV, aWpt);
708 return new BasicWayptCtl(aRNAV, aWpt);
711 } // of namespace flightgear