fly/flightgear
1
0
Fork 0
Code Activity

Improve Leg mode course/track computation

Browse source
This commit is contained in:
James Turner 2019-09-18 16:47:19 +01:00
parent 21e9f3d5e9
commit 2434c037c8
7 changed files with 100 additions and 48 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

5
src/Instrumentation/gps.cxx
View file

@ -723,6 +723,11 @@ double GPS::computeTurnProgress(double aBearing) const
return (aBearing - startBearing) / _turnAngle;
}
double GPS::turnRadiusNm(const double groundSpeedKnots)
{
return computeTurnRadiusNm(groundSpeedKnots);
}
void GPS::computeTurnData()
{
_computeTurnData = false;

2
src/Instrumentation/gps.hxx
View file

@ -85,7 +85,7 @@ public:
virtual double overflightArmDistanceM();
virtual double overflightArmAngleDeg();
virtual SGGeod previousLegWaypointPosition(bool& isValid);
double turnRadiusNm(const double groundSpeedKnots) override;
private:
friend class SearchFilter;

97
src/Instrumentation/rnav_waypt_controller.cxx
View file

@ -189,27 +189,25 @@ public:
virtual void init()
{
double courseOriginTarget;
bool isPreviousLegValid = false;
_waypointOrigin = _rnav->previousLegWaypointPosition(isPreviousLegValid);
courseOriginTarget = SGGeodesy::courseDeg(_waypointOrigin,_waypt->position());
_initialLegCourse = SGGeodesy::courseDeg(_waypointOrigin,_waypt->position());
_courseAircraftToTarget = SGGeodesy::courseDeg(_rnav->position(),_waypt->position());
_distanceAircraftTargetMetre = SGGeodesy::distanceM(_rnav->position(),_waypt->position());
// check reach the leg in 45Deg or going direct
bool canReachLeg = (fabs(courseOriginTarget -_courseAircraftToTarget) < 45.0);
bool canReachLeg = (fabs(_initialLegCourse -_courseAircraftToTarget) < 45.0);
if ( isPreviousLegValid && canReachLeg){
_targetTrack = courseOriginTarget;
}else{
_targetTrack = _courseAircraftToTarget;
_waypointOrigin = _rnav->position();
if (isPreviousLegValid && canReachLeg) {
_targetTrack = _initialLegCourse;
} else {
_targetTrack = _courseAircraftToTarget;
_initialLegCourse = _courseAircraftToTarget;
_waypointOrigin = _rnav->position();
}
}
virtual void update(double)
@ -220,24 +218,69 @@ public:
_courseAircraftToTarget = SGGeodesy::courseDeg(_rnav->position(),_waypt->position());
_distanceAircraftTargetMetre = SGGeodesy::distanceM(_rnav->position(),_waypt->position());
_courseDev = -(_courseOriginToAircraft - _targetTrack);
// from the Aviation Formulary
#if 0
Suppose you are proceeding on a great circle route from A to B (course =crs_AB) and end up at D,
perhaps off course. (We presume that A is ot a pole!) You can calculate the course from A to D
(crs_AD) and the distance from A to D (dist_AD) using the formulae above. In terms of these the
cross track error, XTD, (distance off course) is given by
XTD =asin(sin(dist_AD)*sin(crs_AD-crs_AB))
(positive XTD means right of course, negative means left)
(If the point A is the N. or S. Pole replace crs_AD-crs_AB with
lon_D-lon_B or lon_B-lon_D, respectively.)
#endif
// however, just for fun, our convention for polarity of the cross-track
// sign is opposite, so we add a -ve to the computation below.
double distOriginAircraftRad = _distanceOriginAircraftMetre * SG_METER_TO_NM * SG_NM_TO_RAD;
double xtkRad = asin(sin(distOriginAircraftRad) * sin((_courseOriginToAircraft - _initialLegCourse) * SG_DEGREES_TO_RADIANS));
// convert to NM and flip sign for consistency with existing code.
// since we derive the abeam point and course-deviation from this, and
// thus the GPS cdi-deflection, if we don't fix this here, the sign of
// all of those comes out backwards.
_crossTrackError = -(xtkRad * SG_RAD_TO_NM);
/*
The "along track distance", ATD, the distance from A along the course towards B
to the point abeam D is given by:
ATD=acos(cos(dist_AD)/cos(XTD))
*/
double atd = acos(cos(distOriginAircraftRad) / cos(xtkRad));
const double atdM = atd * SG_RAD_TO_NM * SG_NM_TO_METER;
SGGeod abeamPoint = SGGeodesy::direct(_waypointOrigin, _initialLegCourse, atdM);
// if we're close to the target point, we enter something similar to the VOR zone
// of confusion. Force target track to the final course from the origin.
if (_distanceAircraftTargetMetre < (SG_NM_TO_METER * 2.0)) {
_targetTrack = SGGeodesy::courseDeg(_waypt->position(), _waypointOrigin) + 180.0;
SG_NORMALIZE_RANGE(_targetTrack, 0.0, 360.0);
} else {
// use the abeam point to compute the desired course
double desiredCourse = SGGeodesy::courseDeg(abeamPoint, _waypt->position());
_targetTrack = desiredCourse;
}
_courseDev = _courseAircraftToTarget - _targetTrack;
SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
bool isMinimumOverFlightDistanceReached = _distanceAircraftTargetMetre < _rnav->overflightDistanceM();
bool isOverFlightConeArmed = _distanceAircraftTargetMetre < ( _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;
}
bool leavingOverFlightCone = (fabs(_courseDev) > _rnav->overflightArmAngleDeg());
if ( isMinimumOverFlightDistanceReached ){
_toFlag = false;
setDone();
} else {
if( isOverFlightConeArmed ){
_toFlag = false;
if ( leavingOverFlightCone ) {
setDone();
}
}else{
_toFlag = true;
}
}
}
@ -248,7 +291,7 @@ public:
virtual double xtrackErrorNm() const
{
return greatCircleCrossTrackError(_distanceOriginAircraftMetre * SG_METER_TO_NM, _courseDev);
return _crossTrackError;
}
virtual bool toFlag() const
@ -278,12 +321,14 @@ private:
* A(from), B(to), D(position) perhaps off course
*/
SGGeod _waypointOrigin;
double _initialLegCourse;
double _distanceOriginAircraftMetre;
double _distanceAircraftTargetMetre;
double _courseOriginToAircraft;
double _courseAircraftToTarget;
double _courseDev;
bool _toFlag;
double _crossTrackError;
};

