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

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

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

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