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) {
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;
}
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()
{
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");
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
}
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)
{
}
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();
}
}
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;
}
}
private:
FGRunway* _runway;
- double _distanceM;
+ double _distanceAircraftRunwayEnd;
double _courseDev;
};
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;
}
}
{
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");
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();
}
}
{
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");
_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();
}
}
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)
{
}
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
double DirectToController::trueBearingDeg() const
{
- return SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
+ return _courseAircraftToTarget;
}
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)
{
}
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
double OBSController::trueBearingDeg() const
{
- return SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
+ return _courseAircraftToTarget;
}
SGGeod OBSController::position() const
return new HoldCtl(aRNAV, aWpt);
}
- return new BasicWayptCtl(aRNAV, aWpt);
+ return new LegWayptCtl(aRNAV, aWpt);
}
} // of namespace flightgear
projects / flightgear.git / blobdiff