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

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

src/modules/commander/Commander.cpp

@@ -672,6 +672,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();
 }
@@ -810,6 +812,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) {
@@ -2266,8 +2279,8 @@ Commander::run()
 			}
 
 			if (!_armed.armed && _manual_control.isMavlink() && (_internal_state.main_state_changes == 0)) {
-				// if there's never been a mode change force position control as initial state
-				_internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL;
+				// If there's never been a mode change try to use position control as initial state.
+				_internal_state.desired_main_state = commander_state_s::MAIN_STATE_POSCTL;
 			}
 
 			if (_manual_control_switches_sub.update(&_manual_control_switches) || safety_updated) {
@@ -2528,6 +2541,22 @@ Commander::run()
 			_have_taken_off_since_arming = 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,