diff --git a/test_suite/FGTestApi/TestPilot.cxx b/test_suite/FGTestApi/TestPilot.cxx
index aa4e32695..0cf8b7a89 100644
--- a/test_suite/FGTestApi/TestPilot.cxx
+++ b/test_suite/FGTestApi/TestPilot.cxx
@@ -35,6 +35,21 @@ TestPilot::TestPilot(SGPropertyNode_ptr props) :
_propRoot = globals->get_props();
}
+ _latProp = _propRoot->getNode("position/latitude-deg", true);
+ _lonProp = _propRoot->getNode("position/longitude-deg", true);
+ _altitudeProp = _propRoot->getNode("position/altitude-ft", true);
+ _headingProp = _propRoot->getNode("orientation/heading-deg", true);
+ _speedKnotsProp = _propRoot->getNode("velocities/speed-knots", true);
+ _verticalFPMProp = _propRoot->getNode("velocities/vertical-fpm", true);
+
+
+ SGPropertyNode_ptr _latProp;
+ SGPropertyNode_ptr _lonProp;
+ SGPropertyNode_ptr _altitudeProp;
+ SGPropertyNode_ptr _headingProp;
+ SGPropertyNode_ptr _speedKnotsProp;
+ SGPropertyNode_ptr _verticalFPMProp;
+
globals->add_subsystem("flight", this, SGSubsystemMgr::FDM);
}
@@ -73,9 +88,48 @@ void TestPilot::turnToCourse(double deg)
_turnActive = true;
_targetCourseDeg = deg;
}
+
+void TestPilot::flyHeading(double hdg)
+{
+ _lateralMode = LateralMode::Heading;
+ _turnActive = true;
+ _targetCourseDeg = hdg;
+}
+
+void TestPilot::flyGPSCourse(GPS *gps)
+{
+ _gps = gps;
+ _gpsNode = globals->get_props()->getNode("instrumentation/gps");
+ _lateralMode = LateralMode::GPSCourse;
+ _turnActive = false;
+}
+
+void TestPilot::flyDirectTo(const SGGeod& target)
+{
+ _lateralMode = LateralMode::Direct;
+ _targetPos = target;
+}
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");
+ _targetCourseDeg -= deviationDeg;
+ 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);
+ if (!_turnActive && (fabs(_trueCourseDeg - _targetCourseDeg) > 0.5)) {
+ _turnActive = true;
+ }
+ }
+
if (_turnActive) {
if (fabs(_targetCourseDeg - _trueCourseDeg) < 0.1) {
_trueCourseDeg = _targetCourseDeg;
@@ -84,13 +138,16 @@ void TestPilot::updateValues(double dt)
// 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;
+ if (errorDeg > 180.0) {
+ errorDeg = errorDeg -= 360;
+ } else if (errorDeg < -180) {
+ errorDeg += 360.0;
}
turnDeg = copysign(turnDeg, errorDeg);
// simple integral
_trueCourseDeg += std::min(turnDeg, errorDeg);
+ SG_NORMALIZE_RANGE(_trueCourseDeg, 0.0, 360.0);
}
}
@@ -115,14 +172,13 @@ void TestPilot::updateValues(double dt)
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());
+ _latProp->setDoubleValue(pos.getLatitudeDeg());
+ _lonProp->setDoubleValue(pos.getLongitudeDeg());
+ _altitudeProp->setDoubleValue(pos.getElevationFt());
- _propRoot->setDoubleValue("orientation/heading-deg", _trueCourseDeg);
- _propRoot->setDoubleValue("velocities/speed-knots", _speedKnots);
- _propRoot->setDoubleValue("velocities/vertical-fpm", _vspeedFPM);
-
+ _headingProp->setDoubleValue(_trueCourseDeg);
+ _speedKnotsProp->setDoubleValue(_speedKnots);
+ _verticalFPMProp->setDoubleValue(_vspeedFPM);
}
void TestPilot::setTargetAltitudeFtMSL(double altFt)
diff --git a/test_suite/FGTestApi/TestPilot.hxx b/test_suite/FGTestApi/TestPilot.hxx
index c15b443c7..f4040aa7a 100644
--- a/test_suite/FGTestApi/TestPilot.hxx
+++ b/test_suite/FGTestApi/TestPilot.hxx
@@ -23,6 +23,9 @@
#include <simgear/props/propsfwd.hxx>
#include <simgear/math/SGMathFwd.hxx>
#include <simgear/structure/subsystem_mgr.hxx>
+#include <simgear/math/SGGeod.hxx>
+
+class GPS;
namespace FGTestApi {
@@ -47,14 +50,22 @@ public:
void setVerticalFPM(double fpm);
void setTargetAltitudeFtMSL(double altFt);
- // flyGreatCircle ...
-
// void setTurnRateDegSec();
void init() override;
void update(double dT) override;
+ void flyHeading(double hdg);
+ void flyDirectTo(const SGGeod& target);
+ void flyGPSCourse(GPS *gps);
private:
+ enum class LateralMode
+ {
+ Heading,
+ Direct,
+ GPSCourse
+ };
+
void updateValues(double dt);
void setPosition(const SGGeod& pos);
@@ -68,6 +79,19 @@ private:
bool _altActive = false;
double _targetCourseDeg = 0.0;
double _targetAltitudeFt = 0.0;
+
+ LateralMode _lateralMode = LateralMode::Heading;
+ SGGeod _targetPos;
+ GPS* _gps = nullptr;
+
+ SGPropertyNode_ptr _latProp;
+ SGPropertyNode_ptr _lonProp;
+ SGPropertyNode_ptr _altitudeProp;
+ SGPropertyNode_ptr _headingProp;
+ SGPropertyNode_ptr _speedKnotsProp;
+ SGPropertyNode_ptr _verticalFPMProp;
+
+ SGPropertyNode_ptr _gpsNode;
};
} // of namespace FGTestApi
diff --git a/test_suite/FGTestApi/globals.cxx b/test_suite/FGTestApi/globals.cxx
index d66da0d47..b4d73972a 100644
--- a/test_suite/FGTestApi/globals.cxx
+++ b/test_suite/FGTestApi/globals.cxx
@@ -4,12 +4,16 @@
#include "globals.hxx"
+#include <simgear/io/iostreams/sgstream.hxx>
+
#if defined(HAVE_QT)
#include <GUI/QtLauncher.hxx>
#endif
#include <Main/globals.hxx>
#include <Main/options.hxx>
+#include <Main/util.hxx>
+#include <Main/FGInterpolator.hxx>
#include <Time/TimeManager.hxx>
@@ -21,14 +25,21 @@
#include <Navaids/FlightPlan.hxx>
#include <Navaids/waypoint.hxx>
+#include <Scripting/NasalSys.hxx>
+
using namespace flightgear;
namespace FGTestApi {
+bool global_loggingToKML = false;
+sg_ofstream global_kmlStream;
+bool global_lineStringOpen = false;
+
namespace setUp {
void initTestGlobals(const std::string& testName)
{
+ assert(globals == nullptr);
globals = new FGGlobals;
DataStore &data = DataStore::get();
@@ -44,6 +55,18 @@ void initTestGlobals(const std::string& testName)
}
globals->set_fg_home(homePath);
+
+ auto props = globals->get_props();
+ props->setStringValue("sim/fg-home", homePath.utf8Str());
+ props->setStringValue("sim/flight-model", "null");
+ props->setStringValue("sim/aircraft", "test-suite-aircraft");
+
+ props->setDoubleValue("sim/current-view/config/default-field-of-view-deg", 90.0);
+ // ensure /sim/view/config exists
+ props->setBoolValue("sim/view/config/foo", false);
+
+ props->setBoolValue("sim/rendering/precipitation-gui-enable", false);
+ props->setBoolValue("sim/rendering/precipitation-aircraft-enable", false);
// Activate headless mode.
globals->set_headless(true);
@@ -61,9 +84,50 @@ void initTestGlobals(const std::string& testName)
* destroyed via the subsystem manager.
*/
globals->add_subsystem("events", globals->get_event_mgr(), SGSubsystemMgr::DISPLAY);
+
+ // Nasal needs the interpolator running
+ globals->add_subsystem("prop-interpolator", new FGInterpolator, SGSubsystemMgr::INIT);
+
+}
+
+bool logPositionToKML(const std::string& testName)
+{
+ SGPath p = SGPath::desktop() / (testName + ".kml");
+ global_kmlStream.open(p);
+ if (!global_kmlStream.is_open()) {
+ SG_LOG(SG_GENERAL, SG_WARN, "unable to open:" << p);
+ return false;
+ }
+
+ // pre-amble
+ global_kmlStream << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
+ "<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n"
+ "<Document>\n";
+ // need more precision for doubles when specifying lat/lon, see
+ // https://xkcd.com/2170/ :)
+ global_kmlStream.precision(12);
+
+ global_loggingToKML = true;
+ return true;
+}
+
+void initStandardNasal()
+{
+ fgInitAllowedPaths();
+
+ auto nasalNode = globals->get_props()->getNode("nasal", true);
+
+// disable various larger modules
+ nasalNode->setBoolValue("canvas/enabled", false);
+ nasalNode->setBoolValue("jetways/enabled", false);
+ nasalNode->setBoolValue("jetways_edit/enabled", false);
+ nasalNode->setBoolValue("local_weather/enabled", false);
+
+ // will be inited, since we already did that
+ globals->add_new_subsystem<FGNasalSys>(SGSubsystemMgr::INIT);
}
-void populateFP(flightgear::FlightPlanRef f,
+void populateFPWithoutNasal(flightgear::FlightPlanRef f,
const std::string& depICAO, const std::string& depRunway,
const std::string& destICAO, const std::string& destRunway,
const std::string& waypoints)
@@ -91,12 +155,70 @@ void populateFP(flightgear::FlightPlanRef f,
destRwy->ident() + "-8", f), -1);
f->insertWayptAtIndex(new RunwayWaypt(destRwy, f), -1);
}
+
+void populateFPWithNasal(flightgear::FlightPlanRef f,
+ const std::string& depICAO, const std::string& depRunway,
+ const std::string& destICAO, const std::string& destRunway,
+ const std::string& waypoints)
+{
+ FGAirportRef depApt = FGAirport::getByIdent(depICAO);
+ f->setDeparture(depApt->getRunwayByIdent(depRunway));
+
+ FGAirportRef destApt = FGAirport::getByIdent(destICAO);
+ f->setDestination(destApt->getRunwayByIdent(destRunway));
+
+ // insert after the last departure waypoint
+ int insertIndex = 1;
+
+ for (auto ws : simgear::strutils::split(waypoints)) {
+ WayptRef wpt = f->waypointFromString(ws);
+ f->insertWayptAtIndex(wpt, insertIndex++);
+ }
+}
} // End of namespace setUp.
+
+ void beginLineString(const std::string& ident)
+ {
+ global_lineStringOpen = true;
+ global_kmlStream << "<Placemark>\n";
+ if (!ident.empty()) {
+ global_kmlStream << "<name>" << ident << "</name>\n";
+ }
+ global_kmlStream << "<LineString>\n";
+ global_kmlStream << "<tessellate>1</tessellate>\n";
+ global_kmlStream << "<coordinates>\n";
+ }
+
+ void logCoordinate(const SGGeod& pos)
+ {
+ if (!global_lineStringOpen) {
+ beginLineString({});
+ }
+
+ global_kmlStream << pos.getLongitudeDeg() << "," << pos.getLatitudeDeg() << " " << endl;
+ }
+ void endCurrentLineString()
+ {
+ global_lineStringOpen = false;
+ global_kmlStream <<
+ "</coordinates>\n"
+ "</LineString>\n"
+ "</Placemark>\n" << endl;
+ }
+
void setPosition(const SGGeod& g)
{
+ if (global_loggingToKML) {
+ if (global_lineStringOpen) {
+ endCurrentLineString();
+ }
+
+ logCoordinate(g);
+ }
+
globals->get_props()->setDoubleValue("position/latitude-deg", g.getLatitudeDeg());
globals->get_props()->setDoubleValue("position/longitude-deg", g.getLongitudeDeg());
globals->get_props()->setDoubleValue("position/altitude-ft", g.getElevationFt());
@@ -109,9 +231,46 @@ void runForTime(double t)
for (int t = 0; t < ticks; ++t) {
globals->get_subsystem_mgr()->update(1 / 120.0);
+ if (global_loggingToKML) {
+ logCoordinate(globals->get_aircraft_position());
+ }
}
}
+bool runForTimeWithCheck(double t, RunCheck check)
+{
+ const int tickHz = 30;
+ const double tickDuration = 1.0 / tickHz;
+
+ int ticks = static_cast<int>(t * tickHz);
+ assert(ticks > 0);
+ const int logInterval = 2 * tickHz; // every two seconds
+ int nextLog = 0;
+
+ for (int t = 0; t < ticks; ++t) {
+ globals->get_subsystem_mgr()->update(tickDuration);
+
+ if (global_loggingToKML) {
+ if (nextLog == 0) {
+ logCoordinate(globals->get_aircraft_position());
+ nextLog = logInterval;
+ } else {
+ nextLog--;
+ }
+ }
+
+ bool done = check();
+ if (done) {
+ if (global_loggingToKML) {
+ endCurrentLineString();
+ }
+ return true;
+ }
+ }
+
+ return false;
+}
+
namespace tearDown {
void shutdownTestGlobals()
@@ -123,6 +282,17 @@ void shutdownTestGlobals()
#endif
delete globals;
+ globals = nullptr;
+
+ if (global_loggingToKML) {
+ if (global_lineStringOpen) {
+ endCurrentLineString();
+ }
+ // post-amble
+ global_kmlStream << "</Document>\n"
+ "</kml>" << endl;
+ global_kmlStream.close();
+ }
}
} // End of namespace tearDown.
diff --git a/test_suite/FGTestApi/globals.hxx b/test_suite/FGTestApi/globals.hxx
index 933cdaf05..032ae7099 100644
--- a/test_suite/FGTestApi/globals.hxx
+++ b/test_suite/FGTestApi/globals.hxx
@@ -2,6 +2,8 @@
#define FG_TEST_GLOBALS_HELPERS_HXX
#include <string>
+#include <functional>
+
#include <simgear/structure/SGSharedPtr.hxx>
class SGGeod;
@@ -18,10 +20,19 @@ namespace setUp {
void initTestGlobals(const std::string& testName);
- void populateFP(flightgear::FlightPlanRef f,
- const std::string& depICAO, const std::string& depRunway,
- const std::string& destICAO, const std::string& destRunway,
- const std::string& waypoints);
+bool logPositionToKML(const std::string& testName);
+
+void initStandardNasal();
+
+void populateFPWithoutNasal(flightgear::FlightPlanRef f,
+ const std::string& depICAO, const std::string& depRunway,
+ const std::string& destICAO, const std::string& destRunway,
+ const std::string& waypoints);
+
+void populateFPWithNasal(flightgear::FlightPlanRef f,
+ const std::string& depICAO, const std::string& depRunway,
+ const std::string& destICAO, const std::string& destRunway,
+ const std::string& waypoints);
} // End of namespace setUp.
@@ -29,6 +40,10 @@ void setPosition(const SGGeod& g);
void runForTime(double t);
+ using RunCheck = std::function<bool(void)>;
+
+bool runForTimeWithCheck(double t, RunCheck check);
+
namespace tearDown {
void shutdownTestGlobals();
diff --git a/test_suite/unit_tests/Navaids/CMakeLists.txt b/test_suite/unit_tests/Navaids/CMakeLists.txt
index bf5556a73..bc0bfc66d 100644
--- a/test_suite/unit_tests/Navaids/CMakeLists.txt
+++ b/test_suite/unit_tests/Navaids/CMakeLists.txt
@@ -4,6 +4,7 @@ set(TESTSUITE_SOURCES
${CMAKE_CURRENT_SOURCE_DIR}/test_flightplan.cxx
${CMAKE_CURRENT_SOURCE_DIR}/test_navaids2.cxx
${CMAKE_CURRENT_SOURCE_DIR}/test_aircraftPerformance.cxx
+ ${CMAKE_CURRENT_SOURCE_DIR}/test_routeManager.cxx
PARENT_SCOPE
)
@@ -11,5 +12,6 @@ set(TESTSUITE_HEADERS
${TESTSUITE_HEADERS}
${CMAKE_CURRENT_SOURCE_DIR}/test_flightplan.hxx
${CMAKE_CURRENT_SOURCE_DIR}/test_aircraftPerformance.hxx
+ ${CMAKE_CURRENT_SOURCE_DIR}/test_routeManager.hxx
PARENT_SCOPE
)
diff --git a/test_suite/unit_tests/Navaids/TestSuite.cxx b/test_suite/unit_tests/Navaids/TestSuite.cxx
index e4a1f93a5..9e2af8918 100644
--- a/test_suite/unit_tests/Navaids/TestSuite.cxx
+++ b/test_suite/unit_tests/Navaids/TestSuite.cxx
@@ -20,8 +20,10 @@
#include "test_flightplan.hxx"
#include "test_navaids2.hxx"
#include "test_aircraftPerformance.hxx"
+#include "test_routeManager.hxx"
// Set up the unit tests.
CPPUNIT_TEST_SUITE_NAMED_REGISTRATION(FlightplanTests, "Unit tests");
CPPUNIT_TEST_SUITE_NAMED_REGISTRATION(NavaidsTests, "Unit tests");
CPPUNIT_TEST_SUITE_NAMED_REGISTRATION(AircraftPerformanceTests, "Unit tests");
+CPPUNIT_TEST_SUITE_NAMED_REGISTRATION(RouteManagerTests, "Unit tests");
diff --git a/test_suite/unit_tests/Navaids/test_routeManager.cxx b/test_suite/unit_tests/Navaids/test_routeManager.cxx
new file mode 100644
index 000000000..af15ed8da
--- /dev/null
+++ b/test_suite/unit_tests/Navaids/test_routeManager.cxx
@@ -0,0 +1,219 @@
+#include "test_routeManager.hxx"
+
+#include <memory>
+#include <cstring>
+
+#include "test_suite/FGTestApi/globals.hxx"
+#include "test_suite/FGTestApi/NavDataCache.hxx"
+#include "test_suite/FGTestApi/TestPilot.hxx"
+
+#include <Navaids/FlightPlan.hxx>
+#include <Navaids/NavDataCache.hxx>
+#include <Navaids/navrecord.hxx>
+#include <Navaids/navlist.hxx>
+
+// we need a default GPS instrument, hard to test seperately for now
+#include <Instrumentation/gps.hxx>
+
+#include <Autopilot/route_mgr.hxx>
+
+using namespace flightgear;
+
+static FlightPlanRef makeTestFP(const std::string& depICAO, const std::string& depRunway,
+ const std::string& destICAO, const std::string& destRunway,
+ const std::string& waypoints)
+{
+ FlightPlanRef f = new FlightPlan;
+ FGTestApi::setUp::populateFPWithNasal(f, depICAO, depRunway, destICAO, destRunway, waypoints);
+ return f;
+}
+
+
+// Set up function for each test.
+void RouteManagerTests::setUp()
+{
+ FGTestApi::setUp::initTestGlobals("routemanager");
+ FGTestApi::setUp::initNavDataCache();
+
+ globals->add_new_subsystem<FGRouteMgr>();
+
+// setup the default GPS, which is needed for waypoint
+// sequencing to work
+ SGPropertyNode_ptr configNode(new SGPropertyNode);
+ configNode->setStringValue("name", "gps");
+ configNode->setIntValue("number", 0);
+ GPS* gps(new GPS(configNode, true /* default mode */));
+ m_gps = gps;
+
+ SGPropertyNode_ptr node = globals->get_props()->getNode("instrumentation", true)->getChild("gps", 0, true);
+ // node->setBoolValue("serviceable", true);
+ // globals->get_props()->setDoubleValue("systems/electrical/outputs/gps", 6.0);
+ globals->add_subsystem("gps", gps, SGSubsystemMgr::POST_FDM);
+
+ globals->get_subsystem_mgr()->bind();
+ globals->get_subsystem_mgr()->init();
+
+ FGTestApi::setUp::initStandardNasal();
+ globals->get_subsystem_mgr()->postinit();
+}
+
+
+// Clean up after each test.
+void RouteManagerTests::tearDown()
+{
+ m_gps = nullptr;
+ FGTestApi::tearDown::shutdownTestGlobals();
+}
+
+ void RouteManagerTests::setPositionAndStabilise(const SGGeod& g)
+ {
+ FGTestApi::setPosition(g);
+ for (int i=0; i<60; ++i) {
+ globals->get_subsystem_mgr()->update(0.02);
+ }
+ }
+
+void RouteManagerTests::testBasic()
+{
+ FGTestApi::setUp::logPositionToKML("rm_basic");
+
+ FlightPlanRef fp1 = makeTestFP("EGLC", "27", "EHAM", "06",
+ "CLN IDESI RINIS VALKO RIVER RTM EKROS");
+ fp1->setIdent("testplan");
+ fp1->setCruiseFlightLevel(360);
+
+ auto rm = globals->get_subsystem<FGRouteMgr>();
+ rm->setFlightPlan(fp1);
+
+ auto gpsNode = globals->get_props()->getNode("instrumentation/gps", true);
+ CPPUNIT_ASSERT(!strcmp("obs", gpsNode->getStringValue("mode")));
+
+ rm->activate();
+
+ CPPUNIT_ASSERT(fp1->isActive());
+
+ // Nasal deleagte should have placed GPS into leg mode
+ auto rmNode = globals->get_props()->getNode("autopilot/route-manager", true);
+
+ CPPUNIT_ASSERT(!strcmp("leg", gpsNode->getStringValue("mode")));
+
+ CPPUNIT_ASSERT(!strcmp("EGLC", rmNode->getStringValue("departure/airport")));
+ CPPUNIT_ASSERT(!strcmp("27", rmNode->getStringValue("departure/runway")));
+ CPPUNIT_ASSERT(!strcmp("", rmNode->getStringValue("departure/sid")));
+ CPPUNIT_ASSERT(!strcmp("London City", rmNode->getStringValue("departure/name")));
+
+ CPPUNIT_ASSERT(!strcmp("EHAM", rmNode->getStringValue("destination/airport")));
+ CPPUNIT_ASSERT(!strcmp("06", rmNode->getStringValue("destination/runway")));
+
+ CPPUNIT_ASSERT_EQUAL(360, rmNode->getIntValue("cruise/flight-level"));
+ CPPUNIT_ASSERT_EQUAL(false, rmNode->getBoolValue("airborne"));
+
+ CPPUNIT_ASSERT_EQUAL(0, rmNode->getIntValue("current-wp"));
+ auto wp0Node = rmNode->getNode("wp");
+ CPPUNIT_ASSERT(!strcmp("EGLC-27", wp0Node->getStringValue("id")));
+
+ auto wp1Node = rmNode->getNode("wp[1]");
+ CPPUNIT_ASSERT(!strcmp("CLN", wp1Node->getStringValue("id")));
+
+ FGPositioned::TypeFilter f{FGPositioned::VOR};
+ auto clactonVOR = fgpositioned_cast<FGNavRecord>(FGPositioned::findClosestWithIdent("CLN", SGGeod::fromDeg(0.0, 51.0), &f));
+
+ // verify hold entry course
+ auto pilot = SGSharedPtr<FGTestApi::TestPilot>(new FGTestApi::TestPilot);
+ FGTestApi::setPosition(fp1->departureRunway()->geod());
+ pilot->resetAtPosition(fp1->departureRunway()->geod());
+
+ pilot->setSpeedKts(220);
+ pilot->setCourseTrue(fp1->departureRunway()->headingDeg());
+ pilot->setTargetAltitudeFtMSL(10000);
+
+ bool ok = FGTestApi::runForTimeWithCheck(30.0, [rmNode] () {
+ if (rmNode->getIntValue("current-wp") == 1) return true;
+ return false;
+ });
+
+ CPPUNIT_ASSERT(ok);
+
+ // continue outbound for some time
+ pilot->setSpeedKts(250);
+ FGTestApi::runForTime(30.0);
+
+ // turn towards Clacton VOR
+ pilot->flyDirectTo(clactonVOR->geod());
+ pilot->setTargetAltitudeFtMSL(30000);
+ pilot->setSpeedKts(280);
+ ok = FGTestApi::runForTimeWithCheck(6000.0, [rmNode] () {
+ if (rmNode->getIntValue("current-wp") == 2) return true;
+ return false;
+ });
+ CPPUNIT_ASSERT(ok);
+
+ // short straight leg for testing
+ pilot->flyHeading(90.0);
+ pilot->setSpeedKts(330);
+ FGTestApi::runForTime(30.0);
+
+ // let's engage LNAV mode :)
+ pilot->flyGPSCourse(m_gps);
+
+ ok = FGTestApi::runForTimeWithCheck(6000.0, [rmNode] () {
+ if (rmNode->getIntValue("current-wp") == 5) return true;
+ return false;
+ });
+ CPPUNIT_ASSERT(ok);
+
+ // check where we are - should be heading to RIVER from VALKO
+ CPPUNIT_ASSERT_EQUAL(5, rmNode->getIntValue("current-wp"));
+ CPPUNIT_ASSERT(!strcmp("RIVER", wp0Node->getStringValue("id")));
+ CPPUNIT_ASSERT(!strcmp("RTM", wp1Node->getStringValue("id")));
+ // 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;
+ });
+ CPPUNIT_ASSERT(ok);
+
+ // run until the GPS reverts to OBS mode at the end of the flight plan
+ ok = FGTestApi::runForTimeWithCheck(6000.0, [gpsNode] () {
+ std::string mode = gpsNode->getStringValue("mode");
+ if (mode == "obs") return true;
+ return false;
+ });
+
+ CPPUNIT_ASSERT(ok);
+ CPPUNIT_ASSERT_EQUAL(-1, fp1->currentIndex());
+ CPPUNIT_ASSERT(!fp1->isActive());
+ CPPUNIT_ASSERT_EQUAL(-1, rmNode->getIntValue("current-wp"));
+ CPPUNIT_ASSERT_EQUAL(false, rmNode->getBoolValue("active"));
+}
+
+void RouteManagerTests::testDefaultSID()
+{
+ FlightPlanRef fp1 = makeTestFP("EGLC", "27", "EHAM", "24",
+ "CLN IDRID VALKO");
+ fp1->setIdent("testplan");
+
+ auto rm = globals->get_subsystem<FGRouteMgr>();
+ rm->setFlightPlan(fp1);
+
+ auto gpsNode = globals->get_props()->getNode("instrumentation/gps", true);
+ auto rmNode = globals->get_props()->getNode("autopilot/route-manager", true);
+ rmNode->setStringValue("departure/sid", "DEFAULT");
+
+ // let's see what we got :)
+
+ rm->activate();
+
+ CPPUNIT_ASSERT(fp1->isActive());
+}
+
+void RouteManagerTests::testDefaultApproach()
+{
+
+}
+
diff --git a/test_suite/unit_tests/Navaids/test_routeManager.hxx b/test_suite/unit_tests/Navaids/test_routeManager.hxx
new file mode 100644
index 000000000..e143ab466
--- /dev/null
+++ b/test_suite/unit_tests/Navaids/test_routeManager.hxx
@@ -0,0 +1,62 @@
+/*
+ * 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 _FG_ROUTE_MANAGER_UNIT_TESTS_HXX
+#define _FG_ROUTE_MANAGER_UNIT_TESTS_HXX
+
+
+#include <cppunit/extensions/HelperMacros.h>
+#include <cppunit/TestFixture.h>
+
+class SGGeod;
+class GPS;
+
+// The flight plan unit tests.
+class RouteManagerTests : public CppUnit::TestFixture
+{
+ // Set up the test suite.
+ CPPUNIT_TEST_SUITE(RouteManagerTests);
+ CPPUNIT_TEST(testBasic);
+ CPPUNIT_TEST(testDefaultSID);
+ CPPUNIT_TEST(testDefaultApproach);
+
+ CPPUNIT_TEST_SUITE_END();
+
+ // void setPositionAndStabilise(FGNavRadio* r, const SGGeod& g);
+
+public:
+ // Set up function for each test.
+ void setUp();
+
+ // Clean up after each test.
+ void tearDown();
+
+ void setPositionAndStabilise(const SGGeod& g);
+
+ // The tests.
+ void testBasic();
+ void testDefaultSID();
+ void testDefaultApproach();
+
+private:
+ GPS* m_gps = nullptr;
+};
+
+#endif // _FG_ROUTE_MANAGER_UNIT_TESTS_HXX