diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx index d7717e33f..81a78768a 100644 --- a/src/Instrumentation/gps.cxx +++ b/src/Instrumentation/gps.cxx @@ -492,7 +492,7 @@ GPS::updateBasicData(double dt) double vertical_speed_mpm = ((_indicated_pos.getElevationM() - _last_pos.getElevationM()) * 60 / dt); _last_vertical_speed = vertical_speed_mpm * SG_METER_TO_FEET; - speed_kt = fgGetLowPass(_last_speed_kts, speed_kt, dt/10.0); + speed_kt = fgGetLowPass(_last_speed_kts, speed_kt, dt/5.0); _last_speed_kts = speed_kt; SGGeod g = _indicated_pos; diff --git a/test_suite/FGTestApi/CMakeLists.txt b/test_suite/FGTestApi/CMakeLists.txt index 045ba7dde..7d3e50847 100644 --- a/test_suite/FGTestApi/CMakeLists.txt +++ b/test_suite/FGTestApi/CMakeLists.txt @@ -4,6 +4,7 @@ set(TESTSUITE_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/NavDataCache.cxx ${CMAKE_CURRENT_SOURCE_DIR}/PrivateAccessorFDM.cxx ${CMAKE_CURRENT_SOURCE_DIR}/scene_graph.cxx + ${CMAKE_CURRENT_SOURCE_DIR}/TestPilot.cxx PARENT_SCOPE ) @@ -14,5 +15,6 @@ set(TESTSUITE_HEADERS ${CMAKE_CURRENT_SOURCE_DIR}/NavDataCache.hxx ${CMAKE_CURRENT_SOURCE_DIR}/PrivateAccessorFDM.hxx ${CMAKE_CURRENT_SOURCE_DIR}/scene_graph.hxx + ${CMAKE_CURRENT_SOURCE_DIR}/TestPilot.hxx PARENT_SCOPE ) diff --git a/test_suite/FGTestApi/TestPilot.cxx b/test_suite/FGTestApi/TestPilot.cxx new file mode 100644 index 000000000..44461c754 --- /dev/null +++ b/test_suite/FGTestApi/TestPilot.cxx @@ -0,0 +1,115 @@ +/* + * Copyright (C) 2019 James Turner + * + * This file is part of the program FlightGear. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +#include "TestPilot.hxx" + +#include <simgear/math/SGGeodesy.hxx> +#include <simgear/props/props.hxx> +#include <simgear/math/SGGeod.hxx> + +#include <Main/globals.hxx> + +namespace FGTestApi { + +TestPilot::TestPilot(SGPropertyNode_ptr props) : + _propRoot(props) +{ + if (!_propRoot) { + // use default properties + _propRoot = globals->get_props(); + } + + globals->add_subsystem("flight", this, SGSubsystemMgr::FDM); +} + +TestPilot::~TestPilot() +{ +} + +void TestPilot::resetAtPosition(const SGGeod& pos) +{ + _turnActive = false; + setPosition(pos); +} + +void TestPilot::init() +{ + +} + +void TestPilot::update(double dt) +{ + updateValues(dt); +} + +void TestPilot::setSpeedKts(double knots) +{ + _speedKnots = knots; +} + +void TestPilot::setCourseTrue(double deg) +{ + _trueCourseDeg = deg; +} + +void TestPilot::turnToCourse(double deg) +{ + _turnActive = true; + _targetCourseDeg = deg; +} + +void TestPilot::updateValues(double dt) +{ + if (_turnActive) { + if (fabs(_targetCourseDeg - _trueCourseDeg) < 0.1) { + _trueCourseDeg = _targetCourseDeg; + _turnActive = false; + } else { + // standard 2-minute turn, 180-deg min, thus 3-degrees per second + double turnDeg = 3.0 * dt; + double errorDeg = _targetCourseDeg - _trueCourseDeg; + if (fabs(errorDeg) > 180.0) { + errorDeg = 360.0 - errorDeg; + } + + turnDeg = copysign(turnDeg, errorDeg); + // simple integral + _trueCourseDeg += std::min(turnDeg, errorDeg); + } + } + + SGGeod currentPos = globals->get_aircraft_position(); + double d = _speedKnots * SG_KT_TO_MPS * dt; + SGGeod newPos = SGGeodesy::direct(currentPos, _trueCourseDeg, d); + + setPosition(newPos); +} + +void TestPilot::setPosition(const SGGeod& pos) +{ + _propRoot->setDoubleValue("position/latitude-deg", pos.getLatitudeDeg()); + _propRoot->setDoubleValue("position/longitude-deg", pos.getLongitudeDeg()); + _propRoot->setDoubleValue("position/altitude-ft", pos.getElevationFt()); + + _propRoot->setDoubleValue("orientation/heading-deg", _trueCourseDeg); + _propRoot->setDoubleValue("velocities/speed-knots", _speedKnots); + +} + +} // of namespace diff --git a/test_suite/FGTestApi/TestPilot.hxx b/test_suite/FGTestApi/TestPilot.hxx new file mode 100644 index 000000000..0ec3eebf1 --- /dev/null +++ b/test_suite/FGTestApi/TestPilot.hxx @@ -0,0 +1,75 @@ +/* + * Copyright (C) 2019 James Turner + * + * This file is part of the program FlightGear. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +#ifndef _FGTEST_API_TEST_PILOT_HXX +#define _FGTEST_API_TEST_PILOT_HXX + +#include <simgear/props/propsfwd.hxx> +#include <simgear/math/SGMathFwd.hxx> +#include <simgear/structure/subsystem_mgr.hxx> + +namespace FGTestApi { + +/** + * @brief simulation of the user (pilot) fying in a particular way + * around the world. Standard property tree values are updated + * (position / orientation / velocity) + */ + class TestPilot : public SGSubsystem +{ +public: + TestPilot(SGPropertyNode_ptr props = {}); + ~TestPilot(); + + void resetAtPosition(const SGGeod& pos); + + void setSpeedKts(double knots); + + void setCourseTrue(double deg); + void turnToCourse(double deg); + + void setVerticalFPM(double fpm); + void setTargetAltitudeFtMSL(double altFt); + + // flyGreatCircle ... + + // void setTurnRateDegSec(); + + void init() override; + void update(double dT) override; + +private: + void updateValues(double dt); + void setPosition(const SGGeod& pos); + + SGPropertyNode_ptr _propRoot; + + double _trueCourseDeg = 0.0; + double _speedKnots = 0.0; + double _vspeedFPM = 0.0; + + bool _turnActive = false; + double _targetCourseDeg = 0.0; + double _targetAltitudeFt = 0.0; +}; + +} // of namespace FGTestApi + +#endif // of _FGTEST_API_TEST_PILOT_HXX + diff --git a/test_suite/FGTestApi/globals.cxx b/test_suite/FGTestApi/globals.cxx index 4b8bf0972..a6333a918 100644 --- a/test_suite/FGTestApi/globals.cxx +++ b/test_suite/FGTestApi/globals.cxx @@ -66,6 +66,16 @@ void setPosition(const SGGeod& g) globals->get_props()->setDoubleValue("position/altitude-ft", g.getElevationFt()); } +void runForTime(double t) +{ + int ticks = t * 120.0; + assert(ticks > 0); + + for (int t = 0; t < ticks; ++t) { + globals->get_subsystem_mgr()->update(1 / 120.0); + } +} + namespace tearDown { void shutdownTestGlobals() diff --git a/test_suite/FGTestApi/globals.hxx b/test_suite/FGTestApi/globals.hxx index 947e5eb08..e3d36489a 100644 --- a/test_suite/FGTestApi/globals.hxx +++ b/test_suite/FGTestApi/globals.hxx @@ -15,6 +15,8 @@ void initTestGlobals(const std::string& testName); void setPosition(const SGGeod& g); +void runForTime(double t); + namespace tearDown { void shutdownTestGlobals(); diff --git a/test_suite/unit_tests/Instrumentation/test_gps.cxx b/test_suite/unit_tests/Instrumentation/test_gps.cxx index daaf020ee..fd118db03 100644 --- a/test_suite/unit_tests/Instrumentation/test_gps.cxx +++ b/test_suite/unit_tests/Instrumentation/test_gps.cxx @@ -24,6 +24,7 @@ #include "test_suite/FGTestApi/globals.hxx" #include "test_suite/FGTestApi/NavDataCache.hxx" +#include "test_suite/FGTestApi/TestPilot.hxx" #include <Navaids/NavDataCache.hxx> #include <Navaids/navrecord.hxx> @@ -32,19 +33,6 @@ #include <Instrumentation/gps.hxx> #include <Instrumentation/navradio.hxx> -namespace { - - class GPSShutdowner { - public: - GPSShutdowner(GPS* g) : gps(g) { } - ~GPSShutdowner() { - gps->unbind(); - } - private: - GPS* gps; - }; -} - // Set up function for each test. void GPSTests::setUp() { @@ -52,7 +40,6 @@ void GPSTests::setUp() FGTestApi::setUp::initNavDataCache(); } - // Clean up after each test. void GPSTests::tearDown() { @@ -67,15 +54,15 @@ void GPSTests::setPositionAndStabilise(GPS* gps, const SGGeod& g) } } -std::unique_ptr<GPS> GPSTests::setupStandardGPS(SGPropertyNode_ptr config, - const std::string name, const int index) +GPS* GPSTests::setupStandardGPS(SGPropertyNode_ptr config, + const std::string name, const int index) { SGPropertyNode_ptr configNode(config.valid() ? config : SGPropertyNode_ptr{new SGPropertyNode}); configNode->setStringValue("name", name); configNode->setIntValue("number", index); - std::unique_ptr<GPS> gps(new GPS(configNode)); + GPS* gps(new GPS(configNode)); SGPropertyNode_ptr node = globals->get_props()->getNode("instrumentation", true)->getChild(name, index, true); node->setBoolValue("serviceable", true); @@ -84,35 +71,52 @@ std::unique_ptr<GPS> GPSTests::setupStandardGPS(SGPropertyNode_ptr config, gps->bind(); gps->init(); + globals->add_subsystem("gps", gps, SGSubsystemMgr::POST_FDM); return gps; } void GPSTests::testBasic() { auto gps = setupStandardGPS(); - GPSShutdowner gs(gps.get()); FGPositioned::TypeFilter f{FGPositioned::VOR}; auto bodrumVOR = fgpositioned_cast<FGNavRecord>(FGPositioned::findClosestWithIdent("BDR", SGGeod::fromDeg(27.6, 37), &f)); SGGeod p1 = SGGeodesy::direct(bodrumVOR->geod(), 45.0, 5.0 * SG_NM_TO_METER); - setPositionAndStabilise(gps.get(), p1); + setPositionAndStabilise(gps, p1); auto gpsNode = globals->get_props()->getNode("instrumentation/gps"); CPPUNIT_ASSERT_DOUBLES_EQUAL(p1.getLongitudeDeg(), gpsNode->getDoubleValue("indicated-longitude-deg"), 0.01); CPPUNIT_ASSERT_DOUBLES_EQUAL(p1.getLatitudeDeg(), gpsNode->getDoubleValue("indicated-latitude-deg"), 0.01); + + auto pilot = SGSharedPtr<FGTestApi::TestPilot>(new FGTestApi::TestPilot); + pilot->setSpeedKts(120); + pilot->setCourseTrue(225.0); + + FGTestApi::runForTime(30.0); + + CPPUNIT_ASSERT_DOUBLES_EQUAL(225, gpsNode->getDoubleValue("indicated-track-true-deg"), 0.5); + CPPUNIT_ASSERT_DOUBLES_EQUAL(120, gpsNode->getDoubleValue("indicated-ground-speed-kt"), 1); + + // 120kts = + double speedMSec = 120 * SG_KT_TO_MPS; + double components = speedMSec * (1.0 / sqrt(2.0)); + CPPUNIT_ASSERT_DOUBLES_EQUAL(-components, gpsNode->getDoubleValue("ew-velocity-msec"), 0.1); + CPPUNIT_ASSERT_DOUBLES_EQUAL(-components, gpsNode->getDoubleValue("ns-velocity-msec"), 0.1); + + CPPUNIT_ASSERT_DOUBLES_EQUAL(120 * (30.0 / 3600), gpsNode->getDoubleValue("odometer"), 0.1); + CPPUNIT_ASSERT_DOUBLES_EQUAL(120 * (30.0 / 3600), gpsNode->getDoubleValue("trip-odometer"), 0.1); } void GPSTests::testOBSMode() { auto gps = setupStandardGPS(); - GPSShutdowner gs(gps.get()); FGPositioned::TypeFilter f{FGPositioned::VOR}; auto bodrumVOR = fgpositioned_cast<FGNavRecord>(FGPositioned::findClosestWithIdent("BDR", SGGeod::fromDeg(27.6, 37), &f)); SGGeod p1 = SGGeodesy::direct(bodrumVOR->geod(), 45.0, 5.0 * SG_NM_TO_METER); - setPositionAndStabilise(gps.get(), p1); + setPositionAndStabilise(gps, p1); auto gpsNode = globals->get_props()->getNode("instrumentation/gps"); CPPUNIT_ASSERT_DOUBLES_EQUAL(p1.getLongitudeDeg(), gpsNode->getDoubleValue("indicated-longitude-deg"), 0.01); @@ -132,7 +136,7 @@ void GPSTests::testOBSMode() // select OBS mode one it gpsNode->setStringValue("command", "obs"); - setPositionAndStabilise(gps.get(), p1); + setPositionAndStabilise(gps, p1); CPPUNIT_ASSERT_EQUAL(std::string{"obs"}, std::string{gpsNode->getStringValue("mode")}); CPPUNIT_ASSERT_DOUBLES_EQUAL(5.0, gpsNode->getDoubleValue("wp/wp[1]/distance-nm"), 0.01); @@ -141,11 +145,38 @@ void GPSTests::testOBSMode() CPPUNIT_ASSERT_DOUBLES_EQUAL(bodrumVOR->get_lat(), gpsNode->getDoubleValue("wp/wp[1]/latitude-deg"), 0.01); CPPUNIT_ASSERT_DOUBLES_EQUAL(225.0, gpsNode->getDoubleValue("desired-course-deg"), 0.01); - - CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.1); CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.05); + // off axis, angular + SGGeod p2 = SGGeodesy::direct(bodrumVOR->geod(), 40.0, 4.0 * SG_NM_TO_METER); + setPositionAndStabilise(gps, p2); + + CPPUNIT_ASSERT_EQUAL(std::string{"obs"}, std::string{gpsNode->getStringValue("mode")}); + CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, gpsNode->getDoubleValue("wp/wp[1]/distance-nm"), 0.01); + CPPUNIT_ASSERT_DOUBLES_EQUAL(220.0, gpsNode->getDoubleValue("wp/wp[1]/bearing-true-deg"), 0.5); + CPPUNIT_ASSERT_DOUBLES_EQUAL(225.0, gpsNode->getDoubleValue("desired-course-deg"), 0.01); + CPPUNIT_ASSERT_DOUBLES_EQUAL(-5.0, gpsNode->getDoubleValue("wp/wp[1]/course-deviation-deg"), 0.1); + + // off axis, perpendicular + SGGeod p3 = SGGeodesy::direct(p1, 135, 0.5 * SG_NM_TO_METER); + setPositionAndStabilise(gps, p3); + + CPPUNIT_ASSERT_DOUBLES_EQUAL(225.0, gpsNode->getDoubleValue("desired-course-deg"), 0.01); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.5, gpsNode->getDoubleValue("wp/wp[1]/course-error-nm"), 0.05); +} + +void GPSTests::testDirectTo() +{ + auto gps = setupStandardGPS(); + + + + FGPositioned::TypeFilter f{FGPositioned::VOR}; + auto bodrumVOR = fgpositioned_cast<FGNavRecord>(FGPositioned::findClosestWithIdent("BDR", SGGeod::fromDeg(27.6, 37), &f)); + SGGeod p1 = SGGeodesy::direct(bodrumVOR->geod(), 45.0, 5.0 * SG_NM_TO_METER); + + setPositionAndStabilise(gps, p1); } void GPSTests::testNavRadioSlave() diff --git a/test_suite/unit_tests/Instrumentation/test_gps.hxx b/test_suite/unit_tests/Instrumentation/test_gps.hxx index f129d04e8..fe039e64a 100644 --- a/test_suite/unit_tests/Instrumentation/test_gps.hxx +++ b/test_suite/unit_tests/Instrumentation/test_gps.hxx @@ -42,12 +42,13 @@ class GPSTests : public CppUnit::TestFixture CPPUNIT_TEST(testConfigAutopilotDrive); CPPUNIT_TEST(testTurnAnticipation); CPPUNIT_TEST(testOBSMode); + CPPUNIT_TEST(testDirectTo); CPPUNIT_TEST_SUITE_END(); void setPositionAndStabilise(GPS* gps, const SGGeod& g); - std::unique_ptr<GPS> setupStandardGPS(SGPropertyNode_ptr config = {}, + GPS* setupStandardGPS(SGPropertyNode_ptr config = {}, const std::string name = "gps", const int index = 0); public: @@ -63,6 +64,7 @@ public: void testConfigAutopilotDrive(); void testTurnAnticipation(); void testOBSMode(); + void testDirectTo(); }; #endif // _FG_GPS_UNIT_TESTS_HXX