From b9d3b9f2117311b06b3884518335df128f44ba2f Mon Sep 17 00:00:00 2001
From: Silvan Fuhrer <silvan@auterion.com>
Date: Fri, 24 May 2024 13:34:12 +0200
Subject: [PATCH] RTL_mission_fast: continue mission if RTL is triggered while
 in Mission

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
---
 src/modules/navigator/mission_base.h       | 14 +++++++-------
 src/modules/navigator/rtl_mission_fast.cpp | 19 +++++++++++++++++--
 src/modules/navigator/rtl_mission_fast.h   |  3 +++
 3 files changed, 27 insertions(+), 9 deletions(-)

diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h
index 405291969e..2f416f29e2 100644
--- a/src/modules/navigator/mission_base.h
+++ b/src/modules/navigator/mission_base.h
@@ -314,6 +314,13 @@ protected:
 	 */
 	bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const;
 
+	/**
+	 * @brief Set the Mission Index
+	 *
+	 * @param[in] index Index of the mission item
+	 */
+	void setMissionIndex(int32_t index);
+
 	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*/
 	bool _mission_checked{false};				/**< Flag indicating if the mission has been checked by the mission validator*/
@@ -421,13 +428,6 @@ private:
 	 */
 	bool cameraWasTriggering();
 
-	/**
-	 * @brief Set the Mission Index
-	 *
-	 * @param[in] index Index of the mission item
-	 */
-	void setMissionIndex(int32_t index);
-
 	/**
 	 * @brief Parameters update
 	 *
diff --git a/src/modules/navigator/rtl_mission_fast.cpp b/src/modules/navigator/rtl_mission_fast.cpp
index 70eb4819ff..1da091ce1b 100644
--- a/src/modules/navigator/rtl_mission_fast.cpp
+++ b/src/modules/navigator/rtl_mission_fast.cpp
@@ -52,12 +52,27 @@ RtlMissionFast::RtlMissionFast(Navigator *navigator) :
 
 }
 
+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;
+}
+
 void RtlMissionFast::on_activation()
 {
 	_home_pos_sub.update();
 
-	_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;
+	// set mission item to closest item if not already in mission
+	if (_mission_index_prior_rtl < 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);
+		_is_current_planned_mission_item_valid = isMissionValid();
+	}
 
 	if (_land_detected_sub.get().landed) {
 		// already landed, no need to do anything, invalidad the position mission item.
diff --git a/src/modules/navigator/rtl_mission_fast.h b/src/modules/navigator/rtl_mission_fast.h
index bb3db38c64..c782a471bd 100644
--- a/src/modules/navigator/rtl_mission_fast.h
+++ b/src/modules/navigator/rtl_mission_fast.h
@@ -56,6 +56,7 @@ public:
 	~RtlMissionFast() = default;
 
 	void on_activation() override;
+	void on_inactive() override;
 
 	rtl_time_estimate_s calc_rtl_time_estimate() override;
 
@@ -63,5 +64,7 @@ private:
 	bool setNextMissionItem() override;
 	void setActiveMissionItems() override;
 
+	int _mission_index_prior_rtl{-1};
+
 	uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)};		/**< home position subscription */
 };