From df11aa1d69dc09944a38319ee91624ee245ebef0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 19 Aug 2025 09:26:58 +0200 Subject: [PATCH] fix commander: handle mode executor correctly on disarm 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 --- src/modules/commander/ModeManagement.cpp | 23 +++++++++++++++++++++ src/modules/commander/ModeManagement.hpp | 3 +++ src/modules/commander/UserModeIntention.cpp | 7 ++++++- src/modules/commander/UserModeIntention.hpp | 2 ++ 4 files changed, 34 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/ModeManagement.cpp b/src/modules/commander/ModeManagement.cpp index 63db218c18..b26bb99993 100644 --- a/src/modules/commander/ModeManagement.cpp +++ b/src/modules/commander/ModeManagement.cpp @@ -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) { diff --git a/src/modules/commander/ModeManagement.hpp b/src/modules/commander/ModeManagement.hpp index 4e733e5dc3..52eff77dee 100644 --- a/src/modules/commander/ModeManagement.hpp +++ b/src/modules/commander/ModeManagement.hpp @@ -153,6 +153,8 @@ public: void onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state) override; uint8_t getReplacedModeIfAny(uint8_t nav_state) override; + uint8_t onDisarm(uint8_t stored_nav_state) override; + uint8_t getNavStateReplacementIfValid(uint8_t nav_state, bool report_error = true); bool updateControlMode(uint8_t nav_state, vehicle_control_mode_s &control_mode) const; @@ -209,6 +211,7 @@ public: void onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state) override {} uint8_t getReplacedModeIfAny(uint8_t nav_state) override { return nav_state; } + uint8_t onDisarm(uint8_t stored_nav_state) override { return stored_nav_state; } uint8_t getNavStateReplacementIfValid(uint8_t nav_state, bool report_error = true) { return nav_state; } diff --git a/src/modules/commander/UserModeIntention.cpp b/src/modules/commander/UserModeIntention.cpp index 3a766640ec..086f9501b3 100644 --- a/src/modules/commander/UserModeIntention.cpp +++ b/src/modules/commander/UserModeIntention.cpp @@ -92,5 +92,10 @@ bool UserModeIntention::change(uint8_t user_intended_nav_state, ModeChangeSource void UserModeIntention::onDisarm() { - _user_intented_nav_state = _nav_state_after_disarming; + if (_handler) { + _user_intented_nav_state = _handler->onDisarm(_nav_state_after_disarming); + + } else { + _user_intented_nav_state = _nav_state_after_disarming; + } } diff --git a/src/modules/commander/UserModeIntention.hpp b/src/modules/commander/UserModeIntention.hpp index 1292c95317..b8b6827e06 100644 --- a/src/modules/commander/UserModeIntention.hpp +++ b/src/modules/commander/UserModeIntention.hpp @@ -53,6 +53,8 @@ public: * @return nav_state or the mode that nav_state replaces */ virtual uint8_t getReplacedModeIfAny(uint8_t nav_state) = 0; + + virtual uint8_t onDisarm(uint8_t stored_nav_state) = 0; };