fly/flightgear
1
0
Fork 0
Code Activity

Fly-by support for Radial intercepts

Browse source 
In the common case, avoid an overshoot when doing radial
intercepts.
This commit is contained in:
James Turner 2020-05-30 12:04:18 +01:00
parent 176ccfa8dc
commit e51a0c55b5
3 changed files with 138 additions and 37 deletions
src/Instrumentation
rnav_waypt_controller.cxx
test_suite/unit_tests/Instrumentation
test_gps.cxxtest_hold_controller.cxx

138
src/Instrumentation/rnav_waypt_controller.cxx
View file

@ -35,6 +35,8 @@ extern double pointsKnownDistanceFromGC(const SGGeoc& a, const SGGeoc&b, const S
namespace flightgear
{
const auto turnComputeDist = SG_NM_TO_METER * 4.0;
// declared in routePath.cxx
bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result);
@ -64,8 +66,21 @@ double greatCircleCrossTrackError(double distanceOriginToPosition,double courseD
return crossTrackError * SG_RAD_TO_NM;
}
SGGeod computeTurnCenter(double turnRadiusM, const SGGeod& basePos, double inboundTrack, double turnAngle)
{
// this is the heading half way through the turn. Perpendicular to
// this is our turn center
const auto halfTurnHeading = inboundTrack + (turnAngle * 0.5);
double p = copysign(90.0, turnAngle);
double h = halfTurnHeading + p;
SG_NORMALIZE_RANGE(h, 0.0, 360.0);
const double tcOffset = turnRadiusM / cos(turnAngle * 0.5 * SG_DEGREES_TO_RADIANS);
return SGGeodesy::direct(basePos, h, tcOffset);
}
////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////s
WayptController::~WayptController()
{
@ -315,23 +330,12 @@ public:
}
_flyByTurnRadius = _rnav->turnRadiusNm() * SG_NM_TO_METER;
// this is the heading half way through the turn. Perpendicular to
// this is our turn ceenter
const auto halfTurnHeading = _finalLegCourse + (_flyByTurnAngle * 0.5);
double p = copysign(90.0, _flyByTurnAngle);
double h = halfTurnHeading + p;
SG_NORMALIZE_RANGE(h, 0.0, 360.0);
const double tcOffset = _flyByTurnRadius / cos(_flyByTurnAngle * 0.5 * SG_DEGREES_TO_RADIANS);
_flyByTurnCenter = SGGeodesy::direct(_waypt->position(), h, tcOffset);
_flyByTurnCenter = computeTurnCenter(_flyByTurnRadius, _waypt->position(), _finalLegCourse, _flyByTurnAngle);
_doFlyBy = true;
}
bool updateInTurn()
{
auto dl = FGTestApi::DataLogger::instance();
// find bearing to turn center
// when it hits 90 off our track
@ -343,7 +347,6 @@ public:
auto a = bearingToTurnCenter - _finalLegCourse;
SG_NORMALIZE_RANGE(a, -180.0, 180.0);
if (fabs(a) < 90.0) {
dl->recordSamplePoint("turn-entry-bearing", a);
return false; // keep flying normal leg
}
@ -355,9 +358,7 @@ public:
const auto halfPointAngle = (_finalLegCourse + (_flyByTurnAngle * 0.5));
auto b = bearingToTurnCenter - halfPointAngle;
SG_NORMALIZE_RANGE(b, -180.0, 180.0);
dl->recordSamplePoint("turn-exit-bearing", b);
if (fabs(b) >= 90.0) {
_toFlag = false;
setDone();
@ -377,14 +378,10 @@ public:
void updateInEntryTurn()
{
auto dl = FGTestApi::DataLogger::instance();
auto bearingToTurnCenter = SGGeodesy::courseDeg(_rnav->position(), _flyByTurnCenter);
auto distToTurnCenter = SGGeodesy::distanceM(_rnav->position(), _flyByTurnCenter);
auto b = bearingToTurnCenter - _initialLegCourse;
dl->recordSamplePoint("turn-entry-bearing", b);
SG_NORMALIZE_RANGE(b, -180.0, 180.0);
if (fabs(b) >= 90.0) {
@ -401,8 +398,6 @@ public:
void update(double) override
{
auto dl = FGTestApi::DataLogger::instance();
_courseOriginToAircraft = SGGeodesy::courseDeg(_waypointOrigin,_rnav->position());
_distanceOriginAircraftMetre = SGGeodesy::distanceM(_waypointOrigin,_rnav->position());
@ -414,7 +409,6 @@ public:
return;
}
const auto turnComputeDist = SG_NM_TO_METER * 4.0;
if (!_didComputeTurn && (_distanceAircraftTargetMetre < turnComputeDist)) {
computeTurnAnticipation();
}
@ -742,14 +736,16 @@ public:
}
}
virtual void init()
void init() override
{
RadialIntercept* w = (RadialIntercept*) _waypt.get();
_trueRadial = w->radialDegMagnetic() + _rnav->magvarDeg();
_targetTrack = w->courseDegMagnetic() + _rnav->magvarDeg();
_canFlyBy = !_waypt->flag(WPT_OVERFLIGHT) && _rnav->canFlyBy();
}
virtual void update(double)
void update(double) override
{
SGGeoc c,
geocPos = SGGeoc::fromGeod(_rnav->position()),
@ -774,8 +770,16 @@ public:
}
_projectedPosition = SGGeod::fromGeoc(c);
_distanceToProjectedInterceptM = SGGeodesy::distanceM(_rnav->position(), _projectedPosition);
if (!_didComputeTurn && (_distanceToProjectedInterceptM < turnComputeDist)) {
computeTurn();
}
if (_doFlyBy) {
updateFlyByTurn();
}
// note we want the outbound radial from the waypt, hence the ordering
// of arguments to courseDeg
double r = SGGeodesy::courseDeg(_waypt->position(), _rnav->position());
@ -785,19 +789,91 @@ public:
setDone();
}
}
bool updateFlyByTurn()
{
// find bearing to turn center
// when it hits 90 off our track
auto bearingToTurnCenter = SGGeodesy::courseDeg(_rnav->position(), _flyByTurnCenter);
auto distToTurnCenter = SGGeodesy::distanceM(_rnav->position(), _flyByTurnCenter);
if (!_flyByStarted) {
// check for the turn center being 90 degrees off one wing (start the turn)
auto a = bearingToTurnCenter - _targetTrack;
SG_NORMALIZE_RANGE(a, -180.0, 180.0);
if (fabs(a) < 90.0) {
return false;
}
_flyByStarted = true;
}
// in the actual turn, our desired track is always pependicular to the
// bearing to the turn center we computed
_targetTrack = bearingToTurnCenter - copysign(90, _flyByTurnAngle);
SG_NORMALIZE_RANGE(_targetTrack, 0.0, 360.0);
_crossTrackError = (distToTurnCenter - _flyByTurnRadius) * SG_METER_TO_NM;
return true;
}
void computeTurn()
{
_didComputeTurn = true;
if (!_canFlyBy)
return;
double inverseRadial = _trueRadial + 180.0;
SG_NORMALIZE_RANGE(inverseRadial, 0.0, 360.0);
_flyByTurnAngle = inverseRadial - _targetTrack;
SG_NORMALIZE_RANGE(_flyByTurnAngle, -180.0, 180.0);
if (fabs(_flyByTurnAngle) > 120.0) {
// too sharp, no fly-by
return;
}
_flyByTurnRadius = _rnav->turnRadiusNm() * SG_NM_TO_METER;
_flyByTurnCenter = computeTurnCenter(_flyByTurnRadius, _projectedPosition, _targetTrack, _flyByTurnAngle);
_doFlyBy = true;
}
virtual double distanceToWayptM() const
double distanceToWayptM() const override
{
return SGGeodesy::distanceM(_rnav->position(), position());
return _distanceToProjectedInterceptM;
}
virtual SGGeod position() const
SGGeod position() const override
{
return _projectedPosition;
}
double xtrackErrorNm() const override
{
if (!_flyByStarted)
return 0.0; // unless we're doing the fly-by, we're always on course
return _crossTrackError;
}
double courseDeviationDeg() const override
{
// guessed scaling factor
return xtrackErrorNm() * 10.0;
}
private:
double _trueRadial;
SGGeod _projectedPosition;
bool _canFlyBy = false;
bool _didComputeTurn = false;
bool _doFlyBy = false;
double _distanceToProjectedInterceptM = 0.0;
SGGeod _flyByTurnCenter;
double _flyByTurnAngle;
double _flyByTurnRadius;
bool _flyByStarted = false;
double _crossTrackError = 0.0;
};
class DMEInterceptCtl : public WayptController

