diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 6daf9bab21a2..d3b38d1b84df 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -1007,6 +1007,14 @@ int MissionBase::getNonJumpItem(int32_t &mission_index, mission_item_s &mission, mission_item_s new_mission; for (uint16_t jump_count = 0u; jump_count < MAX_JUMP_ITERATION; jump_count++) { + if (new_mission_index >= _mission.count || new_mission_index < 0) { + // Running off either end of the mission while skipping over jumps is a normal + // outcome, for example when the last item is a DO_JUMP that has used up its + // repeats. Report it the same way an out of range index is reported on entry + // rather than as a storage failure. + return PX4_ERROR; + } + /* read mission item from datamanager */ bool success = loadMissionItemFromCache(new_mission_index, new_mission); diff --git a/src/modules/navigator/test/test_mission_base.cpp b/src/modules/navigator/test/test_mission_base.cpp index 5744ad6e8cbe..996b500fc322 100644 --- a/src/modules/navigator/test/test_mission_base.cpp +++ b/src/modules/navigator/test/test_mission_base.cpp @@ -60,6 +60,11 @@ #include #include +#include +#include +#include + +#include "navigator.h" extern "C" int dataman_main(int argc, char *argv[]); class NavigatorDatamanRuntime @@ -99,7 +104,7 @@ static NavigatorDatamanRuntime &navigatorDatamanRuntime() class MissionBaseTestPeer : public MissionBase { public: - MissionBaseTestPeer() : MissionBase(nullptr, 8, 0) {} + explicit MissionBaseTestPeer(Navigator *navigator = nullptr) : MissionBase(navigator, 8, 0) {} void setActiveMissionItems() override {} bool setNextMissionItem() override { return false; } @@ -337,6 +342,91 @@ TEST_F(MissionBaseTraversalTest, GetNonJumpItemReturnsErrorForOutOfBoundsDoJumpT EXPECT_EQ(mission_index, 0); } +// Fixture with a real Navigator so the storage failure path is observable, it +// publishes to mavlink_log through the navigator instead of a null pointer. +class MissionBasePastBoundsTraversalTest : public ::testing::Test +{ +protected: + static void SetUpTestSuite() + { + (void)navigatorDatamanRuntime(); + } + + static void TearDownTestSuite() {} + + bool storageErrorPublished() + { + mavlink_log_s report; + + while (_mavlink_log_sub.update(&report)) { + if (strstr(reinterpret_cast(report.text), "could not be read") != nullptr) { + return true; + } + } + + return false; + } + + Navigator _navigator{}; + MissionBaseTestPeer mission_base{&_navigator}; + uORB::Subscription _mavlink_log_sub{ORB_ID(mavlink_log)}; +}; + +// WHY: Walking off the end of the mission while skipping an exhausted DO_JUMP is the +// normal end of a mission, not a storage failure. +// WHAT: [WP0, DO_JUMP->0 done] entered at the DO_JUMP returns PX4_ERROR without +// publishing a storage error. +TEST_F(MissionBasePastBoundsTraversalTest, GetNonJumpItemReturnsErrorPastMissionEnd) +{ + // GIVEN: A mission whose last item is a DO_JUMP with no repeats left. + mission_base.loadTestMission({ + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 0 + makeDoJump(0, 1, 1), // idx 1 + }); + + int32_t mission_index = 1; + mission_item_s mission_item{}; + (void)storageErrorPublished(); // drain anything already queued + + // WHEN: The helper skips the exhausted jump while traversing forward. + const int ret = mission_base.getNonJumpItem(mission_index, mission_item, + MissionBaseTestPeer::MissionTraversalType::FollowMissionControlFlow, + false, false); + + // THEN: It reports no further item, exactly like an out of range entry index, + // and no "could not be read" error is published. + EXPECT_EQ(ret, PX4_ERROR); + EXPECT_EQ(mission_index, 1); + EXPECT_FALSE(storageErrorPublished()); +} + +// WHY: The same walk moving backward can step in front of the first item. +// WHAT: [DO_JUMP->1 done, WP1] entered at the DO_JUMP backward returns PX4_ERROR +// without publishing a storage error. +TEST_F(MissionBasePastBoundsTraversalTest, GetNonJumpItemReturnsErrorPastMissionStart) +{ + // GIVEN: A mission that starts with a DO_JUMP with no repeats left. + mission_base.loadTestMission({ + makeDoJump(1, 1, 1), // idx 0 + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 1 + }); + + int32_t mission_index = 0; + mission_item_s mission_item{}; + (void)storageErrorPublished(); // drain anything already queued + + // WHEN: The helper skips the exhausted jump while traversing backward. + const int ret = mission_base.getNonJumpItem(mission_index, mission_item, + MissionBaseTestPeer::MissionTraversalType::FollowMissionControlFlow, + false, true); + + // THEN: It reports no further item, exactly like an out of range entry index, + // and no "could not be read" error is published. + EXPECT_EQ(ret, PX4_ERROR); + EXPECT_EQ(mission_index, 0); + EXPECT_FALSE(storageErrorPublished()); +} + // WHY: Geometry-only position traversal must skip non-position mission items. // WHAT: Starting from a VTOL transition item, the helper skips it and returns the next position item. TEST_F(MissionBaseTraversalTest, FindNextSkipsNonPositionItems)