From 83b832fdced2bb5715fbc0653af457d0e95b3ae8 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 17 May 2023 16:52:30 +0200 Subject: [PATCH] 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 | 1 - src/modules/manual_control/ManualControl.cpp | 5 +- src/modules/manual_control/ManualControl.hpp | 1 + .../manual_control/ManualControlTest.cpp | 47 +++++++++++++++++++ 4 files changed, 51 insertions(+), 3 deletions(-) diff --git a/msg/VehicleStatus.msg b/msg/VehicleStatus.msg index 878b4b04ed..8fcb8c2fa9 100644 --- a/msg/VehicleStatus.msg +++ b/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 - diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 992429b965..9c01750f31 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/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); } diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp index 474762fbbd..8e8bce13a5 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/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}; diff --git a/src/modules/manual_control/ManualControlTest.cpp b/src/modules/manual_control/ManualControlTest.cpp index bcedf448fb..20591b48dc 100644 --- a/src/modules/manual_control/ManualControlTest.cpp +++ b/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_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)); +}