fly/flightgear
1
0
Fork 0
Code Activity

GPS : Offset flight test

Browse source
This commit is contained in:
James Turner 2019-09-19 17:26:03 +01:00
parent 05e99d3a3b
commit 19af6994fb
3 changed files with 120 additions and 5 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

25
test_suite/FGTestApi/TestPilot.cxx
View file

@ -104,6 +104,15 @@ void TestPilot::flyGPSCourse(GPS *gps)
_turnActive = false;
}
void TestPilot::flyGPSCourseOffset(GPS *gps, double offsetNm)
{
_gps = gps;
_gpsNode = globals->get_props()->getNode("instrumentation/gps");
_lateralMode = LateralMode::GPSOffset;
_courseOffsetNm = offsetNm;
_turnActive = false;
}
void TestPilot::flyDirectTo(const SGGeod& target)
{
_lateralMode = LateralMode::Direct;
@ -134,6 +143,22 @@ void TestPilot::updateValues(double dt)
}
}
if (_gps && (_lateralMode == LateralMode::GPSOffset)) {
_targetCourseDeg = _gpsNode->getDoubleValue("wp/leg-true-course-deg");
double crossTrack = _gpsNode->getDoubleValue("wp/wp[1]/course-error-nm");
double offsetError = crossTrack - _courseOffsetNm;
const double offsetCorrectionFactor = 25.0;
const double correction = offsetError * offsetCorrectionFactor;
_targetCourseDeg += correction;
SG_NORMALIZE_RANGE(_targetCourseDeg, 0.0, 360.0);
if (!_turnActive &&(fabs(_trueCourseDeg - _targetCourseDeg) > 0.5)) {
_turnActive = true;
}
}
if (_lateralMode == LateralMode::Direct) {
_targetCourseDeg = SGGeodesy::courseDeg(globals->get_aircraft_position(), _targetPos);
SG_NORMALIZE_RANGE(_targetCourseDeg, 0.0, 360.0);

7
test_suite/FGTestApi/TestPilot.hxx
View file

@ -58,12 +58,16 @@ public:
void flyHeading(double hdg);
void flyDirectTo(const SGGeod& target);
void flyGPSCourse(GPS *gps);
void flyGPSCourseOffset(GPS *gps, double offsetNm);
private:
enum class LateralMode
{
Heading,
Direct,
GPSCourse
GPSCourse,
GPSOffset
};
void updateValues(double dt);
@ -83,6 +87,7 @@ private:
LateralMode _lateralMode = LateralMode::Heading;
SGGeod _targetPos;
GPS* _gps = nullptr;
double _courseOffsetNm =0.0;
SGPropertyNode_ptr _latProp;
SGPropertyNode_ptr _lonProp;

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

@ -649,7 +649,7 @@ void GPSTests::testOverflightSequencing()
{
// check that we sequence as we pass over the top
FGTestApi::setUp::logPositionToKML("gps_sequence");
// FGTestApi::setUp::logPositionToKML("gps_sequence");
auto rm = globals->get_subsystem<FGRouteMgr>();
auto fp = new FlightPlan;
rm->setFlightPlan(fp);
@ -660,7 +660,7 @@ void GPSTests::testOverflightSequencing()
FGTestApi::setUp::populateFPWithoutNasal(fp, "NZCH", "02", "NZAA", "05L",
"ALADA NS WB WN MAMOD KAPTI OH");
FGTestApi::writeFlightPlanToKML(fp);
// FGTestApi::writeFlightPlanToKML(fp);
// takes the place of the Nasal delegates
auto testDelegate = new TestFPDelegate;
@ -747,7 +747,7 @@ void GPSTests::testOffcourseSequencing()
// ensure that if we pass through the overflight arm cone, but not
// over the waypoint direclty, we still sequence at the mid-point
FGTestApi::setUp::logPositionToKML("gps_sequence_off");
// FGTestApi::setUp::logPositionToKML("gps_sequence_off");
auto rm = globals->get_subsystem<FGRouteMgr>();
auto fp = new FlightPlan;
rm->setFlightPlan(fp);
@ -756,7 +756,7 @@ void GPSTests::testOffcourseSequencing()
FGTestApi::setUp::populateFPWithoutNasal(fp, "NZCH", "02", "NZAA", "05L",
"ALADA NS WB WN MAMOD KAPTI OH");
FGTestApi::writeFlightPlanToKML(fp);
// FGTestApi::writeFlightPlanToKML(fp);
// takes the place of the Nasal delegates
auto testDelegate = new TestFPDelegate;
@ -893,6 +893,91 @@ void GPSTests::testOffsetFlight()
// verify the XTK value during waypoint transitions, especially that we
// don't get any weird discontinuities
// FGTestApi::setUp::logPositionToKML("gps_offset_flight");
auto rm = globals->get_subsystem<FGRouteMgr>();
auto fp = new FlightPlan;
rm->setFlightPlan(fp);
FGTestApi::setUp::populateFPWithoutNasal(fp, "UHMA", "01", "PAMR", "25",
"SOMEG BE KIVAK BET");
// FGTestApi::writeFlightPlanToKML(fp);
// takes the place of the Nasal delegates
auto testDelegate = new TestFPDelegate;
testDelegate->thePlan = fp;
CPPUNIT_ASSERT(rm->activate());
fp->addDelegate(testDelegate);
auto gps = setupStandardGPS();
auto gpsNode = globals->get_props()->getNode("instrumentation/gps");
gpsNode->setStringValue("command", "leg");
gpsNode->setDoubleValue("config/over-flight-distance-nm", 0.01);
// enroute to BERINGOVSKY NDB
fp->setCurrentIndex(2);
SGGeod pos = fp->pointAlongRoute(2, -10.0);
pos = SGGeodesy::direct(pos, 180, 2.0 * SG_NM_TO_METER);
setPositionAndStabilise(gps, pos);
auto pilot = SGSharedPtr<FGTestApi::TestPilot>(new FGTestApi::TestPilot);
pilot->resetAtPosition(pos);
pilot->setSpeedKts(300); // decent speed to make things tougher
// start facing the right way
pilot->setCourseTrue(gpsNode->getDoubleValue("wp/leg-true-course-deg"));
pilot->flyGPSCourseOffset(gps, 0.8);
FGTestApi::runForTime(90.0);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.8, gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.1);
bool ok = FGTestApi::runForTimeWithCheck(180.0, [fp] () {
if (fp->currentIndex() == 3) {
return true;
}
return false;
});
CPPUNIT_ASSERT(ok);
FGTestApi::runForTime(120.0);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.8, gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.1);
// fly until we cross the awkward line :)
ok = FGTestApi::runForTimeWithCheck(6000.0, [] () {
const double lon = globals->get_aircraft_position().getLongitudeDeg();
if ((lon > -178.0) && (lon < -170.0)) {
return true;
}
return false;
});
CPPUNIT_ASSERT(ok);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.8, gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.1);
// and try an inside corrner
fp->setCurrentIndex(3);
pos = fp->pointAlongRoute(3, -18.0);
pos = SGGeodesy::direct(pos, 180, 2.0 * SG_NM_TO_METER);
setPositionAndStabilise(gps, pos);
pilot->resetAtPosition(pos);
// start facing the right way
pilot->setCourseTrue(gpsNode->getDoubleValue("wp/leg-true-course-deg"));
FGTestApi::runForTime(120.0);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.8, gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.1);
ok = FGTestApi::runForTimeWithCheck(180.0, [fp] () {
if (fp->currentIndex() == 4) {
return true;
}
return false;
});
CPPUNIT_ASSERT(ok);
FGTestApi::runForTime(120.0);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.8, gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.1);
}
void GPSTests::testTurnAnticipation()