#include "rnav_waypt_controller.hxx"
+#include <cassert>
+
#include <simgear/sg_inlines.h>
#include <simgear/structure/exception.hxx>
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;
}
{
public:
BasicWayptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
- WayptController(aRNAV, aWpt)
+ WayptController(aRNAV, aWpt),
+ _distanceM(0.0),
+ _courseDev(0.0)
{
if (aWpt->flag(WPT_DYNAMIC)) {
throw sg_exception("BasicWayptCtrl doesn't work with dynamic waypoints");
/**
* 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),
+ _distanceM(0.0),
+ _courseDev(0.0)
{
}
double _courseDev = brg - _targetTrack;
SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
- if (fabs(_courseDev) > 90.0) {
+ if ((fabs(_courseDev) > 90.0) && (_distanceM < _rnav->overflightArmDistanceM())) {
setDone();
}
}
case RESTRICT_AT: {
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);
break;
+ case SPEED_RESTRICT_MACH:
+ assert(false);
+ 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),
+ _distanceM(0.0),
+ _courseDev(0.0)
{
}
///////////////////////////////////////////////////////////////////////////////
OBSController::OBSController(RNAV* aRNAV, const WayptRef& aWpt) :
- WayptController(aRNAV, aWpt)
+ WayptController(aRNAV, aWpt),
+ _distanceM(0.0),
+ _courseDev(0.0)
{
}