fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 20:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
* 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 <silvan@auterion.com>

* Update src/modules/navigator/mission_base.cpp

Co-authored-by: Stefano Colli <45536733+StefanoColli@users.noreply.github.com>

---------

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Co-authored-by: Stefano Colli <45536733+StefanoColli@users.noreply.github.com>
This commit is contained in:
Silvan Fuhrer
2025-03-12 09:17:57 +01:00
committed by GitHub
co-authored by Stefano Colli
parent c6c0f2228b
commit 5319b82e4d
2 changed files with 18 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_base.cpp
+17 -2
View File
@@ -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
+1
View File
@@ -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*/