From f0ecd9e757a034cb453d60fc01f06de5d6f12522 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 9 Jul 2025 15:47:45 +0200 Subject: [PATCH] fix commander: set failsafe action state immediately after failsafe update There was a race condition: for example when an external mode disabled failsafe deferring, that then triggered a failsafe, while the mode executor immediately sends a command (to e.g. switch modes). In that case the failsafe got triggered but the mode switch was still allowed. This was because of the processing ordering: - mode updates (and propagating the failsafe_action_active state) - failsafe updates - command handling This patch makes sure failsafe_action_active is set immediately after updating the failsafes. --- src/modules/commander/Commander.cpp | 3 ++- src/modules/commander/ModeManagement.cpp | 4 +--- src/modules/commander/ModeManagement.hpp | 9 +++++++-- 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index b05d829945..280d00d69c 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2343,6 +2343,7 @@ bool Commander::handleModeIntentionAndFailsafe() } // Handle failsafe action + _mode_management.setFailsafeState(_failsafe.selectedAction() > FailsafeBase::Action::Warn); _vehicle_status.nav_state_user_intention = _mode_management.getNavStateReplacementIfValid(_user_mode_intention.get(), false); _vehicle_status.nav_state = _mode_management.getNavStateReplacementIfValid(FailsafeBase::modeFromAction( @@ -2410,7 +2411,7 @@ void Commander::modeManagementUpdate() { ModeManagement::UpdateRequest mode_management_update{}; _mode_management.update(isArmed(), _vehicle_status.nav_state_user_intention, - _failsafe.selectedAction() > FailsafeBase::Action::Warn, mode_management_update); + mode_management_update); if (!isArmed() && mode_management_update.change_user_intended_nav_state) { _user_mode_intention.change(mode_management_update.user_intended_nav_state); diff --git a/src/modules/commander/ModeManagement.cpp b/src/modules/commander/ModeManagement.cpp index 4430a0f563..63db218c18 100644 --- a/src/modules/commander/ModeManagement.cpp +++ b/src/modules/commander/ModeManagement.cpp @@ -364,10 +364,8 @@ void ModeManagement::checkUnregistrations(uint8_t user_intended_nav_state, Updat } } -void ModeManagement::update(bool armed, uint8_t user_intended_nav_state, bool failsafe_action_active, - UpdateRequest &update_request) +void ModeManagement::update(bool armed, uint8_t user_intended_nav_state, UpdateRequest &update_request) { - _failsafe_action_active = failsafe_action_active; _external_checks.update(); bool allow_update_while_armed = _external_checks.allowUpdateWhileArmed(); diff --git a/src/modules/commander/ModeManagement.hpp b/src/modules/commander/ModeManagement.hpp index 2a9c5c533f..4e733e5dc3 100644 --- a/src/modules/commander/ModeManagement.hpp +++ b/src/modules/commander/ModeManagement.hpp @@ -138,7 +138,11 @@ public: bool control_setpoint_update{false}; }; - void update(bool armed, uint8_t user_intended_nav_state, bool failsafe_action_active, UpdateRequest &update_request); + void update(bool armed, uint8_t user_intended_nav_state, UpdateRequest &update_request); + void setFailsafeState(bool failsafe_action_active) + { + _failsafe_action_active = failsafe_action_active; + } /** * Mode executor ID for who is currently in charge (and can send commands etc). @@ -198,7 +202,8 @@ public: bool control_setpoint_update{false}; }; - void update(bool armed, uint8_t user_intended_nav_state, bool failsafe_action_active, UpdateRequest &update_request) {} + void update(bool armed, uint8_t user_intended_nav_state, UpdateRequest &update_request) {} + void setFailsafeState(bool failsafe_action_active) {} int modeExecutorInCharge() const { return ModeExecutors::AUTOPILOT_EXECUTOR_ID; }