From 2c062a45ba0e75afc6b29d0e7b060d14cef0e59d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20B=C3=BCchli?= Date: Wed, 24 Sep 2025 16:10:04 +0200 Subject: [PATCH] feat: expand to 4 modes with more priority options --- .../checks/rcCalibrationCheck.cpp | 4 +- src/modules/commander/commander_params.c | 14 +- src/modules/commander/failsafe/failsafe.h | 6 +- .../manual_control/ManualControlSelector.cpp | 70 +++- .../manual_control/ManualControlSelector.hpp | 6 +- .../ManualControlSelectorTest.cpp | 373 ++++++++++++++---- 6 files changed, 375 insertions(+), 98 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/rcCalibrationCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/rcCalibrationCheck.cpp index ecd152504b..d224939137 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/rcCalibrationCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/rcCalibrationCheck.cpp @@ -75,8 +75,8 @@ void RcCalibrationChecks::checkAndReport(const Context &context, Report &reporte return; } - if (_param_com_rc_in_mode.get() != 0 && _param_com_rc_in_mode.get() != 2 && _param_com_rc_in_mode.get() != 3 - && _param_com_rc_in_mode.get() != 5 && _param_com_rc_in_mode.get() != 6) { + // return early when joystick only or stick input disabled + if (_param_com_rc_in_mode.get() == 1 || _param_com_rc_in_mode.get() == 4) { return; } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 8aac196b67..ed3a98fe18 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -173,19 +173,23 @@ 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, prioritizing by ascending source ID, giving an RC transmitter priority over MAVLink joysticks, and lower MAVLink instance numbers priority over higher ones. - * A value of 6 allows either RC Transmitter or Joystick input, prioritizing by descending source ID, giving MAVLink joysticks priority over an RC transmitter, and higher MAVLink instance numbers priority over lower ones. + * A value of 5 allows either RC Transmitter or Joystick input, giving an RC transmitter priority over MAVLink joysticks, and lower MAVLink instance numbers priority over higher ones. + * A value of 6 allows either RC Transmitter or Joystick input, giving MAVLink joysticks priority over an RC transmitter, and lower MAVLink instance numbers priority over higher ones. + * A value of 7 allows either RC Transmitter or Joystick input, giving an RC transmitter priority over MAVLink joysticks, and higher MAVLink instance numbers priority over lower ones. + * A value of 8 allows either RC Transmitter or Joystick input, giving MAVLink joysticks priority over an RC transmitter, and higher MAVLink instance numbers priority over lower ones. * * @group Commander * @min 0 - * @max 6 + * @max 8 * @value 0 RC Transmitter only * @value 1 Joystick only * @value 2 RC and Joystick with fallback * @value 3 RC or Joystick keep first * @value 4 Stick input disabled - * @value 5 Ascending priority - * @value 6 Descending priority + * @value 5 RC, then Joystick ascending prio + * @value 6 Joystick ascending prio, then RC + * @value 7 RC, then Joystick descending prio + * @value 8 Joystick descending prio, then RC */ PARAM_DEFINE_INT32(COM_RC_IN_MODE, 3); diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index d3af2f9059..47d292652f 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -133,8 +133,10 @@ private: RcAndJoystickWithFallback = 2, // RC And Joystick with fallback RcOrJoystickKeepFirst = 3, // RC or Joystick keep first StickInputDisabled = 4, // input disabled - AscendingPriority = 5, // ascending source priority - DescendingPriority = 6 // descending source priority + RcThenJoystickAscendingPriority = 5, // RC, then Joystick ascending prio + JoystickAscendingThenRcPriority = 6, // Joystick ascending prio, then RC + RcThenJoystickDescendingPriority = 7, // RC, then Joystick descending prio + JoystickDescendingThenRcPriority = 8 // Joystick descending prio, then RC }; enum class command_after_high_wind_failsafe : int32_t { diff --git a/src/modules/manual_control/ManualControlSelector.cpp b/src/modules/manual_control/ManualControlSelector.cpp index 53bd82b89f..9c8720818a 100644 --- a/src/modules/manual_control/ManualControlSelector.cpp +++ b/src/modules/manual_control/ManualControlSelector.cpp @@ -48,8 +48,10 @@ void ManualControlSelector::updateWithNewInputSample(uint64_t now, const manual_ const bool update_existing_input = _setpoint.valid && (input.data_source == _setpoint.data_source); const bool start_using_new_input = !_setpoint.valid; - const bool is_priority_mode = (_rc_in_mode == RcInMode::AscendingPriority - || _rc_in_mode == RcInMode::DescendingPriority); + const bool is_priority_mode = (_rc_in_mode == RcInMode::RcThenJoystickAscendingPriority + || _rc_in_mode == RcInMode::JoystickAscendingThenRcPriority + || _rc_in_mode == RcInMode::RcThenJoystickDescendingPriority + || _rc_in_mode == RcInMode::JoystickDescendingThenRcPriority); // Switch to new input if it's valid and we don't already have a valid one if (isInputValid(input, now) && (update_existing_input || start_using_new_input || is_priority_mode)) { @@ -91,12 +93,68 @@ bool ManualControlSelector::isInputValid(const manual_control_setpoint_s &input, || (_first_valid_source == manual_control_setpoint_s::SOURCE_UNKNOWN); break; - case RcInMode::AscendingPriority: - match = !_setpoint.valid || (input.data_source <= _setpoint.data_source); + case RcInMode::RcThenJoystickAscendingPriority: + if (isRc(input.data_source)) { + match = true; + + } else if (isMavlink(input.data_source)) { + if (!_setpoint.valid) { + match = true; + + } else if (isMavlink(_setpoint.data_source)) { + match = input.data_source <= _setpoint.data_source; + } + } + break; - case RcInMode::DescendingPriority: - match = !_setpoint.valid || (input.data_source >= _setpoint.data_source); + case RcInMode::JoystickAscendingThenRcPriority: + if (isMavlink(input.data_source)) { + if (!_setpoint.valid || isRc(_setpoint.data_source)) { + match = true; + + } else if (isMavlink(_setpoint.data_source)) { + match = input.data_source <= _setpoint.data_source; + } + + } else if (isRc(input.data_source)) { + if (!_setpoint.valid || isRc(_setpoint.data_source)) { + match = true; + } + } + + break; + + case RcInMode::RcThenJoystickDescendingPriority: + if (isRc(input.data_source)) { + match = true; + + } else if (isMavlink(input.data_source)) { + if (!_setpoint.valid) { + match = true; + + } else if (isMavlink(_setpoint.data_source)) { + match = input.data_source >= _setpoint.data_source; + } + } + + break; + + case RcInMode::JoystickDescendingThenRcPriority: + if (isMavlink(input.data_source)) { + if (!_setpoint.valid || isRc(_setpoint.data_source)) { + match = true; + + } else if (isMavlink(_setpoint.data_source)) { + match = input.data_source >= _setpoint.data_source; + } + + } else if (isRc(input.data_source)) { + if (!_setpoint.valid || isRc(_setpoint.data_source)) { + match = true; + } + } + break; case RcInMode::StickInputDisabled: diff --git a/src/modules/manual_control/ManualControlSelector.hpp b/src/modules/manual_control/ManualControlSelector.hpp index b7f5647021..fb4abfe170 100644 --- a/src/modules/manual_control/ManualControlSelector.hpp +++ b/src/modules/manual_control/ManualControlSelector.hpp @@ -58,8 +58,10 @@ private: RcAndJoystickWithFallback = 2, // RC And Joystick with fallback RcOrJoystickKeepFirst = 3, // RC or Joystick keep first StickInputDisabled = 4, // input disabled - AscendingPriority = 5, // ascending source priority - DescendingPriority = 6 // descending source priority + RcThenJoystickAscendingPriority = 5, // RC, then Joystick ascending prio + JoystickAscendingThenRcPriority = 6, // Joystick ascending prio, then RC + RcThenJoystickDescendingPriority = 7, // RC, then Joystick descending prio + JoystickDescendingThenRcPriority = 8 // Joystick descending prio, then RC }; manual_control_setpoint_s _setpoint{}; diff --git a/src/modules/manual_control/ManualControlSelectorTest.cpp b/src/modules/manual_control/ManualControlSelectorTest.cpp index a4850679fd..6b42cfca7b 100644 --- a/src/modules/manual_control/ManualControlSelectorTest.cpp +++ b/src/modules/manual_control/ManualControlSelectorTest.cpp @@ -352,15 +352,15 @@ TEST(ManualControlSelector, RcOutdated) EXPECT_EQ(selector.instance(), -1); } -TEST(ManualControlSelector, AscendingSourcePriority) +TEST(ManualControlSelector, RcThenJoystickAscendingPriority) { ManualControlSelector selector; - selector.setRcInMode(5); // Configure ascending source ID priority (lower number wins) + selector.setRcInMode(5); // Configure RC, then Joystick ascending selector.setTimeout(500_ms); uint64_t timestamp = SOME_TIME; - // Valid RC input (ID 1) gets used + // Valid RC input gets used manual_control_setpoint_s input{}; input.data_source = SOURCE_RC; input.valid = true; @@ -373,7 +373,7 @@ TEST(ManualControlSelector, AscendingSourcePriority) timestamp += 100_ms; - // Now provide input from MAVLink (ID 2) as well which should get ignored (1 < 2) + // Now provide input from MAVLink 0 as well which should get ignored input.data_source = SOURCE_MAVLINK_0; input.timestamp_sample = timestamp; selector.updateWithNewInputSample(timestamp, input, 1); @@ -384,7 +384,7 @@ TEST(ManualControlSelector, AscendingSourcePriority) timestamp += 500_ms; - // Now we update MAVLink and let RC time out, so it should switch to MAVLink + // Now we update MAVLink 0 and let RC time out, so it should switch to MAVLink 0 input.data_source = SOURCE_MAVLINK_0; input.timestamp_sample = timestamp; selector.updateWithNewInputSample(timestamp, input, 1); @@ -393,7 +393,7 @@ TEST(ManualControlSelector, AscendingSourcePriority) EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0); EXPECT_EQ(selector.instance(), 1); - // If we get RC back immediately, it should take over with priority (1 < 2) + // If we get RC back immediately, it should take over with priority input.data_source = SOURCE_RC; input.timestamp_sample = timestamp; selector.updateWithNewInputSample(timestamp, input, 0); @@ -402,157 +402,368 @@ TEST(ManualControlSelector, AscendingSourcePriority) EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC); EXPECT_EQ(selector.instance(), 0); -} + timestamp += 100_ms; -TEST(ManualControlSelector, MavlinkTwoInstanceAscendingPriority) -{ - ManualControlSelector selector; - selector.setRcInMode(5); // Configure ascending source ID priority (lower number wins) - selector.setTimeout(500_ms); - - uint64_t timestamp = SOME_TIME; - - // Valid MAVLink 1 input (ID 3) gets used - manual_control_setpoint_s input{}; + // Now provide input from MAVLink 1, which should get ignored input.data_source = SOURCE_MAVLINK_1; - input.valid = true; input.timestamp_sample = timestamp; - selector.updateWithNewInputSample(timestamp, input, 1); + selector.updateWithNewInputSample(timestamp, input, 2); + + 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 1 and let RC time out, so it should switch to MAVLink 1 + input.data_source = SOURCE_MAVLINK_1; + input.timestamp_sample = timestamp; + selector.updateWithNewInputSample(timestamp, input, 2); EXPECT_TRUE(selector.setpoint().valid); EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1); - EXPECT_EQ(selector.instance(), 1); + EXPECT_EQ(selector.instance(), 2); timestamp += 100_ms; - // Now provide input from MAVLink 0 (ID 2), which has higher priority (2 < 3) and should take over + // Now provide input from MAVLink 0, which has higher priority and should take over 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); - - // If we get MAVLink 1 back immediately, it should be ignored since MAVLink 0 has higher priority (2 < 3) - 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); + EXPECT_EQ(selector.instance(), 1); + + // If we get MAVLink 1 back immediately, it should be ignored since MAVLink 0 has higher priority + input.data_source = SOURCE_MAVLINK_1; + input.timestamp_sample = timestamp; + selector.updateWithNewInputSample(timestamp, input, 2); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0); + EXPECT_EQ(selector.instance(), 1); 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); + selector.updateWithNewInputSample(timestamp, input, 2); EXPECT_TRUE(selector.setpoint().valid); EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1); - EXPECT_EQ(selector.instance(), 1); + EXPECT_EQ(selector.instance(), 2); } -TEST(ManualControlSelector, DescendingSourcePriority) +TEST(ManualControlSelector, JoystickAscendingThenRcPriority) { ManualControlSelector selector; - selector.setRcInMode(6); // Configure descending source ID priority (higher number wins) + selector.setRcInMode(6); // Configure Joystick ascending, then RC selector.setTimeout(500_ms); uint64_t timestamp = SOME_TIME; - // Valid MAVLink input (ID 2) gets used + // 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, 1); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0); + EXPECT_EQ(selector.instance(), 1); + + 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, 2); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0); + EXPECT_EQ(selector.instance(), 1); + + 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, 2); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1); + EXPECT_EQ(selector.instance(), 2); + + // If we get MAVLink 0 back immediately, it should take over with priority + 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); + + timestamp += 100_ms; + + // Now provide input from RC, which should get ignored + 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_MAVLINK_0); + EXPECT_EQ(selector.instance(), 1); + + timestamp += 500_ms; + + // Now we update RC and let MAVLink 0 time out, so it should switch to RC + 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); timestamp += 100_ms; - // Now provide input from RC (ID 1) as well which should get ignored (2 > 1) - input.data_source = SOURCE_RC; + // Now provide input from MAVLink 1, which has higher priority and should take over + input.data_source = SOURCE_MAVLINK_1; input.timestamp_sample = timestamp; - selector.updateWithNewInputSample(timestamp, input, 1); + selector.updateWithNewInputSample(timestamp, input, 2); EXPECT_TRUE(selector.setpoint().valid); - EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0); + EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1); + EXPECT_EQ(selector.instance(), 2); + + // If we get RC back immediately, it should be ignored since MAVLink 1 has higher 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_MAVLINK_1); + EXPECT_EQ(selector.instance(), 2); + + timestamp += 500_ms; + + // Now we update RC and let MAVLink 1 time out, so it should switch to RC + 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, RcThenJoystickDescendingPriority) +{ + ManualControlSelector selector; + selector.setRcInMode(7); // Configure RC, then Joystick descending + 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 1 as well which should get ignored + input.data_source = SOURCE_MAVLINK_1; + input.timestamp_sample = timestamp; + selector.updateWithNewInputSample(timestamp, input, 2); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC); EXPECT_EQ(selector.instance(), 0); timestamp += 500_ms; - // Now we update RC and let MAVLink time out, so it should switch to RC + // Now we update MAVLink 0 and let RC time out, so it should switch to MAVLink 0 + input.data_source = SOURCE_MAVLINK_1; + input.timestamp_sample = timestamp; + selector.updateWithNewInputSample(timestamp, input, 2); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1); + EXPECT_EQ(selector.instance(), 2); + + // If we get RC back immediately, it should take over 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); + + timestamp += 100_ms; + + // Now provide input from MAVLink 0, 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 0 and let RC time out, so it should switch to MAVLink 0 + 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 MAVLink back immediately, it should take over with priority (2 > 1) - 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, MavlinkTwoInstanceDescendingPriority) -{ - ManualControlSelector selector; - selector.setRcInMode(6); // Configure descending source ID priority (higher number wins) - selector.setTimeout(500_ms); - - uint64_t timestamp = SOME_TIME; - - // Valid MAVLink 0 input (ID 2) 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 (ID 3), which has higher priority (3 > 2) and should take over + // Now provide input from MAVLink 1, which has higher priority and should take over input.data_source = SOURCE_MAVLINK_1; input.timestamp_sample = timestamp; + selector.updateWithNewInputSample(timestamp, input, 2); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1); + EXPECT_EQ(selector.instance(), 2); + + // If we get MAVLink 0 back immediately, it should be ignored since MAVLink 1 has higher priority + 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_1); - EXPECT_EQ(selector.instance(), 1); - - // If we get MAVLink 0 back immediately, it should be ignored since MAVLink 1 has higher priority (3 > 2) - 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); + EXPECT_EQ(selector.instance(), 2); timestamp += 500_ms; // Now we update MAVLink 0 and let MAVLink 1 time out, so it should switch to MAVLink 0 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); +} + +TEST(ManualControlSelector, JoystickDescendingThenRcPriority) +{ + ManualControlSelector selector; + selector.setRcInMode(8); // Configure Joystick descending, then RC + selector.setTimeout(500_ms); + + uint64_t timestamp = SOME_TIME; + + // Valid MAVLink 1 input gets used + manual_control_setpoint_s input{}; + input.data_source = SOURCE_MAVLINK_1; + input.valid = true; + input.timestamp_sample = timestamp; + selector.updateWithNewInputSample(timestamp, input, 2); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1); + EXPECT_EQ(selector.instance(), 2); + + timestamp += 100_ms; + + // Now provide input from MAVLink 0 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_MAVLINK_1); + EXPECT_EQ(selector.instance(), 2); + + timestamp += 500_ms; + + // Now we update MAVLink 0 and let MAVLink 1 time out, so it should switch to MAVLink 0 + 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 MAVLink 1 back immediately, it should take over with priority + input.data_source = SOURCE_MAVLINK_1; + input.timestamp_sample = timestamp; + selector.updateWithNewInputSample(timestamp, input, 2); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1); + EXPECT_EQ(selector.instance(), 2); + + timestamp += 100_ms; + + // Now provide input from RC, which should get ignored + 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_MAVLINK_1); + EXPECT_EQ(selector.instance(), 2); + + timestamp += 500_ms; + + // Now we update RC and let MAVLink 1 time out, so it should switch to RC + 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); + + timestamp += 100_ms; + + // Now provide input from MAVLink 0, which has higher priority and should take over + 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 be ignored since MAVLink 0 has higher 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_MAVLINK_0); + EXPECT_EQ(selector.instance(), 1); + + timestamp += 500_ms; + + // Now we update RC and let MAVLink 0 time out, so it should switch to RC + 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); }