MissionBase: replay the gimbal and trigger cached items only upon reaching resume waypoint (#23484)

* Fix: replay the mission cached items only upon reaching resume waypoint

* Refactoring
Split camera mode mission items from gimbal ones so to have a finer control over the relative replays

* Chore: fix formatting

---------

Co-authored-by: Silvan Fuhrer <silvan@auterion.com>

Author: StefanoColli

src/modules/navigator/mission_base.cpp

@@ -235,6 +235,7 @@ MissionBase::on_activation()
 	checkClimbRequired(_mission.current_seq);
 	set_mission_items();
 
+	_mission_activation_index = _mission.current_seq;
 	_inactivation_index = -1; // reset
 
 	// reset cruise speed
@@ -293,17 +294,27 @@ MissionBase::on_active()
 		_align_heading_necessary = false;
 	}
 
-	// replay gimbal and camera commands immediately after resuming mission
-	if (haveCachedGimbalOrCameraItems()) {
-		replayCachedGimbalCameraItems();
+	// Replay camera mode commands immediately upon mission resume
+	if (haveCachedCameraModeItems()) {
+		replayCachedCameraModeItems();
 	}
 
-	// replay trigger commands upon raching the resume waypoint if the trigger relay flag is set
-	if (cameraWasTriggering() && is_mission_item_reached_or_completed()) {
-		replayCachedTriggerItems();
-	}
 
-	replayCachedSpeedChangeItems();
+	// Replay cached mission commands once the last mission waypoint is re-reached after the mission interruption.
+	// Each replay function also clears the cached items afterwards
+	if (_mission.current_seq > _mission_activation_index) {
+		// replay gimbal commands
+		if (haveCachedGimbalItems()) {
+			replayCachedGimbalItems();
+		}
+
+		// replay trigger commands
+		if (cameraWasTriggering()) {
+			replayCachedTriggerItems();
+		}
+
+		replayCachedSpeedChangeItems();
+	}
 
 	/* lets check if we reached the current mission item */
 	if (_mission_type != MissionType::MISSION_TYPE_NONE && is_mission_item_reached_or_completed()) {
@@ -1255,7 +1266,7 @@ void MissionBase::cacheItem(const mission_item_s &mission_item)
 	}
 }
 
-void MissionBase::replayCachedGimbalCameraItems()
+void MissionBase::replayCachedGimbalItems()
 {
 	if (_last_gimbal_configure_item.nav_cmd > 0) {
 		issue_command(_last_gimbal_configure_item);
@@ -1266,7 +1277,10 @@ void MissionBase::replayCachedGimbalCameraItems()
 		issue_command(_last_gimbal_control_item);
 		_last_gimbal_control_item = {}; // delete cached item
 	}
+}
 
+void MissionBase::replayCachedCameraModeItems()
+{
 	if (_last_camera_mode_item.nav_cmd > 0) {
 		issue_command(_last_camera_mode_item);
 		_last_camera_mode_item = {}; // delete cached item
@@ -1297,11 +1311,15 @@ void MissionBase::resetItemCache()
 	_last_camera_trigger_item = {};
 }
 
-bool MissionBase::haveCachedGimbalOrCameraItems()
+bool MissionBase::haveCachedGimbalItems()
 {
 	return _last_gimbal_configure_item.nav_cmd > 0 ||
-	       _last_gimbal_control_item.nav_cmd > 0 ||
-	       _last_camera_mode_item.nav_cmd > 0;
+	       _last_gimbal_control_item.nav_cmd > 0;
+}
+
+bool MissionBase::haveCachedCameraModeItems()
+{
+	return _last_camera_mode_item.nav_cmd > 0;
 }
 
 bool MissionBase::cameraWasTriggering()

src/modules/navigator/mission_base.h

@@ -328,6 +328,7 @@ class MissionBase : public MissionBlock, public ModuleParams
 	mission_s _mission;					/**< Currently active mission*/
 	float _mission_init_climb_altitude_amsl{NAN}; 		/**< altitude AMSL the vehicle will climb to when mission starts */
 	int _inactivation_index{-1}; // index of mission item at which the mission was paused. Used to resume survey missions at previous waypoint to not lose images.
+	int _mission_activation_index{-1};					/**< Index of the mission item that will bring the vehicle back to a mission waypoint */
 
 	int32_t _load_mission_index{-1}; /**< Mission inted of loaded mission items in dataman cache*/
 	int32_t _dataman_cache_size_signed; /**< Size of the dataman cache. A negativ value indicates that previous mission items should be loaded, a positiv value the next mission items*/
@@ -398,9 +399,14 @@ class MissionBase : public MissionBlock, public ModuleParams
 	void updateCachedItemsUpToIndex(int end_index);
 
 	/**
-	 * @brief Replay the cached gimbal and camera mode items
+	 * @brief Replay the cached gimbal items
 	 */
-	void replayCachedGimbalCameraItems();
+	void replayCachedGimbalItems();
+
+	/**
+	 * @brief Replay the cached camera mode items
+	 */
+	void replayCachedCameraModeItems();
 
 	/**
 	 * @brief Replay the cached trigger items
@@ -415,11 +421,18 @@ class MissionBase : public MissionBlock, public ModuleParams
 	void replayCachedSpeedChangeItems();
 
 	/**
-	 * @brief Check if there are cached gimbal or camera mode items to be replayed
+	 * @brief Check if there are cached gimbal items to be replayed
+	 *
+	 * @return true if there are cached items
+	 */
+	bool haveCachedGimbalItems();
+
+	/**
+	 * @brief Check if there are cached camera mode items to be replayed
 	 *
 	 * @return true if there are cached items
 	 */
-	bool haveCachedGimbalOrCameraItems();
+	bool haveCachedCameraModeItems();
 
 	/**
 	 * @brief Check if the camera was triggering