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

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

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

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

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