diff --git a/src/Autopilot/route_mgr.cxx b/src/Autopilot/route_mgr.cxx index 40faf6219..dbb0ca921 100644 --- a/src/Autopilot/route_mgr.cxx +++ b/src/Autopilot/route_mgr.cxx @@ -121,29 +121,40 @@ static bool commandSetActiveWaypt(const SGPropertyNode* arg, SGPropertyNode *) static bool commandInsertWaypt(const SGPropertyNode* arg, SGPropertyNode *) { - FGRouteMgr* self = (FGRouteMgr*) globals->get_subsystem("route-manager"); - const bool haveIndex = arg->hasChild("index"); - int index = arg->getIntValue("index"); + FGRouteMgr* self = globals->get_subsystem(); + const bool haveIndex = arg->hasChild("index"); + int index = arg->getIntValue("index"); - std::string ident(arg->getStringValue("id")); - int alt = arg->getIntValue("altitude-ft", -999); - int ias = arg->getIntValue("speed-knots", -999); - - WayptRef wp; -// lat/lon may be supplied to narrow down navaid search, or to specify -// a raw waypoint - SGGeod pos; - if (arg->hasChild("longitude-deg")) { - pos = SGGeod::fromDeg(arg->getDoubleValue("longitude-deg"), - arg->getDoubleValue("latitude-deg")); + std::string ident(arg->getStringValue("id")); + int alt = arg->getIntValue("altitude-ft", -999); + int ias = arg->getIntValue("speed-knots", -999); + + WayptRef wp; + // lat/lon may be supplied to narrow down navaid search, or to specify + // a raw waypoint + SGGeod pos = SGGeod::invalid(); + if (arg->hasChild("longitude-deg")) { + pos = SGGeod::fromDeg(arg->getDoubleValue("longitude-deg"), + arg->getDoubleValue("latitude-deg")); } if (arg->hasChild("navaid")) { - FGPositionedRef p = FGPositioned::findClosestWithIdent(arg->getStringValue("navaid"), pos); - + if (!pos.isValid()) { + pos = self->flightPlan()->vicinityForInsertIndex(haveIndex ? index : -1 /* append */); + } + + FGPositioned::TypeFilter filter({FGPositioned::Type::NDB, FGPositioned::Type::VOR, + FGPositioned::Type::FIX, FGPositioned::Type::WAYPOINT}); + + FGPositionedRef p = FGPositioned::findClosestWithIdent(arg->getStringValue("navaid"), pos, &filter); + if (!p) { + SG_LOG(SG_AUTOPILOT, SG_WARN, "Unable to find navaid with ident:" << arg->getStringValue("navaid")); + return false; + } + if (arg->hasChild("navaid", 1)) { // intersection of two radials - FGPositionedRef p2 = FGPositioned::findClosestWithIdent(arg->getStringValue("navaid[1]"), pos); + FGPositionedRef p2 = FGPositioned::findClosestWithIdent(arg->getStringValue("navaid[1]"), pos, &filter); if (!p2) { SG_LOG( SG_AUTOPILOT, SG_INFO, "Unable to find FGPositioned with ident:" << arg->getStringValue("navaid[1]")); return false; @@ -1200,24 +1211,7 @@ void FGRouteMgr::setSTAR(const std::string& aIdent) WayptRef FGRouteMgr::waypointFromString(const std::string& target, int insertPosition) { - SGGeod vicinity; - if (insertPosition < 0) { // appending, not inserting - insertPosition = _plan->numLegs(); - if (insertPosition > 0) { - // if we have at least one existing leg, use its position - // for the search vicinity - vicinity = _plan->pointAlongRoute(insertPosition - 1, 0.0); - } - } else { - // if we're somewhere in the middle of the route compute a search - // vicinity halfwya between the previous waypoint and the one we are - // inserting at, i.e th emiddle of the leg. - // if we're at the beginning, just use zero of course. - const double normOffset = (insertPosition > 0) ? -0.5 : 0.0; - vicinity = _plan->pointAlongRouteNorm(insertPosition, normOffset); - } - - return _plan->waypointFromString(target, vicinity); + return _plan->waypointFromString(target, _plan->vicinityForInsertIndex(insertPosition)); } double FGRouteMgr::getDepartureFieldElevation() const diff --git a/src/Navaids/FlightPlan.cxx b/src/Navaids/FlightPlan.cxx index 6c987ee4b..22fb593e6 100644 --- a/src/Navaids/FlightPlan.cxx +++ b/src/Navaids/FlightPlan.cxx @@ -1351,6 +1351,27 @@ double FlightPlan::magvarDegAt(const SGGeod& pos) const return sgGetMagVar(pos, jd) * SG_RADIANS_TO_DEGREES; } +SGGeod FlightPlan::vicinityForInsertIndex(int aIndex) const +{ + if (aIndex < 0) { // appending, not inserting + const int n = numLegs(); + if (n > 0) { + // if we have at least one existing leg, use its position + // for the search vicinity + return pointAlongRoute(n - 1, 0.0); + } + + return SGGeod::invalid(); + } + + // if we're somewhere in the middle of the route compute a search + // vicinity halfway between the previous waypoint and the one we are + // inserting at, i.e the middle of the leg. + // if we're at the beginning, just use zero of course. + const double normOffset = (aIndex > 0) ? -0.5 : 0.0; + return pointAlongRouteNorm(aIndex, normOffset); +} + WayptRef FlightPlan::waypointFromString(const string& tgt, const SGGeod& vicinity) { SGGeod basePosition = vicinity; diff --git a/src/Navaids/FlightPlan.hxx b/src/Navaids/FlightPlan.hxx index 1655446c9..e8a12f73a 100644 --- a/src/Navaids/FlightPlan.hxx +++ b/src/Navaids/FlightPlan.hxx @@ -336,6 +336,16 @@ public: */ SGGeod pointAlongRouteNorm(int aIndex, double aOffsetNorm) const; + /** + @brief given an index to insert a waypoint into the plan, find the geographical vicinity. + This is used to aid disambiguration searches, etc: see the vicinity paramter to 'waypointFromString' + below, for example + + When aIndex is negative, the vicinity used is the end of the current flight-plan, i.e appending to + the waypoints rather than appending. + */ + SGGeod vicinityForInsertIndex(int aIndex) const; + /** * Create a WayPoint from a string in the following format: * - diff --git a/test_suite/unit_tests/Navaids/test_routeManager.cxx b/test_suite/unit_tests/Navaids/test_routeManager.cxx index f0c7b5709..4e80b39f8 100644 --- a/test_suite/unit_tests/Navaids/test_routeManager.cxx +++ b/test_suite/unit_tests/Navaids/test_routeManager.cxx @@ -3,8 +3,9 @@ #include #include -#include #include +#include +#include #include "test_suite/FGTestApi/testGlobals.hxx" #include "test_suite/FGTestApi/NavDataCache.hxx" @@ -615,6 +616,59 @@ void RouteManagerTests::loadGPX() CPPUNIT_ASSERT_EQUAL(std::string{"KBOS"}, wp2->waypoint()->ident()); } +const std::string flightPlanXMLData = + R"( + + 2 + + EDDM + 08R + + + EDDF + + + + runway + true + 08R + EDDM + + + navaid + GIVMI + 11.364700 + 48.701100 + + + navaid + ERNAS + 11.219400 + 48.844700 + + + navaid + TALAL + 11.085300 + 49.108300 + + + navaid + ERMEL + 11.044700 + 49.187800 + + + navaid + PSA + 9.348300 + 49.862200 + + + +)"; + + // The same test as above, but for a file exported from the route manager or online void RouteManagerTests::loadFGFP() { @@ -626,56 +680,7 @@ void RouteManagerTests::loadFGFP() SGPath fgfpPath = simgear::Dir::current().path() / "test_fgfp.fgfp"; { sg_ofstream s(fgfpPath); - s << R"( - - 2 - - EDDM - 08R - - - EDDF - - - - runway - true - 08R - EDDM - - - navaid - GIVMI - 11.364700 - 48.701100 - - - navaid - ERNAS - 11.219400 - 48.844700 - - - navaid - TALAL - 11.085300 - 49.108300 - - - navaid - ERMEL - 11.044700 - 49.187800 - - - navaid - PSA - 9.348300 - 49.862200 - - - - )"; + s << flightPlanXMLData; } CPPUNIT_ASSERT(f->load(fgfpPath)); @@ -861,3 +866,57 @@ void RouteManagerTests::testsSelectNavaid() CPPUNIT_ASSERT_EQUAL(wp2->ident(), string{"OD"}); CPPUNIT_ASSERT_EQUAL(wp2->source()->name(), string{"BRYANSK NDB"}); } + +void RouteManagerTests::testCommandAPI() +{ + auto rm = globals->get_subsystem(); + SGPath fgfpPath = simgear::Dir::current().path() / "test_fgfp_2.fgfp"; + { + sg_ofstream s(fgfpPath); + s << flightPlanXMLData; + } + + { + SGPropertyNode_ptr args(new SGPropertyNode); + args->setStringValue("path", fgfpPath.utf8Str()); + CPPUNIT_ASSERT(globals->get_commands()->execute("load-flightplan", args)); + } + + auto f = rm->flightPlan(); + CPPUNIT_ASSERT_EQUAL(7, f->numLegs()); + + CPPUNIT_ASSERT(!f->isActive()); + + { + SGPropertyNode_ptr args(new SGPropertyNode); + CPPUNIT_ASSERT(globals->get_commands()->execute("activate-flightplan", args)); + } + + CPPUNIT_ASSERT(f->isActive()); + + { + SGPropertyNode_ptr args(new SGPropertyNode); + args->setIntValue("index", 3); + CPPUNIT_ASSERT(globals->get_commands()->execute("set-active-waypt", args)); + } + + CPPUNIT_ASSERT_EQUAL(3, f->currentIndex()); + + { + SGPropertyNode_ptr args(new SGPropertyNode); + args->setIntValue("index", 4); + args->setStringValue("navaid", "WLD"); + + // let's build an offset waypoint for fun + args->setDoubleValue("offset-nm", 10.0); + args->setDoubleValue("radial", 30); + CPPUNIT_ASSERT(globals->get_commands()->execute("insert-waypt", args)); + } + + auto waldaWpt = f->legAtIndex(4)->waypoint(); + auto waldaVOR = waldaWpt->source(); + CPPUNIT_ASSERT_EQUAL(string{"WALDA VOR-DME"}, waldaVOR->name()); + + auto d = SGGeodesy::distanceNm(waldaVOR->geod(), waldaWpt->position()); + CPPUNIT_ASSERT_DOUBLES_EQUAL(10.0, d, 0.1); +} diff --git a/test_suite/unit_tests/Navaids/test_routeManager.hxx b/test_suite/unit_tests/Navaids/test_routeManager.hxx index 51f1b4662..3ae33acb7 100644 --- a/test_suite/unit_tests/Navaids/test_routeManager.hxx +++ b/test_suite/unit_tests/Navaids/test_routeManager.hxx @@ -45,6 +45,7 @@ class RouteManagerTests : public CppUnit::TestFixture CPPUNIT_TEST(testRouteWithProcedures); CPPUNIT_TEST(testRouteWithApproachProcedures); CPPUNIT_TEST(testsSelectNavaid); + CPPUNIT_TEST(testCommandAPI); CPPUNIT_TEST_SUITE_END(); @@ -72,6 +73,7 @@ public: void testRouteWithProcedures(); void testRouteWithApproachProcedures(); void testsSelectNavaid(); + void testCommandAPI(); private: GPS* m_gps = nullptr;