From c168ce254926ce1cfa94afda6a109e7f6a7088ca Mon Sep 17 00:00:00 2001 From: Saibernard <112599512+Saibernard@users.noreply.github.com> Date: Sun, 30 Aug 2026 15:26:36 -0400 Subject: [PATCH] fix(navigator): do not report the end of a mission as a storage failure (#28464) * fix(navigator): do not report the end of a mission as a storage failure getNonJumpItem() walks forward past items it skips, including a DO_JUMP that has no repeats left. When such a DO_JUMP is the last item in the mission the walk steps one index beyond the end and calls loadMissionItemFromCache(), which short circuits on its own bounds check without ever touching the datamanager. The caller then took the read failure path and told the operator: "Waypoint could not be read." (mavlink_log_critical) "Waypoint {1} could not be read from storage" (events, Log::Error) so a mission that simply ran to completion reported a Critical storage error. A mission of the form [WP, WP, WP, DO_JUMP(repeat N)] hits this on its final pass, and the look ahead for the next position setpoint takes the same path, so it can repeat. Check the index against the mission bounds inside the loop, exactly as the function already does for the index it is entered with, and return the same quiet PX4_ERROR that an out of range index produces there. Callers already treat that as "no further item". Genuine datamanager failures still report as before. Assisted-by: Claude:claude-fable-5 Signed-off-by: Saibernard Yogendran * test(navigator): cover traversal past the mission bounds over an exhausted DO_JUMP Two regression tests for getNonJumpItem entered at a DO_JUMP whose repeats are used up. Skipping it walks the index one step outside the mission, forward off the end and backward in front of the start. Both must report PX4_ERROR the same quiet way an out of range entry index does, instead of taking the storage failure path. On unfixed code the forward case reproduces the false "Waypoint could not be read" critical error that a mission ending in an exhausted DO_JUMP publishes on its final pass. Assisted-by: Claude:claude-fable-5 Signed-off-by: Saibernard Yogendran --------- Signed-off-by: Saibernard Yogendran (cherry picked from commit bd0b15d434347b0f089c2fd385ca5cad5f937096) --- src/modules/navigator/mission_base.cpp | 8 ++ .../navigator/test/test_mission_base.cpp | 92 ++++++++++++++++++- 2 files changed, 99 insertions(+), 1 deletion(-) 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)