fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix commander: set failsafe action state immediately after failsafe update

Browse Source 
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.
This commit is contained in:
Beat Küng 2025-07-09 15:47:45 +02:00 committed by Silvan Fuhrer
parent 168d99cd18
commit f0ecd9e757
3 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/commander/Commander.cpp
View File

@ -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);

4
src/modules/commander/ModeManagement.cpp
View File

@ -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();

9
src/modules/commander/ModeManagement.hpp
View File

@ -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; }