35
test_suite/unit_tests/Instrumentation/test_gps.cxx
View file

@ -1092,7 +1092,7 @@ void GPSTests::testOffsetFlight()
void GPSTests::testLegIntercept()
{
FGTestApi::setUp::logPositionToKML("gps_intercept");
// FGTestApi::setUp::logPositionToKML("gps_intercept");
auto rm = globals->get_subsystem<FGRouteMgr>();
auto fp = new FlightPlan;
rm->setFlightPlan(fp);
@ -1263,7 +1263,7 @@ void GPSTests::testLegIntercept()
void GPSTests::testTurnAnticipation()
{
FGTestApi::setUp::logPositionToKML("gps_flyby_sequence");
//FGTestApi::setUp::logPositionToKML("gps_flyby_sequence");
auto rm = globals->get_subsystem<FGRouteMgr>();
auto fp = new FlightPlan;
rm->setFlightPlan(fp);
@ -1378,7 +1378,7 @@ void GPSTests::testTurnAnticipation()
void GPSTests::testRadialIntercept()
{
FGTestApi::setUp::logPositionToKML("gps_radial_intercept");
// FGTestApi::setUp::logPositionToKML("gps_radial_intercept");
auto rm = globals->get_subsystem<FGRouteMgr>();
auto fp = new FlightPlan;
@ -1413,6 +1413,7 @@ void GPSTests::testRadialIntercept()
auto gpsNode = globals->get_props()->getNode("instrumentation/gps");
gpsNode->setBoolValue("config/delegate-sequencing", true);
gpsNode->setBoolValue("config/enable-fly-by", false);
gpsNode->setStringValue("command", "leg");
fp->setCurrentIndex(2);
@ -1434,14 +1435,38 @@ void GPSTests::testRadialIntercept()
// flying to BEBEV now
CPPUNIT_ASSERT_DOUBLES_EQUAL(185, gpsNode->getDoubleValue("wp/leg-true-course-deg"), 1.0);
FGTestApi::runForTime(30.0);
// repeat but with fly-by enabled
gpsNode->setBoolValue("config/enable-fly-by", true);
gpsNode->setStringValue("command", "obs");
FGTestApi::setPositionAndStabilise(initPos);
pilot->resetAtPosition(initPos);
fp->setCurrentIndex(2);
gpsNode->setStringValue("command", "leg");
CPPUNIT_ASSERT_EQUAL(string{"BUNAX"}, string{gpsNode->getStringValue("wp/wp[1]/ID")});
CPPUNIT_ASSERT_DOUBLES_EQUAL(312, gpsNode->getDoubleValue("wp/leg-true-course-deg"), 1.0);
pilot->setCourseTrue(fp->legAtIndex(2)->courseDeg());
pilot->flyGPSCourse(gps);
ok = FGTestApi::runForTimeWithCheck(600.0, [fp]() {
return fp->currentIndex() == 4;
});
CPPUNIT_ASSERT(ok);
// flying to BEBEV now
CPPUNIT_ASSERT_DOUBLES_EQUAL(185, gpsNode->getDoubleValue("wp/leg-true-course-deg"), 1.0);
FGTestApi::runForTime(30.0);
}
void GPSTests::testDMEIntercept()
{
FGTestApi::setUp::logPositionToKML("gps_dme_intercept");
// FGTestApi::setUp::logPositionToKML("gps_dme_intercept");
auto rm = globals->get_subsystem<FGRouteMgr>();
auto fp = new FlightPlan;

2
test_suite/unit_tests/Instrumentation/test_hold_controller.cxx
View file

@ -238,7 +238,7 @@ void HoldControllerTests::testHoldEntryDirect()
void HoldControllerTests::testHoldEntryTeardrop()
{
FGTestApi::setUp::logPositionToKML("hold_teardrop_entry");
// FGTestApi::setUp::logPositionToKML("hold_teardrop_entry");
auto rm = globals->get_subsystem<FGRouteMgr>();
auto fp = new FlightPlan;