Navigator: fix RTL_TYP=2 with NAV_CMD_CONDITION_GATE (#25648)

* rtl_mission_fast: make sure to set a position item on activation

* rtl_mission_fast_reverse: make sure to set a position item on activation

---------

Co-authored-by: Konrad Rudin <konrad@auterion.com>

src/modules/navigator/rtl_mission_fast.cpp

@@ -57,7 +57,7 @@ void RtlMissionFast::on_inactive()
 	MissionBase::on_inactive();
 	_vehicle_status_sub.update();
 	_mission_index_prior_rtl = _vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ?
-				   _mission.current_seq : -1;
+				   _mission.current_seq : INT32_C(-1);
 }
 
 void RtlMissionFast::on_activation()
@@ -65,13 +65,30 @@ void RtlMissionFast::on_activation()
 	_home_pos_sub.update();
 
 	// set mission item to closest item if not already in mission
-	if (_mission_index_prior_rtl < 0) {
+	if (_mission_index_prior_rtl < INT32_C(0)) {
 		_is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
 				_global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK;
 
 	} else {
-		setMissionIndex(_mission_index_prior_rtl);
+		int32_t next_mission_item_index;
-		_is_current_planned_mission_item_valid = isMissionValid();
+		size_t num_found_items{0U};
+		getNextPositionItems(_mission_index_prior_rtl, &next_mission_item_index, num_found_items, UINT8_C(1));
+
+		if (num_found_items > 0U) {
+			setMissionIndex(next_mission_item_index);
+			_is_current_planned_mission_item_valid = isMissionValid();
+
+		} else {
+			// No more position items left. Set it to the land item if it exists
+			if (_mission.land_index > 0) {
+				setMissionIndex(_mission.land_index);
+				_is_current_planned_mission_item_valid = isMissionValid();
+
+			} else {
+				// Nothing we can do, set the validity to false to trigger end of mission reaction
+				_is_current_planned_mission_item_valid = false;
+			}
+		}
 	}
 
 	if (_land_detected_sub.get().landed) {

src/modules/navigator/rtl_mission_fast.h

@@ -64,7 +64,7 @@ private:
 	bool setNextMissionItem() override;
 	void setActiveMissionItems() override;
 
-	int _mission_index_prior_rtl{-1};
+	int32_t _mission_index_prior_rtl{INT32_C(-1)};
 
 	uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)};		/**< home position subscription */
 };

src/modules/navigator/rtl_mission_fast_reverse.cpp

@@ -57,7 +57,7 @@ void RtlMissionFastReverse::on_inactive()
 	MissionBase::on_inactive();
 	_vehicle_status_sub.update();
 	_mission_index_prior_rtl = _vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ?
-				   _mission.current_seq : -1;
+				   _mission.current_seq : INT32_C(-1);
 }
 
 void RtlMissionFastReverse::on_inactivation()
@@ -71,13 +71,24 @@ void RtlMissionFastReverse::on_activation()
 	_home_pos_sub.update();
 
 	// set mission item to closest item if not already in mission. If we are in mission, set to the previous item.
-	if (_mission_index_prior_rtl < 0) {
+	if (_mission_index_prior_rtl < INT32_C(0)) {
 		_is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
 				_global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK;
 
 	} else {
-		setMissionIndex(math::max(_mission_index_prior_rtl - 1, 0));
+		int32_t previous_mission_item_index;
-		_is_current_planned_mission_item_valid = isMissionValid();
+		size_t num_found_items{0U};
+		getPreviousPositionItems(math::max(_mission_index_prior_rtl - INT32_C(1), INT32_C(0)), &previous_mission_item_index,
+					 num_found_items, UINT8_C(1));
+
+		if (num_found_items > 0U) {
+			setMissionIndex(previous_mission_item_index);
+			_is_current_planned_mission_item_valid = isMissionValid();
+
+		} else {
+			// No prior position items, so try to go to the first one.
+			_is_current_planned_mission_item_valid = (goToNextPositionItem(false) == PX4_OK);
+		}
 	}
 
 	if (_land_detected_sub.get().landed) {

src/modules/navigator/rtl_mission_fast_reverse.h

@@ -69,7 +69,7 @@ private:
 	void setActiveMissionItems() override;
 	void handleLanding(WorkItemType &new_work_item_type);
 
-	int _mission_index_prior_rtl{-1};
+	int32_t _mission_index_prior_rtl{INT32_C(-1)};
 
 	bool _in_landing_phase{false};