X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FInstrumentation%2Frnav_waypt_controller.cxx;h=d487e2b3f85b3b475df19125c2af3d029e30b761;hb=01622dd1f4e990fc588a0eed5cde1c42a1b378e8;hp=977452e1806deb8a255e5b6c6f376ab89430e4af;hpb=308fd1b11216f5d7e4bba350a7d798a33f06e70a;p=flightgear.git diff --git a/src/Instrumentation/rnav_waypt_controller.cxx b/src/Instrumentation/rnav_waypt_controller.cxx index 977452e18..d487e2b3f 100644 --- a/src/Instrumentation/rnav_waypt_controller.cxx +++ b/src/Instrumentation/rnav_waypt_controller.cxx @@ -69,8 +69,8 @@ bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double double crs12 = SGGeodesy::courseRad(a, b), crs21 = SGGeodesy::courseRad(b, a); - double degCrs12 = crs12 * SG_RADIANS_TO_DEGREES; - double degCrs21 = crs21 * SG_RADIANS_TO_DEGREES; + //double degCrs12 = crs12 * SG_RADIANS_TO_DEGREES; + //double degCrs21 = crs21 * SG_RADIANS_TO_DEGREES; /* if (sin(diffLon) < 0.0) { @@ -86,12 +86,12 @@ bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double double ang2 = SGMiscd::normalizeAngle2(crs21-crs23); if ((sin(ang1) == 0.0) && (sin(ang2) == 0.0)) { - SG_LOG(SG_GENERAL, SG_WARN, "geocRadialIntersection: infinity of intersections"); + SG_LOG(SG_INSTR, SG_WARN, "geocRadialIntersection: infinity of intersections"); return false; } if ((sin(ang1)*sin(ang2))<0.0) { - SG_LOG(SG_GENERAL, SG_WARN, "geocRadialIntersection: intersection ambiguous"); + SG_LOG(SG_INSTR, SG_WARN, "geocRadialIntersection: intersection ambiguous"); return false; } @@ -121,6 +121,34 @@ bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double return true; } + +/** + * Helper function Cross track error: + * http://williams.best.vwh.net/avform.htm#XTE + * + * param distanceOriginToPosition in Nautical Miles + * param courseDev difference between courseOriginToAircraft(AD) and courseOriginToTarget(AB) + * return XTE in Nautical Miles + * + * A(origin) + * B(target) + * D(aircraft) perhaps off course + * + * A-->--B + * \ / + * \ / + * D + */ + +double greatCircleCrossTrackError(double distanceOriginToPosition,double courseDev){ + double crossTrackError = asin( + sin(distanceOriginToPosition * SG_NM_TO_RAD) * + sin(courseDev * SG_DEGREES_TO_RADIANS) + ); + return crossTrackError * SG_RAD_TO_NM; +} + + //////////////////////////////////////////////////////////////////////////// WayptController::~WayptController() @@ -157,7 +185,9 @@ class BasicWayptCtl : public WayptController { public: BasicWayptCtl(RNAV* aRNAV, const WayptRef& aWpt) : - WayptController(aRNAV, aWpt) + WayptController(aRNAV, aWpt), + _distanceAircraftTargetMeter(0.0), + _courseDev(0.0) { if (aWpt->flag(WPT_DYNAMIC)) { throw sg_exception("BasicWayptCtrl doesn't work with dynamic waypoints"); @@ -171,30 +201,33 @@ public: virtual void update() { - double brg, az2; - SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM); - _courseDev = brg - _targetTrack; + double bearingAircraftToTarget; + + bearingAircraftToTarget = SGGeodesy::courseDeg(_rnav->position(), _waypt->position()); + _distanceAircraftTargetMeter = SGGeodesy::distanceM(_rnav->position(), _waypt->position()); + + _courseDev = bearingAircraftToTarget - _targetTrack; SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0); - if ((fabs(_courseDev) > 90.0) && (_distanceM < _rnav->overflightArmDistanceM())) { + if ((fabs(_courseDev) > _rnav->overflightArmAngleDeg()) && (_distanceAircraftTargetMeter < _rnav->overflightArmDistanceM())) { setDone(); } } virtual double distanceToWayptM() const { - return _distanceM; + return _distanceAircraftTargetMeter; } virtual double xtrackErrorNm() const { - double x = sin(courseDeviationDeg() * SG_DEGREES_TO_RADIANS) * _distanceM; + double x = sin(courseDeviationDeg() * SG_DEGREES_TO_RADIANS) * _distanceAircraftTargetMeter; return x * SG_METER_TO_NM; } virtual bool toFlag() const { - return (fabs(_courseDev) < 90.0); + return (fabs(_courseDev) < _rnav->overflightArmAngleDeg()); } virtual double courseDeviationDeg() const @@ -213,20 +246,147 @@ public: } private: - double _distanceM; + 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; double _courseDev; + bool _toFlag; + }; /** * Special controller for runways. For runways, we want very narrow deviation - * contraints, and to understand that any point along the paved area is + * constraints, and to understand that any point along the paved area is * equivalent to being 'at' the runway. */ class RunwayCtl : public WayptController { public: RunwayCtl(RNAV* aRNAV, const WayptRef& aWpt) : - WayptController(aRNAV, aWpt) + WayptController(aRNAV, aWpt), + _runway(NULL), + _distanceAircraftRunwayEnd(0.0), + _courseDev(0.0) { } @@ -238,16 +398,19 @@ public: virtual void update() { - double brg, az2; + double bearingAircraftRunwayEnd; // use the far end of the runway for course deviation calculations. // this should do the correct thing both for takeoffs (including entering // the runway at a taxiway after the threshold) and also landings. // seperately compute the distance to the threshold for timeToWaypt calc - SGGeodesy::inverse(_rnav->position(), _runway->end(), brg, az2, _distanceM); - double _courseDev = brg - _targetTrack; + + bearingAircraftRunwayEnd = SGGeodesy::courseDeg(_rnav->position(), _runway->end()); + _distanceAircraftRunwayEnd = SGGeodesy::distanceM(_rnav->position(), _runway->end()); + + double _courseDev = bearingAircraftRunwayEnd - _targetTrack; SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0); - if (fabs(_courseDev) > 90.0) { + if ((fabs(_courseDev) > _rnav->overflightArmAngleDeg()) && (_distanceAircraftRunwayEnd < _rnav->overflightArmDistanceM())) { setDone(); } } @@ -259,7 +422,7 @@ public: virtual double xtrackErrorNm() const { - double x = sin(_courseDev * SG_RADIANS_TO_DEGREES) * _distanceM; + double x = sin(_courseDev * SG_RADIANS_TO_DEGREES) * _distanceAircraftRunwayEnd; return x * SG_METER_TO_NM; } @@ -281,7 +444,7 @@ public: } private: FGRunway* _runway; - double _distanceM; + double _distanceAircraftRunwayEnd; double _courseDev; }; @@ -312,30 +475,31 @@ public: double curAlt = _rnav->position().getElevationFt(); switch (_waypt->altitudeRestriction()) { - case RESTRICT_AT: { + case RESTRICT_AT: + case RESTRICT_COMPUTED: + { double d = curAlt - _waypt->altitudeFt(); if (fabs(d) < 50.0) { - SG_LOG(SG_GENERAL, SG_INFO, "ConstHdgToAltCtl, reached target altitude " << _waypt->altitudeFt()); + SG_LOG(SG_INSTR, SG_INFO, "ConstHdgToAltCtl, reached target altitude " << _waypt->altitudeFt()); setDone(); } } break; case RESTRICT_ABOVE: if (curAlt >= _waypt->altitudeFt()) { - SG_LOG(SG_GENERAL, SG_INFO, "ConstHdgToAltCtl, above target altitude " << _waypt->altitudeFt()); + SG_LOG(SG_INSTR, SG_INFO, "ConstHdgToAltCtl, above target altitude " << _waypt->altitudeFt()); setDone(); } break; case RESTRICT_BELOW: if (curAlt <= _waypt->altitudeFt()) { - SG_LOG(SG_GENERAL, SG_INFO, "ConstHdgToAltCtl, below target altitude " << _waypt->altitudeFt()); + SG_LOG(SG_INSTR, SG_INFO, "ConstHdgToAltCtl, below target altitude " << _waypt->altitudeFt()); setDone(); } break; - case RESTRICT_NONE: - assert(false); + default: break; } } @@ -365,8 +529,8 @@ class InterceptCtl : public WayptController { public: InterceptCtl(RNAV* aRNAV, const WayptRef& aWpt) : - WayptController(aRNAV, aWpt) - + WayptController(aRNAV, aWpt), + _trueRadial(0.0) { if (_waypt->type() != "radialIntercept") { throw sg_exception("invalid waypoint type", "InterceptCtl ctor"); @@ -387,7 +551,7 @@ public: double r = SGGeodesy::courseDeg(_waypt->position(), _rnav->position()); SG_LOG(SG_AUTOPILOT, SG_INFO, "current radial=" << r); if (fabs(r - _trueRadial) < 0.5) { - SG_LOG(SG_GENERAL, SG_INFO, "InterceptCtl, intercepted radial " << _trueRadial); + SG_LOG(SG_INSTR, SG_INFO, "InterceptCtl, intercepted radial " << _trueRadial); setDone(); } } @@ -412,8 +576,9 @@ class DMEInterceptCtl : public WayptController { public: DMEInterceptCtl(RNAV* aRNAV, const WayptRef& aWpt) : - WayptController(aRNAV, aWpt) - + WayptController(aRNAV, aWpt), + _dme(NULL), + _distanceNm(0.0) { if (_waypt->type() != "dmeIntercept") { throw sg_exception("invalid waypoint type", "DMEInterceptCtl ctor"); @@ -431,7 +596,7 @@ public: _distanceNm = SGGeodesy::distanceNm(_rnav->position(), _dme->position()); double d = fabs(_distanceNm - _dme->dmeDistanceNm()); if (d < 0.1) { - SG_LOG(SG_GENERAL, SG_INFO, "DMEInterceptCtl, intercepted DME " << _dme->dmeDistanceNm()); + SG_LOG(SG_INSTR, SG_INFO, "DMEInterceptCtl, intercepted DME " << _dme->dmeDistanceNm()); setDone(); } } @@ -546,7 +711,10 @@ private: DirectToController::DirectToController(RNAV* aRNAV, const WayptRef& aWpt, const SGGeod& aOrigin) : WayptController(aRNAV, aWpt), - _origin(aOrigin) + _origin(aOrigin), + _distanceAircraftTargetMeter(0.0), + _courseDev(0.0), + _courseAircraftToTarget(0.0) { } @@ -555,31 +723,44 @@ void DirectToController::init() if (_waypt->flag(WPT_DYNAMIC)) { throw sg_exception("can't direct-to a dynamic waypoint"); } - + _targetTrack = SGGeodesy::courseDeg(_origin, _waypt->position()); } void DirectToController::update() { - double brg, az2; - SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM); - _courseDev = brg - _targetTrack; + double courseOriginToAircraft; + + courseOriginToAircraft = SGGeodesy::courseDeg(_origin,_rnav->position()); + + _courseAircraftToTarget = SGGeodesy::courseDeg(_rnav->position(),_waypt->position()); + _distanceAircraftTargetMeter = SGGeodesy::distanceM(_rnav->position(),_waypt->position()); + + _courseDev = -(courseOriginToAircraft - _targetTrack); + SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0); - - if ((fabs(_courseDev) > 90.0) && (_distanceM < _rnav->overflightArmDistanceM())) { - setDone(); + + bool isMinimumOverFlightDistanceReached = _distanceAircraftTargetMeter < _rnav->overflightDistanceM(); + bool isOverFlightConeArmed = _distanceAircraftTargetMeter < ( _rnav->overflightArmDistanceM() + _rnav->overflightDistanceM() ); + bool leavingOverFlightCone = (fabs(_courseAircraftToTarget - _targetTrack) > _rnav->overflightArmAngleDeg()); + + if( isMinimumOverFlightDistanceReached ){ + setDone(); + }else{ + if( isOverFlightConeArmed && leavingOverFlightCone ){ + setDone(); + } } } double DirectToController::distanceToWayptM() const { - return _distanceM; + return _distanceAircraftTargetMeter; } double DirectToController::xtrackErrorNm() const { - double x = sin(courseDeviationDeg() * SG_DEGREES_TO_RADIANS) * _distanceM; - return x * SG_METER_TO_NM; + return greatCircleCrossTrackError(_distanceAircraftTargetMeter * SG_METER_TO_NM, _courseDev); } double DirectToController::courseDeviationDeg() const @@ -589,7 +770,7 @@ double DirectToController::courseDeviationDeg() const double DirectToController::trueBearingDeg() const { - return SGGeodesy::courseDeg(_rnav->position(), _waypt->position()); + return _courseAircraftToTarget; } SGGeod DirectToController::position() const @@ -600,7 +781,10 @@ SGGeod DirectToController::position() const /////////////////////////////////////////////////////////////////////////////// OBSController::OBSController(RNAV* aRNAV, const WayptRef& aWpt) : - WayptController(aRNAV, aWpt) + WayptController(aRNAV, aWpt), + _distanceAircraftTargetMeter(0.0), + _courseDev(0.0), + _courseAircraftToTarget(0.0) { } @@ -616,26 +800,28 @@ void OBSController::init() void OBSController::update() { _targetTrack = _rnav->selectedMagCourse() + _rnav->magvarDeg(); - double brg, az2; - SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM); - _courseDev = brg - _targetTrack; + + _courseAircraftToTarget = SGGeodesy::courseDeg(_rnav->position(),_waypt->position()); + _distanceAircraftTargetMeter = SGGeodesy::distanceM(_rnav->position(),_waypt->position()); + + // _courseDev inverted we use point target as origin + _courseDev = (_courseAircraftToTarget - _targetTrack); SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0); } bool OBSController::toFlag() const { - return (fabs(_courseDev) < 90.0); + return (fabs(_courseDev) < _rnav->overflightArmAngleDeg()); } double OBSController::distanceToWayptM() const { - return _distanceM; + return _distanceAircraftTargetMeter; } double OBSController::xtrackErrorNm() const { - double x = sin(_courseDev * SG_DEGREES_TO_RADIANS) * _distanceM; - return x * SG_METER_TO_NM; + return greatCircleCrossTrackError(_distanceAircraftTargetMeter * SG_METER_TO_NM, _courseDev); } double OBSController::courseDeviationDeg() const @@ -651,7 +837,7 @@ double OBSController::courseDeviationDeg() const double OBSController::trueBearingDeg() const { - return SGGeodesy::courseDeg(_rnav->position(), _waypt->position()); + return _courseAircraftToTarget; } SGGeod OBSController::position() const @@ -692,7 +878,7 @@ WayptController* WayptController::createForWaypt(RNAV* aRNAV, const WayptRef& aW return new HoldCtl(aRNAV, aWpt); } - return new BasicWayptCtl(aRNAV, aWpt); + return new LegWayptCtl(aRNAV, aWpt); } } // of namespace flightgear