14
src/Instrumentation/rnav_waypt_controller.hxx
View file

@ -77,6 +77,20 @@ public:
*/
virtual SGGeod previousLegWaypointPosition(bool& isValid)= 0;
/**
* @brief compute turn radius based on current ground-speed
*/
double turnRadiusNm()
{
return turnRadiusNm(groundSpeedKts());
}
/**
* @brief compute the turn radius (based on standard rate turn) for
* a given ground speed in knots.
*/
virtual double turnRadiusNm(const double groundSpeedKnots) = 0;
};
class WayptController

4
test_suite/FGTestApi/TestPilot.cxx
View file

@ -115,9 +115,7 @@ void TestPilot::updateValues(double dt)
if (_gps && (_lateralMode == LateralMode::GPSCourse)) {
const double deviationDeg = _gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg");
_targetCourseDeg = _gpsNode->getDoubleValue("wp/leg-true-course-deg");
// leave out for now
// _targetCourseDeg += deviationDeg;
_targetCourseDeg += deviationDeg;
SG_NORMALIZE_RANGE(_targetCourseDeg, 0.0, 360.0);
if (!_turnActive &&(fabs(_trueCourseDeg - _targetCourseDeg) > 0.5)) {

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

@ -329,16 +329,16 @@ void GPSTests::testLegMode()
const double legCourse = SGGeodesy::courseDeg(depRwy->end(), nikPos);
CPPUNIT_ASSERT_DOUBLES_EQUAL(legCourse, gpsNode->getDoubleValue("wp/leg-true-course-deg"), 0.5);
#if 0
CPPUNIT_ASSERT_DOUBLES_EQUAL(legCourse - 270, gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5);
#endif
const double crsToNIK = SGGeodesy::courseDeg(p2, nikPos);
CPPUNIT_ASSERT_DOUBLES_EQUAL(crsToNIK - legCourse, gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5);
pilot->setSpeedKts(200);
pilot->setCourseTrue(270);
FGTestApi::runForTime(120.0);
// CPPUNIT_ASSERT_EQUAL(2, testDelegate->sequenceCount);
#if 0
CPPUNIT_ASSERT_EQUAL(2, testDelegate->sequenceCount);
#endif
CPPUNIT_ASSERT_EQUAL(2, fp->currentIndex());
CPPUNIT_ASSERT_EQUAL(std::string{"NIK"}, std::string{gpsNode->getStringValue("wp/wp[0]/ID")});
CPPUNIT_ASSERT_EQUAL(std::string{"COA"}, std::string{gpsNode->getStringValue("wp/wp[1]/ID")});
@ -366,10 +366,8 @@ void GPSTests::testLegMode()
double courseToDover = SGGeodesy::courseDeg(off1, doverPos);
CPPUNIT_ASSERT_DOUBLES_EQUAL(courseToDover, gpsNode->getDoubleValue("wp/wp[1]/bearing-true-deg"), 0.5);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-5.0, gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5);
#if 0
CPPUNIT_ASSERT_DOUBLES_EQUAL(5.0, gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5);
#endif
CPPUNIT_ASSERT_EQUAL(true, gpsNode->getBoolValue("wp/wp[1]/to-flag"));
CPPUNIT_ASSERT_EQUAL(false, gpsNode->getBoolValue("wp/wp[1]/from-flag"));
@ -378,9 +376,7 @@ void GPSTests::testLegMode()
courseToDover = SGGeodesy::courseDeg(off2, doverPos);
CPPUNIT_ASSERT_DOUBLES_EQUAL(courseToDover, gpsNode->getDoubleValue("wp/wp[1]/bearing-true-deg"), 0.5);
#if 0
CPPUNIT_ASSERT_DOUBLES_EQUAL(-8.0, gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5);
#endif
CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.5);
CPPUNIT_ASSERT_EQUAL(true, gpsNode->getBoolValue("wp/wp[1]/to-flag"));
CPPUNIT_ASSERT_EQUAL(false, gpsNode->getBoolValue("wp/wp[1]/from-flag"));
@ -411,17 +407,14 @@ void GPSTests::testLegMode()
SGGeod offTrack3 = SGGeodesy::direct(alongTrack2, course + 90, SG_NM_TO_METER * 1.6);
setPositionAndStabilise(gps, offTrack3);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.6, gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.1);
#if 0
CPPUNIT_ASSERT_DOUBLES_EQUAL(revCourse + 180.0, gpsNode->getDoubleValue("wp/leg-true-course-deg"), 0.1);
#endif
// check cross track in the middle
SGGeod alongTrack3 = SGGeodesy::direct(coaPos, course, distanceCOA_DVR * 0.52);
SGGeod offTrack4 = SGGeodesy::direct(alongTrack3, course + 90, SG_NM_TO_METER * 15.6);
setPositionAndStabilise(gps, offTrack4);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-15.6, gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.1);
#if 0
CPPUNIT_ASSERT_DOUBLES_EQUAL(261.55, gpsNode->getDoubleValue("wp/leg-true-course-deg"), 0.1);
#endif
}
void GPSTests::testDirectToLegOnFlightplan()

3
test_suite/unit_tests/Navaids/test_routeManager.cxx
View file

@ -169,9 +169,6 @@ void RouteManagerTests::testBasic()
// slightly rapid descent
pilot->setTargetAltitudeFtMSL(3000);
// temporary until GPS course / deviation fixes are merged
return;
ok = FGTestApi::runForTimeWithCheck(6000.0, [rmNode] () {
if (rmNode->getIntValue("current-wp") == 7) return true;
return false;