From: Dirk Dittmann Date: Sat, 27 Jul 2013 12:06:03 +0000 (+0200) Subject: GPS X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;h=778cc8c6a0abb88a1238850376ea2374358fd887;p=flightgear.git GPS - greatCircleCrossTrackError added to LegWayptCtl, DirectToController, OBSController - config overflight seqence config/over-flight-arm-angle-deg 90 config/over-flight-arm-distance-nm 1 config/over-flight-distance-nm 0 --- diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx index 5552fdf1c..a50ee1ff7 100644 --- a/src/Instrumentation/gps.cxx +++ b/src/Instrumentation/gps.cxx @@ -61,7 +61,9 @@ static const char* makeTTWString(double TTW) GPS::Config::Config() : _enableTurnAnticipation(true), _turnRate(3.0), // degrees-per-second, so 180 degree turn takes 60 seconds + _overflightDistance(0.0), _overflightArmDistance(1.0), + _overflightArmAngle(90.0), _waypointAlertTime(30.0), _requireHardSurface(true), _cdiMaxDeflectionNm(3.0), // linear mode, 3nm at the peg @@ -80,6 +82,9 @@ void GPS::Config::bind(GPS* aOwner, SGPropertyNode* aCfg) aOwner->tie(aCfg, "cdi-max-deflection-nm", SGRawValuePointer(&_cdiMaxDeflectionNm)); aOwner->tie(aCfg, "drive-autopilot", SGRawValuePointer(&_driveAutopilot)); aOwner->tie(aCfg, "course-selectable", SGRawValuePointer(&_courseSelectable)); + aOwner->tie(aCfg, "over-flight-distance-nm", SGRawValuePointer(&_overflightDistance)); + aOwner->tie(aCfg, "over-flight-arm-distance-nm", SGRawValuePointer(&_overflightArmDistance)); + aOwner->tie(aCfg, "over-flight-arm-angle-deg", SGRawValuePointer(&_overflightArmAngle)); } //////////////////////////////////////////////////////////////////////////// @@ -417,16 +422,39 @@ double GPS::magvarDeg() return _magvar_node->getDoubleValue(); } +double GPS::overflightDistanceM() +{ + return _config.overflightDistanceNm() * SG_NM_TO_METER; +} + double GPS::overflightArmDistanceM() { return _config.overflightArmDistanceNm() * SG_NM_TO_METER; } +double GPS::overflightArmAngleDeg() +{ + return _config.overflightArmAngleDeg(); +} + double GPS::selectedMagCourse() { return _selectedCourse; } +SGGeod GPS::previousLegWaypointPosition(bool& isValid) +{ + FlightPlan::Leg* leg = _route->previousLeg(); + if (leg){ + Waypt* waypt = leg->waypoint(); + if(waypt){ + isValid = true; + return waypt->position(); + } + } + isValid = false; + return SGGeod(); +} /////////////////////////////////////////////////////////////////////////// void @@ -653,7 +681,6 @@ void GPS::updateOverflight() } else if (_mode == "obs") { // nothing to do here, TO/FROM will update but that's fine } - _computeTurnData = true; } @@ -1128,7 +1155,6 @@ void GPS::directTo() _wp0_position = _indicated_pos; _currentWaypt = new BasicWaypt(_scratchPos, _scratchNode->getStringValue("ident"), NULL); _mode = "dto"; - clearScratch(); wp1Changed(); } @@ -1145,6 +1171,7 @@ void GPS::selectOBSMode(flightgear::Waypt* waypt) } _mode = "obs"; + _currentWaypt = waypt; _wp0_position = _indicated_pos; wp1Changed(); @@ -1162,6 +1189,7 @@ void GPS::selectLegMode() } _mode = "leg"; + // depending on the situation, this will either get over-written // in routeManagerSequenced or not; either way it does no harm to // set it here. diff --git a/src/Instrumentation/gps.hxx b/src/Instrumentation/gps.hxx index a7d3b3be1..444f029d2 100644 --- a/src/Instrumentation/gps.hxx +++ b/src/Instrumentation/gps.hxx @@ -79,7 +79,10 @@ public: virtual double vspeedFPM(); virtual double magvarDeg(); virtual double selectedMagCourse(); + virtual double overflightDistanceM(); virtual double overflightArmDistanceM(); + virtual double overflightArmAngleDeg(); + virtual SGGeod previousLegWaypointPosition(bool& isValid); private: friend class SearchFilter; @@ -102,12 +105,20 @@ private: */ double turnRateDegSec() const { return _turnRate; } + /** + * Distance at which we switch to next waypoint. + */ + double overflightDistanceNm() const { return _overflightDistance; } /** * Distance at which we arm overflight sequencing. Once inside this * distance, a change of the wp1 'TO' flag to false will be considered * overlight of the wp. */ double overflightArmDistanceNm() const { return _overflightArmDistance; } + /** + * abs angle at which we arm overflight sequencing. + */ + double overflightArmAngleDeg() const { return _overflightArmAngle; } /** * Time before the next WP to activate an external annunciator @@ -134,9 +145,15 @@ private: // desired turn rate in degrees per second double _turnRate; + // distance from waypoint to arm overflight sequencing (in nm) + double _overflightDistance; + // distance from waypoint to arm overflight sequencing (in nm) double _overflightArmDistance; + //abs angle from course to waypoint to arm overflight sequencing (in deg) + double _overflightArmAngle; + // time before reaching a waypoint to trigger annunciator light/sound // (in seconds) double _waypointAlertTime; diff --git a/src/Instrumentation/rnav_waypt_controller.cxx b/src/Instrumentation/rnav_waypt_controller.cxx index f893b9413..d0bed4ce7 100644 --- a/src/Instrumentation/rnav_waypt_controller.cxx +++ b/src/Instrumentation/rnav_waypt_controller.cxx @@ -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() @@ -158,7 +186,7 @@ class BasicWayptCtl : public WayptController public: BasicWayptCtl(RNAV* aRNAV, const WayptRef& aWpt) : WayptController(aRNAV, aWpt), - _distanceM(0.0), + _distanceAircraftTargetMeter(0.0), _courseDev(0.0) { if (aWpt->flag(WPT_DYNAMIC)) { @@ -173,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 @@ -215,8 +246,134 @@ 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(), + _distanceOriginTargetMeter(0.0), + _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 _distanceOriginTargetMeter; + double _distanceOriginAircraftMeter; + double _distanceAircraftTargetMeter; + double _courseOriginToAircraft; + double _courseAircraftToTarget; double _courseDev; + bool _toFlag; + }; /** @@ -230,7 +387,7 @@ public: RunwayCtl(RNAV* aRNAV, const WayptRef& aWpt) : WayptController(aRNAV, aWpt), _runway(NULL), - _distanceM(0.0), + _distanceAircraftRunwayEnd(0.0), _courseDev(0.0) { } @@ -243,16 +400,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) && (_distanceM < _rnav->overflightArmDistanceM())) { + if ((fabs(_courseDev) > _rnav->overflightArmAngleDeg()) && (_distanceAircraftRunwayEnd < _rnav->overflightArmDistanceM())) { setDone(); } } @@ -264,7 +424,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; } @@ -286,7 +446,7 @@ public: } private: FGRunway* _runway; - double _distanceM; + double _distanceAircraftRunwayEnd; double _courseDev; }; @@ -554,8 +714,9 @@ private: DirectToController::DirectToController(RNAV* aRNAV, const WayptRef& aWpt, const SGGeod& aOrigin) : WayptController(aRNAV, aWpt), _origin(aOrigin), - _distanceM(0.0), - _courseDev(0.0) + _distanceAircraftTargetMeter(0.0), + _courseDev(0.0), + _courseAircraftToTarget(0.0) { } @@ -564,31 +725,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 @@ -598,7 +772,7 @@ double DirectToController::courseDeviationDeg() const double DirectToController::trueBearingDeg() const { - return SGGeodesy::courseDeg(_rnav->position(), _waypt->position()); + return _courseAircraftToTarget; } SGGeod DirectToController::position() const @@ -610,8 +784,9 @@ SGGeod DirectToController::position() const OBSController::OBSController(RNAV* aRNAV, const WayptRef& aWpt) : WayptController(aRNAV, aWpt), - _distanceM(0.0), - _courseDev(0.0) + _distanceAircraftTargetMeter(0.0), + _courseDev(0.0), + _courseAircraftToTarget(0.0) { } @@ -627,26 +802,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 @@ -662,7 +839,7 @@ double OBSController::courseDeviationDeg() const double OBSController::trueBearingDeg() const { - return SGGeodesy::courseDeg(_rnav->position(), _waypt->position()); + return _courseAircraftToTarget; } SGGeod OBSController::position() const @@ -703,7 +880,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 diff --git a/src/Instrumentation/rnav_waypt_controller.hxx b/src/Instrumentation/rnav_waypt_controller.hxx index b5c4b19e1..8ff8ab853 100644 --- a/src/Instrumentation/rnav_waypt_controller.hxx +++ b/src/Instrumentation/rnav_waypt_controller.hxx @@ -59,10 +59,24 @@ public: */ virtual double selectedMagCourse() = 0; + /** + * minimum distance to switch next waypoint. + */ + virtual double overflightDistanceM() = 0; /** * minimum distance to a waypoint for overflight sequencing. */ virtual double overflightArmDistanceM() = 0; + /** + * angle for overflight sequencing. + */ + virtual double overflightArmAngleDeg() = 0; + + /** + * device leg previous waypoint position(eg, from route manager) + */ + virtual SGGeod previousLegWaypointPosition(bool& isValid)= 0; + }; class WayptController @@ -164,8 +178,9 @@ public: virtual SGGeod position() const; private: SGGeod _origin; - double _distanceM; + double _distanceAircraftTargetMeter; double _courseDev; + double _courseAircraftToTarget; }; /** @@ -185,8 +200,9 @@ public: virtual bool toFlag() const; virtual SGGeod position() const; private: - double _distanceM; + double _distanceAircraftTargetMeter; double _courseDev; + double _courseAircraftToTarget; }; } // of namespace flightgear