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