Mission resume: make logic lighter and more separated

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

src/modules/navigator/mission.cpp

@@ -200,17 +200,17 @@ Mission::on_activation()
 
 	if (_inactivation_index > 0 && cameraWasTriggering()
 	    && getPreviousPositionItemIndex(_mission, _inactivation_index - 1, resume_index)) {
-		// the mission we are resuming had camera triggering enabled. In order to not lose any images
+		// The mission we are resuming had camera triggering enabled. In order to not lose any images
 		// we restart the mission at the previous position item.
-		// we will replay the cached commands once we reach the previous position item
+		// We will replay the cached commands once we reach the previous position item and have yaw aligned.
 		set_current_mission_index(resume_index);
-		printf("reset to previous position item (resume_index %i)\n", resume_index);
+
+		_align_heading_necessary = true;
 
 	} else {
 		set_mission_items();
 	}
 
-	_replay_cached_gimbal_items_at_next_waypoint = haveCachedItems();
 	_inactivation_index = -1; // reset
 
 	// reset cruise speed
@@ -250,21 +250,16 @@ Mission::on_active()
 		set_mission_items();
 	}
 
-	if (_replay_cached_gimbal_items_at_next_waypoint && is_mission_item_reached_or_completed()
-	    && _work_item_type != WORK_ITEM_TYPE_TAKEOFF) {
-		replayCachedGimbalItems();
-		_replay_cached_gimbal_items_at_next_waypoint = false;
-
+	// check if heading alignment is necessary, and add it to the current mission item if necessary
+	if (_align_heading_necessary && is_mission_item_reached_or_completed()) {
 		mission_item_s next_position_mission_item = {};
 
+		// add yaw alignment requirement on t he current mission item
 		if (getNextPositionMissionItem(_mission, _current_mission_index + 1, next_position_mission_item)
 		    && !PX4_ISFINITE(_mission_item.yaw)) {
-			// we are at the waypoint from which we want to resume the mission, first make sure that we are facing the next waypoint
 			_mission_item.yaw = matrix::wrap_pi(get_bearing_to_next_waypoint(_mission_item.lat, _mission_item.lon,
 							    next_position_mission_item.lat, next_position_mission_item.lon));
 			_mission_item.force_heading = true; // note: doesn't have effect in fixed-wing mode
-			// replay camera commands at the next waypoint to ensure the gimbal and vehicle are aligned for shooting
-			_replay_cached_camera_items_at_next_waypoint = cameraWasTriggering();
 		}
 
 		mission_apply_limitation(_mission_item);
@@ -273,13 +268,17 @@ Mission::on_active()
 		reset_mission_item_reached();
 
 		_navigator->set_position_setpoint_triplet_updated();
+		_align_heading_necessary = false;
+	}
 
-		return;
+	// replay gimbal and camera commands immediately after resuming mission
+	if (haveCachedGimbalOrCameraItems()) {
+		replayCachedGimbalCameraItems();
 	}
 
-	if (_replay_cached_camera_items_at_next_waypoint && is_mission_item_reached_or_completed()) {
-		replayCachedCameraItems();
-		_replay_cached_camera_items_at_next_waypoint = false;
+	// replay trigger commands upon raching the resume waypoint if the trigger relay flag is set
+	if (cameraWasTriggering() && is_mission_item_reached_or_completed()) {
+		replayCachedTriggerItems();
 	}
 
 	/* lets check if we reached the current mission item */
@@ -2006,7 +2005,6 @@ bool Mission::getPreviousPositionItemIndex(const mission_s &mission, int inactiv
 
 		if (MissionBlock::item_contains_position(missionitem)) {
 			prev_pos_index = index;
-			printf("prev_pos_index %i, inactivation_index: %i\n", prev_pos_index, inactivation_index);
 			return true;
 		}
 	}
@@ -2067,25 +2065,29 @@ void Mission::cacheItem(const mission_item_s &mission_item)
 	}
 }
 
