ManualControl: fix case where mode switches unintentionally in air

Case: A vehicle is already operating but has no stick input or another
source than RC. When RC stick input is switched to either because it gets
first time available or as a fallback to joystick then the mode was
immediately changed to the switch position. This can lead to loss of
control e.g. when the vehicle is flying far away and the
mode switch of the RC is in some fully manual piloted mode.

I added tests to cover the cases where RC mode initialization is expected
and also unexpceted because the vehicle is already armed.

msg/VehicleStatus.msg

@@ -121,4 +121,3 @@ bool rc_calibration_in_progress
 bool calibration_enabled
 
 bool pre_flight_checks_pass		# true if all checks necessary to arm pass
-

src/modules/manual_control/ManualControl.cpp

@@ -81,6 +81,7 @@ void ManualControl::processInput(hrt_abstime now)
 		vehicle_status_s vehicle_status;
 
 		if (_vehicle_status_sub.copy(&vehicle_status)) {
+			_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
 			_system_id = vehicle_status.system_id;
 			_rotary_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
 			_vtol = vehicle_status.is_vtol;
@@ -269,8 +270,8 @@ void ManualControl::processSwitches(hrt_abstime &now)
 					}
 				}
 
-			} else {
-				// Send an initial request to switch to the mode requested by RC
+			} else if (!_armed) {
+				// Directly initialize mode using RC switch but only before arming
 				evaluateModeSlot(switches.mode_slot);
 			}
 

src/modules/manual_control/ManualControl.hpp

@@ -136,6 +136,7 @@ private:
 	unsigned _image_sequence{0};
 	bool _video_recording{false};
 
+	bool _armed{false};
 	uint8_t _system_id{1};
 	bool _rotary_wing{false};
 	bool _vtol{false};

src/modules/manual_control/ManualControlTest.cpp

@@ -282,3 +282,50 @@ TEST_F(SwitchTest, ModeSwitch)
 	EXPECT_EQ(_action_request_sub.get().action, ACTION_SWITCH_MODE);
 	EXPECT_EQ(_action_request_sub.get().mode, TestManualControl::navStateFromParam(NAVIGATION_STATE_AUTO_MISSION));
 }
+
+TEST_F(SwitchTest, ModeSwitchInitialization)
+{
+	// GIVEN: the mode switch is already in position 1 but there's no valid RC stick input yet
+	_manual_control_switches_pub.publish({.timestamp_sample = _timestamp, .mode_slot = manual_control_switches_s::MODE_SLOT_1});
+	_manual_control.processInput(_timestamp += 10_ms);
+	// THEN: there is no action requested
+	EXPECT_FALSE(_action_request_sub.update());
+
+	// GIVEN: new valid stick input from RC
+	_manual_control_input_pub.publish({.timestamp_sample = _timestamp, .valid = true, .data_source = manual_control_setpoint_s::SOURCE_RC});
+	_manual_control.processInput(_timestamp += 10_ms);
+	// THEN: there is still no action requested because the switches were not updated yet
+	EXPECT_FALSE(_action_request_sub.update());
+
+	// GIVEN: switch update with the same positions
+	_manual_control_switches_pub.publish({.timestamp_sample = _timestamp, .mode_slot = manual_control_switches_s::MODE_SLOT_1});
+	_manual_control.processInput(_timestamp += 10_ms);
+	// THEN: the mode switch is requested
+	EXPECT_TRUE(_action_request_sub.update());
+	EXPECT_EQ(_action_request_sub.get().action, ACTION_SWITCH_MODE);
+	EXPECT_EQ(_action_request_sub.get().mode, TestManualControl::navStateFromParam(NAVIGATION_STATE_ACRO));
+}
+
+TEST_F(SwitchTest, ModeSwitchInitializationArmed)
+{
+	// GIVEN: vehicle is armed
+	uORB::Publication<vehicle_status_s> vehicle_status_pub{ORB_ID(vehicle_status)};
+	vehicle_status_pub.publish({.arming_state = vehicle_status_s::ARMING_STATE_ARMED});
+
+	// GIVEN: valid stick input from RC
+	_manual_control_input_pub.publish({.timestamp_sample = _timestamp, .valid = true, .data_source = manual_control_setpoint_s::SOURCE_RC});
+	// and mode switch in position 1
+	_manual_control_switches_pub.publish({.timestamp_sample = _timestamp, .mode_slot = manual_control_switches_s::MODE_SLOT_1});
+	_manual_control.processInput(_timestamp += 10_ms);
+
+	// THEN: no action requested because the vehicle is flying
+	EXPECT_FALSE(_action_request_sub.update());
+
+	// GIVEN: the switch changes position
+	_manual_control_switches_pub.publish({.timestamp_sample = _timestamp, .mode_slot = manual_control_switches_s::MODE_SLOT_2});
+	_manual_control.processInput(_timestamp += 10_ms);
+	// THEN: the mode switch is requested
+	EXPECT_TRUE(_action_request_sub.update());
+	EXPECT_EQ(_action_request_sub.get().action, ACTION_SWITCH_MODE);
+	EXPECT_EQ(_action_request_sub.get().mode, TestManualControl::navStateFromParam(NAVIGATION_STATE_MANUAL));
+}