diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 0d653a1882..c24a791f8d 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/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(); } diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h index 65a021d45d..3479c3752d 100644 --- a/src/modules/navigator/mission_base.h +++ b/src/modules/navigator/mission_base.h @@ -332,6 +332,7 @@ protected: 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*/