diff --git a/src/Navaids/routePath.cxx b/src/Navaids/routePath.cxx index 484b47d97..ee39ccebe 100644 --- a/src/Navaids/routePath.cxx +++ b/src/Navaids/routePath.cxx @@ -632,7 +632,7 @@ public: void computeDynamicPosition(int index) { auto previous(previousValidWaypoint(index)); - if ((previous == waypoints.end()) || !previous->posValid); + if ((previous == waypoints.end()) || !previous->posValid) { SG_LOG(SG_NAVAID, SG_WARN, "couldn't compute position for dynamic waypoint: no preceeding valid waypoint"); return; @@ -1165,69 +1165,80 @@ double RoutePath::distanceBetweenIndices(int from, int to) const SGGeod RoutePath::positionForDistanceFrom(int index, double distanceM) const { - int sz = (int) d->waypoints.size(); - if (index < 0) { - index = sz - 1; // map negative values to end of the route + int sz = (int) d->waypoints.size(); + if (index < 0) { + index = sz - 1; // map negative values to end of the route + } + + if ((index < 0) || (index >= sz)) { + throw sg_range_exception("waypt index out of range", + "RoutePath::positionForDistanceFrom"); + } + + // find the actual leg we're within + if (distanceM < 0.0) { + // scan backwards + while ((index > 0) && (distanceM < 0.0)) { + // we are looking at index n, say 4, but with a negative distance. + // we want to look at index n-1 (so, 3), and see if this makes + // distance positive. We need to offset by distance from 3 -> 4, + // which is waypoint 4's path distance. + + // note pathDistanceM is 0 for skipped waypoints, so this works out + distanceM += d->waypoints[index].pathDistanceM; + --index; } - - if ((index < 0) || (index >= sz)) { - throw sg_range_exception("waypt index out of range", - "RoutePath::positionForDistanceFrom"); - } - - // find the actual leg we're within + if (distanceM < 0.0) { - // scan backwards - while ((index > 0) && (distanceM < 0.0)) { - // we are looking at index n, say 4, but with a negative distance. - // we want to look at index n-1 (so, 3), and see if this makes - // distance positive. We need to offset by distance from 3 -> 4, - // which is waypoint 4's path distance. - distanceM += d->waypoints[index].pathDistanceM; - --index; - } - - if (distanceM < 0.0) { - // still negative, return route start - return d->waypoints[0].pos; - } - - } else { - // scan forwards - int nextIndex = index + 1; - while ((nextIndex < sz) && (d->waypoints[nextIndex].pathDistanceM < distanceM)) { - distanceM -= d->waypoints[nextIndex].pathDistanceM; - index = nextIndex++; - } + // still negative, return route start + return d->waypoints[0].pos; } - - if ((index + 1) >= sz) { - // past route end, just return final position - return d->waypoints[sz - 1].pos; + + } else { + // scan forwards + int nextIndex = index + 1; + while ((nextIndex < sz) && (d->waypoints[nextIndex].pathDistanceM < distanceM)) { + distanceM -= d->waypoints[nextIndex].pathDistanceM; + index = nextIndex++; } - - - const WayptData& wpt(d->waypoints[index]); - const WayptData& next(d->waypoints[index+1]); - if (next.wpt->type() == "via") { - return positionAlongVia(static_cast(next.wpt.get()), index, distanceM); - } - - if (wpt.turnPathDistanceM > distanceM) { - // on the exit path of current wpt - return wpt.pointAlongExitPath(distanceM); - } else { - distanceM -= wpt.turnPathDistanceM; - } - - double corePathDistance = next.pathDistanceM - next.turnPathDistanceM; - if (next.hasEntry && (distanceM > corePathDistance)) { - // on the entry path of next waypoint - return next.pointAlongEntryPath(distanceM - corePathDistance); - } - - // linear between turn exit and turn entry points - return SGGeodesy::direct(wpt.turnExitPos, next.legCourseTrue, distanceM); + } + + auto nextIt = d->nextValidWaypoint(index); + if (nextIt == d->waypoints.end()) { + // past route end, just return final position + return d->waypoints[sz - 1].pos; + } + + // this is important so we start from a valid WP if we're + // working either side of a DISCON + auto curIt = d->previousValidWaypoint(nextIt); + if (curIt == d->waypoints.end()) { + SG_LOG(SG_NAVAID, SG_WARN, "Couldn't find valid preceeding waypoint " << index); + return nextIt->pos; + } + + const WayptData& wpt = *curIt; + const WayptData& next = *nextIt; + + if (next.wpt->type() == "via") { + return positionAlongVia(static_cast(next.wpt.get()), index, distanceM); + } + + if (wpt.turnPathDistanceM > distanceM) { + // on the exit path of current wpt + return wpt.pointAlongExitPath(distanceM); + } else { + distanceM -= wpt.turnPathDistanceM; + } + + double corePathDistance = next.pathDistanceM - next.turnPathDistanceM; + if (next.hasEntry && (distanceM > corePathDistance)) { + // on the entry path of next waypoint + return next.pointAlongEntryPath(distanceM - corePathDistance); + } + + // linear between turn exit and turn entry points + return SGGeodesy::direct(wpt.turnExitPos, next.legCourseTrue, distanceM); } SGGeod RoutePath::positionAlongVia(Via* via, int previousIndex, double distanceM) const diff --git a/test_suite/unit_tests/Navaids/test_flightplan.cxx b/test_suite/unit_tests/Navaids/test_flightplan.cxx index 3fecec369..ce9212a7e 100644 --- a/test_suite/unit_tests/Navaids/test_flightplan.cxx +++ b/test_suite/unit_tests/Navaids/test_flightplan.cxx @@ -405,6 +405,7 @@ void FlightplanTests::testBasicDiscontinuity() const SGGeod balenPos = fp1->legAtIndex(3)->waypoint()->position(); const SGGeod murenPos = fp1->legAtIndex(4)->waypoint()->position(); + const auto crs = SGGeodesy::courseDeg(balenPos, murenPos); // total distance should not change fp1->insertWayptAtIndex(new Discontinuity(fp1), 4); // betwee BALEN and MUREN @@ -420,6 +421,20 @@ void FlightplanTests::testBasicDiscontinuity() CPPUNIT_ASSERT_DOUBLES_EQUAL(fp1->legAtIndex(5)->distanceNm(), SGGeodesy::distanceNm(balenPos, murenPos), 0.1); + // ensure that pointAlongRoute works correctly going into the DISCON + + + const auto pos1 = fp1->pointAlongRoute(3, 20.0); + const auto validP1 = SGGeodesy::direct(balenPos, crs, 20.0 * SG_NM_TO_METER); + + CPPUNIT_ASSERT(SGGeodesy::distanceM(pos1, validP1) < 500.0); + + const auto pos2 = fp1->pointAlongRoute(5, -10.0); + const auto crs2 = SGGeodesy::courseDeg(murenPos, balenPos); + const auto validP2 = SGGeodesy::direct(murenPos, crs2, 10.0 * SG_NM_TO_METER); + + CPPUNIT_ASSERT(SGGeodesy::distanceM(pos2, validP2) < 500.0); + // remove the discontinuity fp1->deleteIndex(4); CPPUNIT_ASSERT_DOUBLES_EQUAL(fp1->legAtIndex(4)->courseDeg(), diff --git a/test_suite/unit_tests/Navaids/test_routeManager.cxx b/test_suite/unit_tests/Navaids/test_routeManager.cxx index b5a01f8f0..28e71383f 100644 --- a/test_suite/unit_tests/Navaids/test_routeManager.cxx +++ b/test_suite/unit_tests/Navaids/test_routeManager.cxx @@ -279,6 +279,83 @@ void RouteManagerTests::testDirectToLegOnFlightplanAndResume() CPPUNIT_ASSERT_EQUAL(std::string{"leg"}, std::string{gpsNode->getStringValue("mode")}); } +void RouteManagerTests::testSequenceDiscontinuityAndResume() +{ + FGTestApi::setUp::logPositionToKML("rm_seq_discon_resume_leg"); + + FlightPlanRef fp1 = makeTestFP("LIRF", "16R", "LEBL", "07L", + "GITRI BALEN MUREN TOSNU"); + + + fp1->insertWayptAtIndex(new Discontinuity(fp1), 3); + + auto rm = globals->get_subsystem(); + rm->setFlightPlan(fp1); + + auto gpsNode = globals->get_props()->getNode("instrumentation/gps", true); + // auto rmNode = globals->get_props()->getNode("autopilot/route-manager", true); + + CPPUNIT_ASSERT(!strcmp("obs", gpsNode->getStringValue("mode"))); + rm->activate(); + + CPPUNIT_ASSERT(fp1->isActive()); + + auto balenLeg = fp1->legAtIndex(2); + auto pos = fp1->pointAlongRoute(2, -8.0); // 8nm before BALEN + + + fp1->setCurrentIndex(2); + + FGTestApi::setPosition(pos); + FGTestApi::runForTime(10.0); // let the GPS stabilize + + auto pilot = SGSharedPtr(new FGTestApi::TestPilot); + pilot->resetAtPosition(pos); + pilot->setCourseTrue(270); + pilot->setSpeedKts(250); + pilot->flyGPSCourse(m_gps); + + bool ok = FGTestApi::runForTimeWithCheck(180.0, [gpsNode] () { + return (gpsNode->getStringValue("mode") == std::string{"obs"}); + }); + + CPPUNIT_ASSERT(ok); + CPPUNIT_ASSERT_EQUAL(2, fp1->currentIndex()); // shouldn't sequence + + FGTestApi::runForTime(30.0); + + + // 2nm before MUREN : this should be on the GC course from BALEN + auto pos2 = fp1->pointAlongRoute(4, -6.0); + FGTestApi::setPosition(pos2); + FGTestApi::runForTime(2.0); // let the GPS stabilize + + // initiate a direct-to the next real WP + const auto murenPos = fp1->legAtIndex(4)->waypoint()->position(); + gpsNode->setStringValue("scratch/ident", "MUREN"); + gpsNode->setDoubleValue("scratch/longitude-deg", murenPos.getLongitudeDeg()); + gpsNode->setDoubleValue("scratch/latitude-deg", murenPos.getLatitudeDeg()); + gpsNode->setStringValue("command", "direct"); + CPPUNIT_ASSERT_EQUAL(std::string{"dto"}, std::string{gpsNode->getStringValue("mode")}); + + pilot->resetAtPosition(pos2); + pilot->flyGPSCourse(m_gps); + + ok = FGTestApi::runForTimeWithCheck(600.0, [fp1] () { + if (fp1->currentIndex() == 5) { + return true; + } + return false; + }); + + CPPUNIT_ASSERT(ok); + CPPUNIT_ASSERT_EQUAL(std::string{"leg"}, std::string{gpsNode->getStringValue("mode")}); + CPPUNIT_ASSERT_EQUAL(5, fp1->currentIndex()); + + FGTestApi::runForTime(30.0); + +} + void RouteManagerTests::testDefaultApproach() { diff --git a/test_suite/unit_tests/Navaids/test_routeManager.hxx b/test_suite/unit_tests/Navaids/test_routeManager.hxx index c0b8d1c83..5cad591c7 100644 --- a/test_suite/unit_tests/Navaids/test_routeManager.hxx +++ b/test_suite/unit_tests/Navaids/test_routeManager.hxx @@ -38,6 +38,8 @@ class RouteManagerTests : public CppUnit::TestFixture CPPUNIT_TEST(testDefaultApproach); CPPUNIT_TEST(testDirectToLegOnFlightplanAndResume); CPPUNIT_TEST(testHoldFromNasal); + CPPUNIT_TEST(testSequenceDiscontinuityAndResume); + CPPUNIT_TEST_SUITE_END(); // void setPositionAndStabilise(FGNavRadio* r, const SGGeod& g); @@ -57,6 +59,7 @@ public: void testDefaultApproach(); void testDirectToLegOnFlightplanAndResume(); void testHoldFromNasal(); + void testSequenceDiscontinuityAndResume(); private: GPS* m_gps = nullptr; };