From e49b596edc83e67600ea3cc137032c65629fc6ee Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 6 May 2021 15:27:51 +0200
Subject: [PATCH] commander: add desired main state

This is an intermediate solution to carry forward the initial state of
the mode slot. Basically, it allows that we start up in Stabilized but
switch to POSCTL as soon we have the required GPS.
---
 msg/commander_state.msg             |  3 ++-
 src/modules/commander/Commander.cpp | 29 +++++++++++++++++++++++++++++
 2 files changed, 31 insertions(+), 1 deletion(-)

diff --git a/msg/commander_state.msg b/msg/commander_state.msg
index 7d5c85e11d..df12911f9e 100644
--- a/msg/commander_state.msg
+++ b/msg/commander_state.msg
@@ -18,6 +18,7 @@ uint8 MAIN_STATE_AUTO_PRECLAND      = 13
 uint8 MAIN_STATE_ORBIT              = 14
 uint8 MAIN_STATE_MAX                = 15
 
-uint8 main_state		    	# main state machine
+uint8 main_state
+uint8 desired_main_state
 
 uint16 main_state_changes
diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index 4be4e55bcd..60110848c7 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -733,6 +733,8 @@ Commander::Commander() :
 	// default for vtol is rotary wing
 	_vtol_status.vtol_in_rw_mode = true;
 
+	_internal_state.desired_main_state = commander_state_s::MAIN_STATE_MAX;
+
 	/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
 	mission_init();
 }
@@ -879,6 +881,17 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				reset_posvel_validity();
 
 				main_ret = main_state_transition(_status, desired_main_state, _status_flags, _internal_state);
+
+				// If the command is internal (e.g. sent by RC), and we were not (yet) able to switch
+				// to it, we will try again later. However, we only do that for ALTCTL and POSCTL.
+				if (!cmd.from_external && main_ret == TRANSITION_DENIED &&
+				    (desired_main_state == commander_state_s::MAIN_STATE_ALTCTL ||
+				     desired_main_state == commander_state_s::MAIN_STATE_POSCTL)) {
+					_internal_state.desired_main_state = desired_main_state;
+
+				} else {
+					_internal_state.desired_main_state = commander_state_s::MAIN_STATE_MAX;
+				}
 			}
 
 			if (main_ret != TRANSITION_DENIED) {
@@ -2710,6 +2723,22 @@ Commander::run()
 			_imbalanced_propeller_check_triggered = false;
 		}
 
+		// If we have an desired state, we should try to reach it but only when still disarmed.
+		if (_internal_state.desired_main_state != commander_state_s::commander_state_s::MAIN_STATE_MAX &&
+		    !_armed.armed) {
+			const transition_result_t desired_ret = main_state_transition(_status, _internal_state.desired_main_state,
+								_status_flags, _internal_state);
+
+			if (desired_ret == TRANSITION_CHANGED) {
+				// Reset it for next time.
+				_internal_state.desired_main_state = commander_state_s::MAIN_STATE_MAX;
+			}
+
+		} else if (_armed.armed) {
+			// Once armed we reset it.
+			_internal_state.desired_main_state = commander_state_s::MAIN_STATE_MAX;
+		}
+
 		/* now set navigation state according to failsafe and main state */
 		bool nav_state_changed = set_nav_state(_status,
 						       _armed,