fix(navigator): goToNextPositionItem skip loops when required (#26993)

docs/en/flight_modes/return.md

@@ -110,11 +110,13 @@ The behaviour is fairly complex because it depends on the flight mode, and wheth
 
 Mission _with_ landing pattern:
 
-- **Mission mode:** Mission is continued in "fast-forward mode" (jumps, delay and any other non-position commands ignored, loiter and other position waypoints converted to simple waypoints) and then lands.
+- **Mission mode:**
+  - Mission is continued in "fast-forward mode" and then lands.
+    - DO_JUMP commands, delays and other non-position mission items are ignored, and loiter and other position waypoints are converted to simple waypoints.
 - **Auto mode other than mission mode:**
   - Ascend to a safe [minimum return altitude](#minimum-return-altitude) above any expected obstacles.
   - Fly directly to closest waypoint (for FW not a landing WP) and descend to waypoint altitude.
-  - Continue mission in fast forward mode from that waypoint.
+  - Continue mission in fast forward mode from that waypoint, using the same traversal rules as above.
 - **Manual modes:**
   - Ascend to a safe [minimum return altitude](#minimum-return-altitude) above any expected obstacles.
   - Fly directly to landing sequence position and descend to waypoint altitude
@@ -124,7 +126,7 @@ Mission _without_ landing pattern defined:
 
 - **Mission mode:**
   - Mission flown "fast-backward" (in reverse) starting from the previous waypoint
-    - Jumps, delay and any other non-position commands ignored, loiter and other position waypoints converted to simple waypoints.
+    - DO_JUMP commands, delays and other non-position mission items are ignored, and loiter and other position waypoints are converted to simple waypoints.
     - VTOL vehicles transition to FW mode (if needed) before flying the mission in reverse.
   - On reaching waypoint 1, the vehicle ascends to the [minimum return altitude](#minimum-return-altitude) and flies to the home position (where it [lands or waits](#loiter-landing-at-destination)).
 - **Auto mode other than mission mode:**
@@ -136,6 +138,11 @@ If no mission is defined PX4 will fly directly to home location and land (rally
 
 If the mission changes during return mode, then the behaviour is re-evaluated based on the new mission following the same rules as above (e.g. if the new mission has no landing sequence and you're in a mission, the mission is reversed).
 
+::: info
+For `RTL_TYPE=4`, PX4 currently chooses between continuing to a mission landing and reversing toward home by comparing raw mission item indices.
+This is only an approximation of the flown path length, because the number if mission items is not indicative of the distance remaining and non-position items are also counted.
+:::
+
 ### Closest Safe Destination Return Type (RTL_TYPE=3)
 
 In this return type the vehicle:
@@ -207,7 +214,7 @@ The RTL parameters are listed in [Parameter Reference > Return Mode](../advanced
 
 | Parameter                                                                                                   | Description                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        |
 | ----------------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| <a id="RTL_TYPE"></a>[RTL_TYPE](../advanced_config/parameter_reference.md#RTL_TYPE)                         | Return mechanism (path and destination).<br>`0`: Return to a rally point or home (whichever is closest) via direct path.<br>`1`: Return to a rally point or the mission landing pattern start point (whichever is closest), via direct path. If neither mission landing or rally points are defined return home via a direct path. If the destination is a mission landing pattern, follow the pattern to land.<br>`2`: Use mission path fast-forward to landing if a landing pattern is defined, otherwise fast-reverse to home. Ignore rally points. Fly direct to home if no mission plan is defined.<br>`3`: Return via direct path to closest destination: home, start of mission landing pattern or safe point. If the destination is a mission landing pattern, follow the pattern to land. |
+| <a id="RTL_TYPE"></a>[RTL_TYPE](../advanced_config/parameter_reference.md#RTL_TYPE)                         | Return mechanism (path and destination).<br>`0`: Return to a rally point or home (whichever is closest) via direct path.<br>`1`: Return to a rally point or the mission landing pattern start point (whichever is closest), via direct path. If neither mission landing or rally points are defined return home via a direct path. If the destination is a mission landing pattern, follow the pattern to land.<br>`2`: Use the mission path to landing while skipping DO_JUMP and other non-position mission items if a landing pattern is defined, otherwise fast-reverse to home with the same traversal rules. Ignore rally points. Fly direct to home if no mission plan is defined.<br>`3`: Return via direct path to closest destination: home, start of mission landing pattern or safe point. If the destination is a mission landing pattern, follow the pattern to land. |
 | <a id="RTL_RETURN_ALT"></a>[RTL_RETURN_ALT](../advanced_config/parameter_reference.md#RTL_RETURN_ALT)       | Return altitude in meters (default: 60m) when [RTL_CONE_ANG](../advanced_config/parameter_reference.md#RTL_CONE_ANG) is 0. If already above this value the vehicle will return at its current altitude.                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            |
 | <a id="RTL_DESCEND_ALT"></a>[RTL_DESCEND_ALT](../advanced_config/parameter_reference.md#RTL_DESCEND_ALT)    | Minimum return altitude and altitude at which the vehicle will slow or stop its initial descent from a higher return altitude (default: 30m)                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       |
 | <a id="RTL_LAND_DELAY"></a>[RTL_LAND_DELAY](../advanced_config/parameter_reference.md#RTL_LAND_DELAY)       | Time to wait at `RTL_DESCEND_ALT` before landing (default: 0.5s) -by default this period is short so that the vehicle will simply slow and then land immediately. If set to -1 the system will loiter at `RTL_DESCEND_ALT` rather than landing. The delay is provided to allow you to configure time for landing gear to be deployed (triggered automatically).                                                                                                                                                                                                                                                                                                                                                                                                                                    |

src/modules/navigator/CMakeLists.txt

@@ -34,6 +34,10 @@
 add_subdirectory(GeofenceBreachAvoidance)
 add_subdirectory(MissionFeasibility)
 
+if(BUILD_TESTING)
+	add_subdirectory(test)
+endif()
+
 set(NAVIGATOR_SOURCES
 	navigator_main.cpp
 	navigator_mode.cpp

src/modules/navigator/mission.cpp

@@ -99,7 +99,7 @@ Mission::set_current_mission_index(uint16_t index)
 	}
 
 	if (_navigator->get_mission_result()->valid && (index < _mission.count)) {
-		if (goToItem(index, true) != PX4_OK) {
+		if (goToItem(index, MissionTraversalType::FollowMissionControlFlow) != PX4_OK) {
 			// Keep the old mission index (it was not updated by the interface) and report back.
 			return false;
 		}
@@ -130,7 +130,7 @@ Mission::set_current_mission_index(uint16_t index)
 
 bool Mission::setNextMissionItem()
 {
-	return (goToNextItem(true) == PX4_OK);
+	return (goToNextItem() == PX4_OK);
 }
 
 bool

src/modules/navigator/mission_base.cpp

@@ -290,9 +290,7 @@ MissionBase::on_active()
 
 		if (num_found_items == 1U && !PX4_ISFINITE(_mission_item.yaw)) {
 			mission_item_s next_position_mission_item;
-			const dm_item_t mission_dataman_id = static_cast<dm_item_t>(_mission.mission_dataman_id);
-			bool success = _dataman_cache.loadWait(mission_dataman_id, next_mission_item_index,
-							       reinterpret_cast<uint8_t *>(&next_position_mission_item), sizeof(next_position_mission_item), MAX_DATAMAN_LOAD_WAIT);
+			const bool success = loadMissionItemFromCache(next_mission_item_index, next_position_mission_item);
 
 			if (success) {
 				_mission_item.yaw = matrix::wrap_pi(get_bearing_to_next_waypoint(_mission_item.lat, _mission_item.lon,
@@ -535,9 +533,7 @@ MissionBase::set_mission_items()
 
 bool MissionBase::loadCurrentMissionItem()
 {
-	const dm_item_t dm_item = static_cast<dm_item_t>(_mission.mission_dataman_id);
-	bool success = _dataman_cache.loadWait(dm_item, _mission.current_seq, reinterpret_cast<uint8_t *>(&_mission_item),
-					       sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT);
+	const bool success = loadMissionItemFromCache(_mission.current_seq, _mission_item);
 
 	if (!success) {
 		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission item could not be set.\t");
@@ -913,7 +909,7 @@ MissionBase::do_abort_landing()
 
 	} else {
 		// move mission index back (landing approach point)
-		_is_current_planned_mission_item_valid = (goToPreviousItem(false) == PX4_OK);
+		_is_current_planned_mission_item_valid = (goToPreviousItem(MissionTraversalType::IgnoreDoJump) == PX4_OK);
 	}
 
 	// send reposition cmd to get out of mission
@@ -971,21 +967,19 @@ bool MissionBase::isMissionValid() const
 	return ret_val;
 }
 
-int MissionBase::getNonJumpItem(int32_t &mission_index, mission_item_s &mission, bool execute_jump,
+int MissionBase::getNonJumpItem(int32_t &mission_index, mission_item_s &mission, MissionTraversalType traversal_type,
 				bool write_jumps, bool mission_direction_backward)
 {
 	if (mission_index >= _mission.count || mission_index < 0) {
 		return PX4_ERROR;
 	}
 
-	const dm_item_t mission_dataman_id = (dm_item_t)_mission.mission_dataman_id;
 	int32_t new_mission_index{mission_index};
 	mission_item_s new_mission;
 
 	for (uint16_t jump_count = 0u; jump_count < MAX_JUMP_ITERATION; jump_count++) {
 		/* read mission item from datamanager */
-		bool success = _dataman_cache.loadWait(mission_dataman_id, new_mission_index, reinterpret_cast<uint8_t *>(&new_mission),
-						       sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT);
+		bool success = loadMissionItemFromCache(new_mission_index, new_mission);
 
 		if (!success) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
@@ -1001,8 +995,10 @@ int MissionBase::getNonJumpItem(int32_t &mission_index, mission_item_s &mission,
 				return PX4_ERROR;
 			}
 
-			if ((new_mission.do_jump_current_count < new_mission.do_jump_repeat_count) && execute_jump) {
+			if ((new_mission.do_jump_current_count < new_mission.do_jump_repeat_count)
+			    && traversal_type == MissionTraversalType::FollowMissionControlFlow) {
 				if (write_jumps) {
+					const dm_item_t mission_dataman_id = static_cast<dm_item_t>(_mission.mission_dataman_id);
 					new_mission.do_jump_current_count++;
 					success = _dataman_cache.writeWait(mission_dataman_id, new_mission_index, reinterpret_cast<uint8_t *>(&new_mission),
 									   sizeof(struct mission_item_s));
@@ -1040,11 +1036,11 @@ int MissionBase::getNonJumpItem(int32_t &mission_index, mission_item_s &mission,
 	return PX4_OK;
 }
 
-int MissionBase::goToItem(int32_t index, bool execute_jump, bool mission_direction_backward)
+int MissionBase::goToItem(int32_t index, MissionTraversalType traversal_type, bool mission_direction_backward)
 {
 	mission_item_s mission_item;
 
-	if (getNonJumpItem(index, mission_item, execute_jump, true, mission_direction_backward) == PX4_OK) {
+	if (getNonJumpItem(index, mission_item, traversal_type, true, mission_direction_backward) == PX4_OK) {
 		setMissionIndex(index);
 		return PX4_OK;
 
@@ -1062,29 +1058,94 @@ void MissionBase::setMissionIndex(int32_t index)
 	}
 }
 
+bool MissionBase::loadMissionItemFromCache(int32_t index, mission_item_s &mission_item)
+{
+	return index >= 0
+	       && index < _mission.count
+	       && _dataman_cache.loadWait(static_cast<dm_item_t>(_mission.mission_dataman_id), index,
+					  reinterpret_cast<uint8_t *>(&mission_item), sizeof(mission_item),
+					  MAX_DATAMAN_LOAD_WAIT);
+}
+
+bool MissionBase::findNextPositionIndex(int32_t start_index, int32_t &next_index,
+					MissionTraversalType traversal_type)
+{
+	for (int32_t mission_index = start_index; mission_index < _mission.count;) {
+		int32_t traversed_index = mission_index;
+		mission_item_s mission_item{};
+
+		if (!loadTraversalItem(traversed_index, mission_item, traversal_type, false)) {
+			return false;
+		}
+
+		if (item_contains_position(mission_item)) {
+			next_index = traversed_index;
+			return true;
+		}
+
+		mission_index = traversed_index + 1;
+	}
+
+	return false;
+}
+
+bool MissionBase::findPreviousPositionIndex(int32_t start_index, int32_t &previous_index,
+		MissionTraversalType traversal_type)
+{
+	for (int32_t mission_index = start_index - 1; mission_index >= 0;) {
+		int32_t traversed_index = mission_index;
+		mission_item_s mission_item{};
+
+		if (!loadTraversalItem(traversed_index, mission_item, traversal_type, true)) {
+			return false;
+		}
+
+		if (item_contains_position(mission_item)) {
+			previous_index = traversed_index;
+			return true;
+		}
+
+		mission_index = traversed_index - 1;
+	}
+
+	return false;
+}
+
+bool MissionBase::loadTraversalItem(int32_t &mission_index, mission_item_s &mission_item,
+				    MissionTraversalType traversal_type, bool direction_backward)
+{
+	if (traversal_type == MissionTraversalType::FollowMissionControlFlow) {
+		return getNonJumpItem(mission_index, mission_item, MissionTraversalType::FollowMissionControlFlow,
+				      false, direction_backward) == PX4_OK;
+	}
+
+	return loadMissionItemFromCache(mission_index, mission_item);
+}
+
 void MissionBase::getPreviousPositionItems(int32_t start_index, int32_t items_index[],
 		size_t &num_found_items, uint8_t max_num_items)
+{
+	getPreviousPositionItems(start_index, items_index, num_found_items, max_num_items, traversalType());
+}
+
+void MissionBase::getPreviousPositionItems(int32_t start_index, int32_t items_index[],
+		size_t &num_found_items, uint8_t max_num_items, MissionTraversalType traversal_type)
 {
 	num_found_items = 0u;
 
-	int32_t next_mission_index{start_index};
+	int32_t search_index{start_index};
 
 	for (size_t item_idx = 0u; item_idx < max_num_items; item_idx++) {
-		if (next_mission_index < 0) {
+		if (search_index < 0) {
 			break;
 		}
 
-		mission_item_s next_mission_item;
-		bool found_next_item{false};
+		int32_t previous_position_index{-1};
 
-		do {
-			next_mission_index--;
-			found_next_item = getNonJumpItem(next_mission_index, next_mission_item, true, false, true) == PX4_OK;
-		} while (!MissionBlock::item_contains_position(next_mission_item) && found_next_item);
-
-		if (found_next_item) {
-			items_index[item_idx] = next_mission_index;
+		if (findPreviousPositionIndex(search_index, previous_position_index, traversal_type)) {
+			items_index[item_idx] = previous_position_index;
 			num_found_items = item_idx + 1;
+			search_index = previous_position_index;
 
 		} else {
 			break;
@@ -1094,29 +1155,30 @@ void MissionBase::getPreviousPositionItems(int32_t start_index, int32_t items_in
 
 void MissionBase::getNextPositionItems(int32_t start_index, int32_t items_index[],
 				       size_t &num_found_items, uint8_t max_num_items)
+{
+	getNextPositionItems(start_index, items_index, num_found_items, max_num_items, traversalType());
+}
+
+void MissionBase::getNextPositionItems(int32_t start_index, int32_t items_index[],
+				       size_t &num_found_items, uint8_t max_num_items,
+				       MissionTraversalType traversal_type)
 {
 	// Make sure vector does not contain any preexisting elements.
 	num_found_items = 0u;
 
-	int32_t next_mission_index{start_index};
+	int32_t search_index{start_index};
 
 	for (size_t item_idx = 0u; item_idx < max_num_items; item_idx++) {
-		if (next_mission_index >= _mission.count) {
+		if (search_index >= _mission.count) {
 			break;
 		}
 
-		mission_item_s next_mission_item;
-		bool found_next_item{false};
+		int32_t next_position_index{-1};
 
-		do {
-			found_next_item = getNonJumpItem(next_mission_index, next_mission_item, true, false, false) == PX4_OK;
-			next_mission_index++;
-		} while (!MissionBlock::item_contains_position(next_mission_item) && found_next_item);
-
-		if (found_next_item) {
-			items_index[item_idx] = math::max(next_mission_index - 1,
-							  static_cast<int32_t>(0)); // subtract 1 to get the index of the first position item
+		if (findNextPositionIndex(search_index, next_position_index, traversal_type)) {
+			items_index[item_idx] = next_position_index;
 			num_found_items = item_idx + 1;
+			search_index = next_position_index + 1;
 
 		} else {
 			break;
@@ -1124,31 +1186,44 @@ void MissionBase::getNextPositionItems(int32_t start_index, int32_t items_index[
 	}
 }
 
-int MissionBase::goToNextItem(bool execute_jump)
+int MissionBase::goToNextItem()
+{
+	return goToNextItem(traversalType());
+}
+
+int MissionBase::goToNextItem(MissionTraversalType traversal_type)
 {
 	if (_mission.current_seq + 1 >= (_mission.count)) {
 		return PX4_ERROR;
 	}
 
-	return goToItem(_mission.current_seq + 1, execute_jump);
+	return goToItem(_mission.current_seq + 1, traversal_type);
 }
 
-int MissionBase::goToPreviousItem(bool execute_jump)
+int MissionBase::goToPreviousItem()
+{
+	return goToPreviousItem(traversalType());
+}
+
+int MissionBase::goToPreviousItem(MissionTraversalType traversal_type)
 {
 	if (_mission.current_seq <= 0) {
 		return PX4_ERROR;
 	}
 
-	return goToItem(_mission.current_seq - 1, execute_jump, true);
+	return goToItem(_mission.current_seq - 1, traversal_type, true);
 }
 
-int MissionBase::goToPreviousPositionItem(bool execute_jump)
+int MissionBase::goToPreviousPositionItem()
 {
-	size_t num_found_items{0U};
-	int32_t previous_position_item_index;
-	getPreviousPositionItems(_mission.current_seq, &previous_position_item_index, num_found_items, 1);
+	return goToPreviousPositionItem(traversalType());
+}
 
-	if (num_found_items == 1U) {
+int MissionBase::goToPreviousPositionItem(MissionTraversalType traversal_type)
+{
+	int32_t previous_position_item_index{-1};
+
+	if (findPreviousPositionIndex(_mission.current_seq, previous_position_item_index, traversal_type)) {
 		setMissionIndex(previous_position_item_index);
 		return PX4_OK;
 
@@ -1157,13 +1232,16 @@ int MissionBase::goToPreviousPositionItem(bool execute_jump)
 	}
 }
 
-int MissionBase::goToNextPositionItem(bool execute_jump)
+int MissionBase::goToNextPositionItem()
 {
-	size_t num_found_items{0U};
-	int32_t next_position_item_index;
-	getNextPositionItems(_mission.current_seq + 1, &next_position_item_index, num_found_items, 1);
+	return goToNextPositionItem(traversalType());
+}
 
-	if (num_found_items == 1U) {
+int MissionBase::goToNextPositionItem(MissionTraversalType traversal_type)
+{
+	int32_t next_position_item_index{-1};
+
+	if (findNextPositionIndex(_mission.current_seq + 1, next_position_item_index, traversal_type)) {
 		setMissionIndex(next_position_item_index);
 		return PX4_OK;
 
@@ -1177,13 +1255,11 @@ int MissionBase::setMissionToClosestItem(double lat, double lon, float alt, floa
 {
 	int32_t min_dist_index(-1);
 	float min_dist(FLT_MAX), dist_xy(FLT_MAX), dist_z(FLT_MAX);
-	const dm_item_t mission_dataman_id = static_cast<dm_item_t>(_mission.mission_dataman_id);
 
 	for (int32_t mission_item_index = 0; mission_item_index < _mission.count; mission_item_index++) {
 		mission_item_s mission;
 
-		bool success = _dataman_cache.loadWait(mission_dataman_id, mission_item_index, reinterpret_cast<uint8_t *>(&mission),
-						       sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT);
+		const bool success = loadMissionItemFromCache(mission_item_index, mission);
 
 		if (!success) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
@@ -1254,8 +1330,7 @@ void MissionBase::resetMissionJumpCounter()
 	for (size_t mission_index = 0u; mission_index < _mission.count; mission_index++) {
 		mission_item_s mission_item;
 
-		bool success = _dataman_client.readSync(mission_dataman_id, mission_index, reinterpret_cast<uint8_t *>(&mission_item),
-							sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT);
+		const bool success = loadMissionItemFromCache(mission_index, mission_item);
 
 		if (!success) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
@@ -1385,11 +1460,8 @@ void MissionBase::updateCachedItemsUpToIndex(const int end_index)
 {
 	for (int i = 0; i <= end_index; i++) {
 		mission_item_s mission_item;
-		const dm_item_t dm_current = (dm_item_t)_mission.mission_dataman_id;
-		bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast<uint8_t *>(&mission_item),
-							sizeof(mission_item), 500_ms);
 
-		if (success) {
+		if (loadMissionItemFromCache(i, mission_item)) {
 			cacheItem(mission_item);
 		}
 	}
@@ -1415,13 +1487,10 @@ void MissionBase::checkClimbRequired(int32_t mission_item_index)
 
 	if (num_found_items > 0U) {
 
-		const dm_item_t mission_dataman_id = static_cast<dm_item_t>(_mission.mission_dataman_id);
 		mission_item_s mission;
 		_mission_init_climb_altitude_amsl = NAN; // default to NAN, overwrite below if applicable
 
-		const bool success = _dataman_cache.loadWait(mission_dataman_id, next_mission_item_index,
-				     reinterpret_cast<uint8_t *>(&mission),
-				     sizeof(mission), MAX_DATAMAN_LOAD_WAIT);
+		const bool success = loadMissionItemFromCache(next_mission_item_index, mission);
 
 		const bool is_fw_and_takeoff = mission.nav_cmd == NAV_CMD_TAKEOFF
 					       && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;

src/modules/navigator/mission_base.h

@@ -99,26 +99,58 @@ protected:
 		MISSION_TYPE_MISSION
 	} _mission_type{MissionType::MISSION_TYPE_NONE};
 
+	enum class MissionTraversalType : uint8_t {
+		FollowMissionControlFlow = 0,
+		IgnoreDoJump,
+	};
+
 	/**
-	 * @brief Get the Previous Mission Position Items
+	 * @brief Get the previous mission position items using this mode's traversal policy.
 	 *
-	 * @param[in] start_index is the index from where to start searching the previous mission position items
-	 * @param[out] items_index is an array of indexes indicating the previous mission position items found
-	 * @param[out] num_found_items are the amount of previous position items found
-	 * @param[in] max_num_items are the maximum amount of previous position items to be searched
+	 * Convenience overload for callers that want the navigation mode default. Keep the
+	 * explicit traversal overload below for cases that intentionally need different semantics.
+	 *
+	 * @param[in] start_index Index from which to start searching backward
+	 * @param[out] items_index Array of the previous position item indices
+	 * @param[out] num_found_items Number of position items found
+	 * @param[in] max_num_items Maximum number of position items to collect
 	 */
 	void getPreviousPositionItems(int32_t start_index, int32_t items_index[], size_t &num_found_items,
 				      uint8_t max_num_items);
 	/**
-	 * @brief Get the next mission item containing a position setpoint
+	 * @brief Get the previous mission position items using an explicit traversal policy.
 	 *
-	 * @param[in] start_index is the index from where to start searching (first possible return index)
-	 * @param[out] items_index is an array of indexes indicating the next mission position items found
-	 * @param[out] num_found_items are the amount of next position items found
-	 * @param[in] max_num_items are the maximum amount of next position items to be searched
+	 * @param[in] start_index Index from which to start searching backward
+	 * @param[out] items_index Array of the previous position item indices
+	 * @param[out] num_found_items Number of position items found
+	 * @param[in] max_num_items Maximum number of position items to collect
+	 * @param[in] traversal_type Whether to follow DO_JUMP mission control flow or ignore DO_JUMP items
+	 */
+	void getPreviousPositionItems(int32_t start_index, int32_t items_index[], size_t &num_found_items,
+				      uint8_t max_num_items,
+				      MissionTraversalType traversal_type);
+	/**
+	 * @brief Get the next mission item containing a position setpoint using this mode's traversal policy.
+	 *
+	 * @param[in] start_index Index from which to start searching forward
+	 * @param[out] items_index Array of the next position item indices
+	 * @param[out] num_found_items Number of position items found
+	 * @param[in] max_num_items Maximum number of position items to collect
 	 */
 	void getNextPositionItems(int32_t start_index, int32_t items_index[], size_t &num_found_items,
 				  uint8_t max_num_items);
+	/**
+	 * @brief Get the next mission item containing a position setpoint using an explicit traversal policy.
+	 *
+	 * @param[in] start_index Index from which to start searching forward
+	 * @param[out] items_index Array of the next position item indices
+	 * @param[out] num_found_items Number of position items found
+	 * @param[in] max_num_items Maximum number of position items to collect
+	 * @param[in] traversal_type Whether to follow DO_JUMP mission control flow or ignore DO_JUMP items
+	 */
+	void getNextPositionItems(int32_t start_index, int32_t items_index[], size_t &num_found_items,
+				  uint8_t max_num_items,
+				  MissionTraversalType traversal_type);
 	/**
 	 * @brief Mission has a land start, a land, and is valid
 	 *
@@ -127,43 +159,71 @@ protected:
 	 */
 	bool hasMissionLandStart() const { return _mission.land_start_index >= 0 && _mission.land_index >= 0 && isMissionValid();};
 	/**
-	 * @brief Go to next Mission Item
+	 * @brief Go to next Mission Item using this mode's traversal policy.
 	 * Go to next non jump mission item
 	 *
-	 * @param[in] execute_jump Flag indicating if a jump should be executed or ignored
 	 * @return PX4_OK if next mission item exists, PX4_ERR otherwise
 	 */
-	int goToNextItem(bool execute_jump);
+	int goToNextItem();
 	/**
-	 * @brief Go to previous Mission Item
+	 * @brief Go to next Mission Item using an explicit traversal policy.
+	 * Go to next non jump mission item
+	 *
+	 * @param[in] traversal_type Whether to follow DO_JUMP mission control flow or ignore DO_JUMP items
+	 * @return PX4_OK if next mission item exists, PX4_ERR otherwise
+	 */
+	int goToNextItem(MissionTraversalType traversal_type);
+	/**
+	 * @brief Go to previous Mission Item using this mode's traversal policy.
 	 * Go to previous non jump mission item
-	 * @param[in] execute_jump Flag indicating if a jump should be executed or ignored
+	 *
 	 * @return PX4_OK if previous mission item exists, PX4_ERR otherwise
 	 */
-	int goToPreviousItem(bool execute_jump);
+	int goToPreviousItem();
+	/**
+	 * @brief Go to previous Mission Item using an explicit traversal policy.
+	 * Go to previous non jump mission item
+	 *
+	 * @param[in] traversal_type Whether to follow DO_JUMP mission control flow or ignore DO_JUMP items
+	 * @return PX4_OK if previous mission item exists, PX4_ERR otherwise
+	 */
+	int goToPreviousItem(MissionTraversalType traversal_type);
 	/**
 	 * @brief Go to Mission Item
 	 *
 	 * @param[in] index Index of the mission item to go to
-	 * @param[in] execute_jump Flag indicating if a jump should be executed of ignored
+	 * @param[in] traversal_type Whether to follow DO_JUMP mission control flow or ignore DO_JUMP items
 	 * @param[in] mission_direction_backward Flag indicating if a mission is flown backward
 	 * @return PX4_OK if the mission item exists, PX4_ERR otherwise
 	 */
-	int goToItem(int32_t index, bool execute_jump, bool mission_direction_backward = false);
+	int goToItem(int32_t index, MissionTraversalType traversal_type = MissionTraversalType::FollowMissionControlFlow,
+		     bool mission_direction_backward = false);
 	/**
-	 * @brief Go To Previous Mission Position Item
+	 * @brief Go To Previous Mission Position Item using this mode's traversal policy.
 	 *
-	 * @param[in] execute_jump Flag indicating if a jump should be executed or ignored
 	 * @return PX4_OK if previous mission item exists, PX4_ERR otherwise
 	 */
-	int goToPreviousPositionItem(bool execute_jump);
+	int goToPreviousPositionItem();
 	/**
-	 * @brief Go To Next Mission Position Item
+	 * @brief Go To Previous Mission Position Item using an explicit traversal policy.
+	 *
+	 * @param[in] traversal_type Whether to follow DO_JUMP mission control flow or ignore DO_JUMP items
+	 * @return PX4_OK if previous mission item exists, PX4_ERR otherwise
+	 */
+	int goToPreviousPositionItem(MissionTraversalType traversal_type);
+	/**
+	 * @brief Go To Next Mission Position Item using this mode's traversal policy.
 	 *
-	 * @param[in] execute_jump Flag indicating if a jump should be executed or ignored
 	 * @return PX4_OK if next mission item exists, PX4_ERR otherwise
 	 */
-	int goToNextPositionItem(bool execute_jump);
+	int goToNextPositionItem();
+	/**
+	 * @brief Go To Next Mission Position Item using an explicit traversal policy.
+	 *
+	 * @param[in] traversal_type Whether to follow DO_JUMP mission control flow or ignore DO_JUMP items
+	 * @return PX4_OK if next mission item exists, PX4_ERR otherwise
+	 */
+	int goToNextPositionItem(MissionTraversalType traversal_type);
 	/**
 	 * @brief Go to Mission Land Start Item
 	 *
@@ -202,13 +262,13 @@ protected:
 	 *
 	 * @param[out] mission_index Index of the mission item
 	 * @param[out] mission The return mission item
-	 * @param execute_jump Flag indicating if a jump item should be executed or ignored
+	 * @param traversal_type Whether to follow DO_JUMP mission control flow or ignore DO_JUMP items
 	 * @param write_jumps Flag indicating if the jump counter should be updated
 	 * @param mission_direction_backward Flag indicating if the mission is flown backwards
 	 * @return PX4_OK if mission item could be loaded, PX4_ERR otherwise
 	 */
-	int getNonJumpItem(int32_t &mission_index, mission_item_s &mission, bool execute_jump, bool write_jumps,
-			   bool mission_direction_backward = false);
+	int getNonJumpItem(int32_t &mission_index, mission_item_s &mission, MissionTraversalType traversal_type,
+			   bool write_jumps, bool mission_direction_backward = false);
 	/**
 	 * @brief Is Mission Valid
 	 *
@@ -316,6 +376,18 @@ protected:
 	 */
 	bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const;
 
+	/**
+	 * @brief Traversal mode used by this navigation mode when walking position items.
+	 *
+	 * Mission mode follows active DO_JUMP control flow by default. Derived modes such as
+	 * mission-based RTL can override this to walk the geometric mission path instead.
+	 * Traversal helpers use this policy unless the caller explicitly overrides it.
+	 */
+	virtual MissionTraversalType traversalType() const
+	{
+		return MissionTraversalType::FollowMissionControlFlow;
+	}
+
 	/**
 	 * @brief Set the Mission Index
 	 *
@@ -323,6 +395,40 @@ protected:
 	 */
 	void setMissionIndex(int32_t index);
 
+	/**
+	 * @brief Load a single mission item from the dataman cache.
+	 *
+	 * @param[in] index Index of the mission item
+	 * @param[out] mission_item The loaded mission item
+	 * @return true if the item was loaded successfully
+	 */
+	virtual bool loadMissionItemFromCache(int32_t index, mission_item_s &mission_item);
+
+	/**
+	 * @brief Find the next position mission item.
+	 *
+	 * Walks forward through the mission starting at @p start_index.
+	 *
+	 * @param[in] start_index First index to check
+	 * @param[out] next_index Index of the found position item
+	 * @param[in] traversal_type Whether to follow DO_JUMP mission control flow or ignore DO_JUMP items
+	 * @return true if a position item was found
+	 */
+	bool findNextPositionIndex(int32_t start_index, int32_t &next_index,
+				   MissionTraversalType traversal_type);
+
+	/**
+	 * @brief Find the previous position mission item.
+	 *
+	 * Walks backward through the mission starting at @p start_index - 1.
+	 *
+	 * @param[in] start_index Search starts one before this index
+	 * @param[out] previous_index Index of the found position item
+	 * @param[in] traversal_type Whether to follow DO_JUMP mission control flow or ignore DO_JUMP items
+	 * @return true if a position item was found
+	 */
+	bool findPreviousPositionIndex(int32_t start_index, int32_t &previous_index,
+				       MissionTraversalType traversal_type);
 
 	bool _is_current_planned_mission_item_valid{false};	/**< Flag indicating if the currently loaded mission item is valid*/
 	bool _mission_has_been_activated{false};		/**< Flag indicating if the mission has been activated*/
@@ -363,6 +469,12 @@ private:
 	 */
 	void updateMavlinkMission();
 
+	/**
+	 * @brief Load a mission item according to the requested mission traversal type.
+	 */
+	bool loadTraversalItem(int32_t &mission_index, mission_item_s &mission_item,
+			       MissionTraversalType traversal_type, bool direction_backward);
+
 	/**
 	 * Reset mission
 	 */

src/modules/navigator/rtl_direct_mission_land.cpp

@@ -96,7 +96,7 @@ void RtlDirectMissionLand::on_activation()
 	_needs_climbing = false;
 
 	if (hasMissionLandStart()) {
-		_is_current_planned_mission_item_valid = (goToItem(_mission.land_start_index, false) == PX4_OK);
+		_is_current_planned_mission_item_valid = (goToItem(_mission.land_start_index, MissionTraversalType::IgnoreDoJump) == PX4_OK);
 
 		_needs_climbing = checkNeedsToClimb();
 
@@ -115,7 +115,7 @@ void RtlDirectMissionLand::on_activation()
 
 bool RtlDirectMissionLand::setNextMissionItem()
 {
-	return (goToNextPositionItem(true) == PX4_OK);
+	return (goToNextPositionItem() == PX4_OK);
 }
 
 void RtlDirectMissionLand::setActiveMissionItems()

src/modules/navigator/rtl_mission_fast.cpp

@@ -101,7 +101,7 @@ void RtlMissionFast::on_activation()
 
 bool RtlMissionFast::setNextMissionItem()
 {
-	return (goToNextPositionItem(true) == PX4_OK);
+	return (goToNextPositionItem() == PX4_OK);
 }
 
 void RtlMissionFast::setActiveMissionItems()

src/modules/navigator/rtl_mission_fast.h

@@ -63,6 +63,7 @@ public:
 private:
 	bool setNextMissionItem() override;
 	void setActiveMissionItems() override;
+	MissionTraversalType traversalType() const override { return MissionTraversalType::IgnoreDoJump; }
 
 	int32_t _mission_index_prior_rtl{INT32_C(-1)};
 

src/modules/navigator/rtl_mission_fast_reverse.cpp

@@ -87,7 +87,7 @@ void RtlMissionFastReverse::on_activation()
 
 		} else {
 			// No prior position items, so try to go to the first one.
-			_is_current_planned_mission_item_valid = (goToNextPositionItem(false) == PX4_OK);
+			_is_current_planned_mission_item_valid = (goToNextPositionItem() == PX4_OK);
 		}
 	}
 
@@ -107,7 +107,7 @@ void RtlMissionFastReverse::on_active()
 
 bool RtlMissionFastReverse::setNextMissionItem()
 {
-	return (goToPreviousPositionItem(true) == PX4_OK);
+	return (goToPreviousPositionItem() == PX4_OK);
 }
 
 void RtlMissionFastReverse::setActiveMissionItems()

src/modules/navigator/rtl_mission_fast_reverse.h

@@ -67,6 +67,7 @@ public:
 private:
 	bool setNextMissionItem() override;
 	void setActiveMissionItems() override;
+	MissionTraversalType traversalType() const override { return MissionTraversalType::IgnoreDoJump; }
 	void handleLanding(WorkItemType &new_work_item_type);
 
 	int32_t _mission_index_prior_rtl{INT32_C(-1)};

src/modules/navigator/rtl_params.yaml

@@ -66,14 +66,17 @@ parameters:
           rally point), via direct path. If no mission landing or rally points are
           defined return home via direct path. Always choose closest safe landing
           point if vehicle is a VTOL in hover mode.
-        2: Return to a planned mission landing, if available, using the mission path,
-          else return to home via the reverse mission path. Do not consider rally
-          points.
+        2: Return to a planned mission landing, if available, using the mission
+          path while skipping DO_JUMP and other non-position mission items, else
+          return to home via the reverse mission path with the same traversal
+          rules. Do not consider rally points.
         3: 'Return via direct path to closest destination: home, start of mission
           landing pattern or safe point. If the destination is a mission landing pattern,
           follow the pattern to land.'
         4: Return to the planned mission landing, or to home via the reverse mission
-          path, whichever is closer by counting waypoints. Do not consider rally points.
+          path, whichever is estimated to be closer using mission item indices.
+          Skip DO_JUMP and other non-position mission items while following either
+          mission path. Do not consider rally points.
         5: Return directly to safe landing point (do not consider mission landing
           and Home)
       default: 0

src/modules/navigator/test/CMakeLists.txt

@@ -0,0 +1,36 @@
+############################################################################
+#
+#   Copyright (c) 2026 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+include_directories(${CMAKE_CURRENT_SOURCE_DIR}/..)
+
+px4_add_functional_gtest(SRC test_mission_base.cpp LINKLIBS modules__navigator)