RTL_mission_reverse: start from previous WP if RTL is triggered while in Mission

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/navigator/rtl_mission_fast_reverse.cpp

@@ -52,12 +52,27 @@ RtlMissionFastReverse::RtlMissionFastReverse(Navigator *navigator) :
 
 }
 
+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;
+}
+
 void RtlMissionFastReverse::on_activation()
 {
 	_home_pos_sub.update();
 
-	_is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
+	// set mission item to closest item if not already in mission. If we are in mission, set to the previous item.
-			_global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK;
+	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(math::max(_mission_index_prior_rtl - 1, 0));
+		_is_current_planned_mission_item_valid = isMissionValid();
+	}
 
 	if (_land_detected_sub.get().landed) {
 		// already landed, no need to do anything, invalidate the position mission item.

src/modules/navigator/rtl_mission_fast_reverse.h

@@ -57,6 +57,7 @@ public:
 
 	void on_activation() override;
 	void on_active() override;
+	void on_inactive() override;
 
 	rtl_time_estimate_s calc_rtl_time_estimate() override;
 
@@ -65,5 +66,7 @@ private:
 	void setActiveMissionItems() override;
 	void handleLanding(WorkItemType &new_work_item_type);
 
+	int _mission_index_prior_rtl{-1};
+
 	uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)};		/**< home position subscription */
 };