From a09352c079c8bdcab988eccd231d282e8883b979 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 29 Jul 2025 14:22:57 +0200 Subject: [PATCH] ManualControl: add option to prioritize RC or MAVLink but with fallback --- src/modules/commander/commander_params.c | 4 + .../manual_control/ManualControlSelector.cpp | 36 +++- .../manual_control/ManualControlSelector.hpp | 4 + .../ManualControlSelectorTest.cpp | 169 +++++++++++++++++- 4 files changed, 202 insertions(+), 11 deletions(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 430229253f..b3ff841a6c 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -173,6 +173,8 @@ PARAM_DEFINE_INT32(COM_HOME_IN_AIR, 0); * A value of 2 allows either RC Transmitter or Joystick input. The first valid input is used, will fallback to other sources if the input stream becomes invalid. * A value of 3 allows either input from RC or joystick. The first available source is selected and used until reboot. * A value of 4 ignores any stick input. + * A value of 5 allows either RC Transmitter or Joystick input. But RC has priority and whenever avaiable is immedietely used. + * A value of 6 allows either RC Transmitter or Joystick input. But Joystick has priority and whenever avaiable is immedietely used. * * @group Commander * @min 0 @@ -182,6 +184,8 @@ PARAM_DEFINE_INT32(COM_HOME_IN_AIR, 0); * @value 2 RC and Joystick with fallback * @value 3 RC or Joystick keep first * @value 4 Stick input disabled + * @value 5 RC priority, Joystick fallback + * @value 6 Joystick priority, RC fallback */ PARAM_DEFINE_INT32(COM_RC_IN_MODE, 3); diff --git a/src/modules/manual_control/ManualControlSelector.cpp b/src/modules/manual_control/ManualControlSelector.cpp index 0ccc93635c..9607e0c13c 100644 --- a/src/modules/manual_control/ManualControlSelector.cpp +++ b/src/modules/manual_control/ManualControlSelector.cpp @@ -43,6 +43,10 @@ void ManualControlSelector::updateValidityOfChosenInput(uint64_t now) void ManualControlSelector::updateWithNewInputSample(uint64_t now, const manual_control_setpoint_s &input, int instance) { + if (isRc(input.data_source)) { _timestamp_last_rc = input.timestamp_sample; } + + if (isMavlink(input.data_source)) { _timestamp_last_mavlink = input.timestamp_sample; } + // First check if the chosen input got invalid, so it can get replaced updateValidityOfChosenInput(now); @@ -70,24 +74,38 @@ bool ManualControlSelector::isInputValid(const manual_control_setpoint_s &input, const bool sample_newer_than_timeout = now < input.timestamp_sample + _timeout; // Check if source matches the configuration - const bool source_rc_matched = (_rc_in_mode == 0) && (input.data_source == manual_control_setpoint_s::SOURCE_RC); - const bool source_mavlink_matched = (_rc_in_mode == 1) && - (input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_0 - || input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_1 - || input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_2 - || input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_3 - || input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_4 - || input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_5); + const bool source_rc_matched = (_rc_in_mode == 0) && isRc(input.data_source); + const bool source_mavlink_matched = (_rc_in_mode == 1) && isMavlink(input.data_source); const bool source_any_matched = (_rc_in_mode == 2); const bool source_first_matched = (_rc_in_mode == 3) && (input.data_source == _first_valid_source || _first_valid_source == manual_control_setpoint_s::SOURCE_UNKNOWN); + const bool source_rc_priority = (_rc_in_mode == 5) + && (isRc(input.data_source) || (now > _timestamp_last_rc + _timeout)); + const bool source_mavlink_priority = (_rc_in_mode == 6) + && (isMavlink(input.data_source) || (now > _timestamp_last_mavlink + _timeout)); return sample_from_the_past && sample_newer_than_timeout && input.valid - && (source_rc_matched || source_mavlink_matched || source_any_matched || source_first_matched); + && (source_rc_matched || source_mavlink_matched || source_any_matched || source_first_matched || source_rc_priority + || source_mavlink_priority); } manual_control_setpoint_s &ManualControlSelector::setpoint() { return _setpoint; } + +bool ManualControlSelector::isRc(uint8_t source) +{ + return source == manual_control_setpoint_s::SOURCE_RC; +} + +bool ManualControlSelector::isMavlink(uint8_t source) +{ + return (source == manual_control_setpoint_s::SOURCE_MAVLINK_0 + || source == manual_control_setpoint_s::SOURCE_MAVLINK_1 + || source == manual_control_setpoint_s::SOURCE_MAVLINK_2 + || source == manual_control_setpoint_s::SOURCE_MAVLINK_3 + || source == manual_control_setpoint_s::SOURCE_MAVLINK_4 + || source == manual_control_setpoint_s::SOURCE_MAVLINK_5); +} diff --git a/src/modules/manual_control/ManualControlSelector.hpp b/src/modules/manual_control/ManualControlSelector.hpp index ab6dfbd9e4..ae9b8662a3 100644 --- a/src/modules/manual_control/ManualControlSelector.hpp +++ b/src/modules/manual_control/ManualControlSelector.hpp @@ -48,10 +48,14 @@ public: private: bool isInputValid(const manual_control_setpoint_s &input, uint64_t now) const; + static bool isRc(uint8_t source); + static bool isMavlink(uint8_t source); manual_control_setpoint_s _setpoint{}; uint64_t _timeout{0}; int32_t _rc_in_mode{0}; int _instance{-1}; uint8_t _first_valid_source{manual_control_setpoint_s::SOURCE_UNKNOWN}; + uint64_t _timestamp_last_rc{0}; + uint64_t _timestamp_last_mavlink{0}; }; diff --git a/src/modules/manual_control/ManualControlSelectorTest.cpp b/src/modules/manual_control/ManualControlSelectorTest.cpp index 8eef8f0b7b..544f1dae2c 100644 --- a/src/modules/manual_control/ManualControlSelectorTest.cpp +++ b/src/modules/manual_control/ManualControlSelectorTest.cpp @@ -40,6 +40,8 @@ using namespace time_literals; static constexpr uint64_t SOME_TIME = 12345678; static constexpr uint8_t SOURCE_RC = manual_control_setpoint_s::SOURCE_RC; static constexpr uint8_t SOURCE_MAVLINK_0 = manual_control_setpoint_s::SOURCE_MAVLINK_0; +static constexpr uint8_t SOURCE_MAVLINK_1 = manual_control_setpoint_s::SOURCE_MAVLINK_1; +static constexpr uint8_t SOURCE_MAVLINK_2 = manual_control_setpoint_s::SOURCE_MAVLINK_2; static constexpr uint8_t SOURCE_MAVLINK_3 = manual_control_setpoint_s::SOURCE_MAVLINK_3; static constexpr uint8_t SOURCE_MAVLINK_4 = manual_control_setpoint_s::SOURCE_MAVLINK_4; @@ -175,6 +177,7 @@ TEST(ManualControlSelector, RcMavlinkInputFallback) uint64_t timestamp = SOME_TIME; + // Valid RC input gets used manual_control_setpoint_s input {}; input.data_source = SOURCE_RC; input.valid = true; @@ -187,7 +190,7 @@ TEST(ManualControlSelector, RcMavlinkInputFallback) timestamp += 100_ms; - // Now provide input from MAVLink as well which should get ignored. + // Now provide input from MAVLink as well which should get ignored input.data_source = SOURCE_MAVLINK_0; input.timestamp_sample = timestamp; selector.updateWithNewInputSample(timestamp, input, 1); @@ -198,7 +201,7 @@ TEST(ManualControlSelector, RcMavlinkInputFallback) timestamp += 500_ms; - // Now we'll let RC time out, so it should switch to MAVLINK. + // Now we update MAVLink and let RC time out, so it should switch to RC input.data_source = SOURCE_MAVLINK_0; input.timestamp_sample = timestamp; selector.updateWithNewInputSample(timestamp, input, 1); @@ -206,6 +209,15 @@ TEST(ManualControlSelector, RcMavlinkInputFallback) EXPECT_TRUE(selector.setpoint().valid); EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0); EXPECT_EQ(selector.instance(), 1); + + // If we get RC back immediately, it stays with MAVLink (until RC is lost again) + input.data_source = SOURCE_RC; + input.timestamp_sample = timestamp; + selector.updateWithNewInputSample(timestamp, input, 1); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0); + EXPECT_EQ(selector.instance(), 1); } TEST(ManualControlSelector, RcMavlinkInputKeepFirst) @@ -339,3 +351,156 @@ TEST(ManualControlSelector, RcOutdated) EXPECT_FALSE(selector.setpoint().valid); EXPECT_EQ(selector.instance(), -1); } + +TEST(ManualControlSelector, RcMavlinkInputRcPriority) +{ + ManualControlSelector selector; + selector.setRcInMode(5); // Configure RC priority + selector.setTimeout(500_ms); + + uint64_t timestamp = SOME_TIME; + + // Valid RC input gets used + manual_control_setpoint_s input{}; + input.data_source = SOURCE_RC; + input.valid = true; + input.timestamp_sample = timestamp; + selector.updateWithNewInputSample(timestamp, input, 0); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC); + EXPECT_EQ(selector.instance(), 0); + + timestamp += 100_ms; + + // Now provide input from MAVLink as well which should get ignored + input.data_source = SOURCE_MAVLINK_0; + input.timestamp_sample = timestamp; + selector.updateWithNewInputSample(timestamp, input, 1); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC); + EXPECT_EQ(selector.instance(), 0); + + timestamp += 500_ms; + + // Now we update MAVLink and let RC time out, so it should switch to RC + input.data_source = SOURCE_MAVLINK_0; + input.timestamp_sample = timestamp; + selector.updateWithNewInputSample(timestamp, input, 1); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0); + EXPECT_EQ(selector.instance(), 1); + + // If we get RC back immediately, it should use it with priority + input.data_source = SOURCE_RC; + input.timestamp_sample = timestamp; + selector.updateWithNewInputSample(timestamp, input, 0); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC); + EXPECT_EQ(selector.instance(), 0); +} + +TEST(ManualControlSelector, RcMavlinkInputMavlinkPriority) +{ + ManualControlSelector selector; + selector.setRcInMode(6); // Configure MAVLink priority + selector.setTimeout(500_ms); + + uint64_t timestamp = SOME_TIME; + + // Valid MAVLink input gets used + manual_control_setpoint_s input{}; + input.data_source = SOURCE_MAVLINK_0; + input.valid = true; + input.timestamp_sample = timestamp; + selector.updateWithNewInputSample(timestamp, input, 0); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0); + EXPECT_EQ(selector.instance(), 0); + + timestamp += 100_ms; + + // Now provide input from RC as well which should get ignored + input.data_source = SOURCE_RC; + input.timestamp_sample = timestamp; + selector.updateWithNewInputSample(timestamp, input, 1); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0); + EXPECT_EQ(selector.instance(), 0); + + timestamp += 500_ms; + + // Now we update RC and let MAVLink time out, so it should switch to RC + input.data_source = SOURCE_RC; + input.timestamp_sample = timestamp; + selector.updateWithNewInputSample(timestamp, input, 1); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC); + EXPECT_EQ(selector.instance(), 1); + + // If we get MAVLink back immediately, it should use it with priority + input.data_source = SOURCE_MAVLINK_0; + input.timestamp_sample = timestamp; + selector.updateWithNewInputSample(timestamp, input, 0); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0); + EXPECT_EQ(selector.instance(), 0); +} + +TEST(ManualControlSelector, MavlinkTwoInstanceInputMavlinkPriority) +{ + ManualControlSelector selector; + selector.setRcInMode(6); // Configure MAVLink priority + selector.setTimeout(500_ms); + + uint64_t timestamp = SOME_TIME; + + // Valid MAVLink 0 input gets used + manual_control_setpoint_s input{}; + input.data_source = SOURCE_MAVLINK_0; + input.valid = true; + input.timestamp_sample = timestamp; + selector.updateWithNewInputSample(timestamp, input, 0); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0); + EXPECT_EQ(selector.instance(), 0); + + timestamp += 100_ms; + + // Now provide input from MAVLink 1 as well which should get ignored + input.data_source = SOURCE_MAVLINK_1; + input.timestamp_sample = timestamp; + selector.updateWithNewInputSample(timestamp, input, 1); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0); + EXPECT_EQ(selector.instance(), 0); + + timestamp += 500_ms; + + // Now we update MAVLink 1 and let MAVLink 0 time out, so it should switch to MAVLink 1 + input.data_source = SOURCE_MAVLINK_1; + input.timestamp_sample = timestamp; + selector.updateWithNewInputSample(timestamp, input, 1); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1); + EXPECT_EQ(selector.instance(), 1); + + // If we get MAVLink 0 back immediately, it should not switch since MAVLink 1 has the same priority + input.data_source = SOURCE_MAVLINK_0; + input.timestamp_sample = timestamp; + selector.updateWithNewInputSample(timestamp, input, 0); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1); + EXPECT_EQ(selector.instance(), 1); +}