-void Mission::replayCachedGimbalItems()
+void Mission::replayCachedGimbalCameraItems()
 {
 	if (_last_gimbal_configure_item.nav_cmd > 0) {
 		issue_command(_last_gimbal_configure_item);
+		_last_gimbal_configure_item = {}; // delete cached item
 	}
 
 	if (_last_gimbal_control_item.nav_cmd > 0) {
 		issue_command(_last_gimbal_control_item);
+		_last_gimbal_control_item = {}; // delete cached item
 	}
-}
 
-void Mission::replayCachedCameraItems()
-{
 	if (_last_camera_mode_item.nav_cmd > 0) {
 		issue_command(_last_camera_mode_item);
+		_last_camera_mode_item = {}; // delete cached item
 	}
+}
 
+void Mission::replayCachedTriggerItems()
+{
 	if (_last_camera_trigger_item.nav_cmd > 0) {
 		issue_command(_last_camera_trigger_item);
+		_last_camera_trigger_item = {}; // delete cached item
 	}
 }
 
@@ -2097,12 +2099,11 @@ void Mission::resetItemCache()
 	_last_camera_trigger_item = {};
 }
 
-bool Mission::haveCachedItems()
+bool Mission::haveCachedGimbalOrCameraItems()
 {
 	return _last_gimbal_configure_item.nav_cmd > 0 ||
 	       _last_gimbal_control_item.nav_cmd > 0 ||
-	       _last_camera_mode_item.nav_cmd > 0 ||
-	       _last_camera_trigger_item.nav_cmd > 0;
+	       _last_camera_mode_item.nav_cmd > 0;
 }
 
 bool Mission::cameraWasTriggering()

src/modules/navigator/mission.h

@@ -271,7 +271,7 @@ class Mission : public MissionBlock, public ModuleParams
 	bool readMissionItemAtIndex(const mission_s &mission, const int index, mission_item_s &missionitem);
 
 	/**
-	 * @brief Cache the mission items containing gimbal and camera commands
+	 * @brief Cache the mission items containing gimbal, camera mode and trigger commands
 	 *
 	 * @param mission_item The mission item to cache if applicable
 	 */
@@ -285,27 +285,27 @@ class Mission : public MissionBlock, public ModuleParams
 	void updateCachedItemsUpToIndex(int end_index);
 
 	/**
-	 * @brief Replay the cached gimbal items
+	 * @brief Replay the cached gimbal and camera mode items
 	 */
-	void replayCachedGimbalItems();
+	void replayCachedGimbalCameraItems();
 
 	/**
-	 * @brief Replay the cached camera items
+	 * @brief Replay the cached trigger items
 	 *
 	 */
-	void replayCachedCameraItems();
+	void replayCachedTriggerItems();
 
 	/**
 	 * @brief Reset the item cache
 	 */
 	void resetItemCache();
 
 	/**
-	 * @brief Check if there are cached items
+	 * @brief Check if there are cached gimbal or camera mode items to be replayed
 	 *
 	 * @return true if there are cached items
 	 */
-	bool haveCachedItems();
+	bool haveCachedGimbalOrCameraItems();
 
 	/**
 	 * @brief Check if the camera was triggering
@@ -366,9 +366,8 @@ class Mission : public MissionBlock, public ModuleParams
 	uint8_t _mission_execution_mode{mission_result_s::MISSION_EXECUTION_MODE_NORMAL};	/**< the current mode of how the mission is executed,look at mission_result.msg for the definition */
 	bool _execution_mode_changed{false};
 
-	bool _replay_cached_gimbal_items_at_next_waypoint = false;
-	bool _replay_cached_camera_items_at_next_waypoint = false;
-	int _inactivation_index = -1;
+	int _inactivation_index{-1}; // index of mission item at which the mission was paused. Used to resume survey missions at previous waypoint to not loose images.
+	bool _align_heading_necessary{false}; // if true, heading of vehicle needs to be aligned with heading of next waypoint. Used to create new mission items for heading alignment.
 
 	mission_item_s _last_gimbal_configure_item {};
 	mission_item_s _last_gimbal_control_item {};