fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-13 16:30:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix commander: handle mode executor correctly on disarm

Browse Source 
There were a number of cases where the state was not correct or not as
desired after disarming, when running an external mode 'MyMission' with
executor:
- run MyMission, which triggers Hold, then Land
  - before: Mode: Hold, executor_in_charge: 1
  - after:  Mode: MyMission, executor_in_charge: 1
- run MyMission, then user switches to RTL
  - before: Mode: MyMission, executor_in_charge: 0
  - after:  Mode: MyMission, executor_in_charge: 1
- run MyMission, then while in Hold mode, low battery failsafe (RTL)
  - before: Mode: Hold, executor_in_charge: 1
  - after:  Mode: MyMission, executor_in_charge: 1
- run MyMission, then stop external mode (terminate the process)
  - before: Mode: (mode not available), executor_in_charge: 0
  - after:  Mode: Hold, executor_in_charge: 0

This case is unchanged:
- run MyMission, then low battery failsafe (RTL)
  - before: Mode: MyMission, executor_in_charge: 1
  - after:  Mode: MyMission, executor_in_charge: 1
This commit is contained in:
Beat Küng
2025-08-19 09:26:58 +02:00
parent a1bc09a6ad
commit df11aa1d69
4 changed files with 34 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/ModeManagement.cpp
+23
View File
@@ -473,6 +473,29 @@ uint8_t ModeManagement::getReplacedModeIfAny(uint8_t nav_state)
return nav_state;
}
uint8_t ModeManagement::onDisarm(uint8_t stored_nav_state)
{
// Switch to the owned mode if an executor is active
uint8_t returned_nav_state = stored_nav_state;
if (_mode_executors.valid(_mode_executor_in_charge)) {
returned_nav_state = _mode_executors.executor(_mode_executor_in_charge).owned_nav_state;
}
// Switch to Hold if the mode is unresponsive
if (_modes.valid(returned_nav_state)) {
if (_external_checks.isUnresponsive(_modes.mode(returned_nav_state).arming_check_registration_id)) {
returned_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
}
}
// Update _mode_executor_in_charge if needed (in case stored_nav_state belongs to an executor
// that is currently not active)
onUserIntendedNavStateChange(ModeChangeSource::User, returned_nav_state);
return returned_nav_state;
}
void ModeManagement::removeModeExecutor(int mode_executor_id)
{
if (mode_executor_id == -1) {