MissionBase: replay change speed on resume immediately if not going to previous (#24484)

* MissionBase: replay change speed on resume immediately if not going to previous

This fixes an issue where the speed was not correctly set at the beginning of a
survey (with first wp having a DO_CHANGE_SPEED attached) when the user paused
and resumed the mission prior to reaching the first waypoint.

Signed-off-by: Silvan Fuhrer <[email protected]>

* Update src/modules/navigator/mission_base.cpp

Co-authored-by: Stefano Colli <[email protected]>

---------

Signed-off-by: Silvan Fuhrer <[email protected]>
Co-authored-by: Stefano Colli <[email protected]>

Author: sfuhrer

src/modules/navigator/mission_base.cpp

@@ -218,6 +218,8 @@ MissionBase::on_activation()
 
 	int32_t resume_index = _inactivation_index > 0 ? _inactivation_index : 0;
 
+	bool resume_mission_on_previous = false;
+
 	if (_inactivation_index > 0 && cameraWasTriggering()) {
 		size_t num_found_items{0U};
 		getPreviousPositionItems(_inactivation_index - 1, &resume_index, num_found_items, 1U);
@@ -229,9 +231,20 @@ MissionBase::on_activation()
 			setMissionIndex(resume_index);
 
 			_align_heading_necessary = true;
+			resume_mission_on_previous = true;
 		}
 	}
 
+	if (!resume_mission_on_previous) {
+		// Only replay speed changes immediately if we are not resuming the mission at the previous position item.
+		// Otherwise it must be handled in the on_active() method once we reach the previous position item.
+		replayCachedSpeedChangeItems();
+		_speed_replayed_on_activation = true;
+
+	} else {
+		_speed_replayed_on_activation = false;
+	}
+
 	checkClimbRequired(_mission.current_seq);
 	set_mission_items();
 
@@ -304,14 +317,16 @@ MissionBase::on_active()
 		replayCachedGimbalItems();
 	}
 
-	// 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
+	// Replay cached trigger commands once the last mission waypoint is re-reached after the mission resume
 	if (_mission.current_seq > _mission_activation_index) {
 		// replay trigger commands
 		if (cameraWasTriggering()) {
 			replayCachedTriggerItems();
 		}
+	}
 
+	if (!_speed_replayed_on_activation && _mission.current_seq > _mission_activation_index) {
+		// replay speed change items if not already done on mission (re-)activation
 		replayCachedSpeedChangeItems();
 	}
 

src/modules/navigator/mission_base.h

@@ -332,6 +332,7 @@ class MissionBase : public MissionBlock, public ModuleParams
 	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 */
+	bool _speed_replayed_on_activation{false};			/**< Flag indicating if the speed change items have been replayed on activation */
 
 	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*/