diff --git a/src/modules/commander/HealthAndArmingChecks/checks/rcCalibrationCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/rcCalibrationCheck.cpp index e41d3c0653..ecd152504b 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/rcCalibrationCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/rcCalibrationCheck.cpp @@ -75,7 +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) { + 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; } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 45da253f11..8aac196b67 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -173,19 +173,19 @@ 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. + * 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. * * @group Commander * @min 0 - * @max 4 + * @max 6 * @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 RC priority, Joystick fallback - * @value 6 Joystick priority, RC fallback + * @value 5 Ascending priority + * @value 6 Descending priority */ 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 e3849bf8b1..d3af2f9059 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -132,7 +132,9 @@ private: JoystickOnly = 1, // Joystick only RcAndJoystickWithFallback = 2, // RC And Joystick with fallback RcOrJoystickKeepFirst = 3, // RC or Joystick keep first - StickInputDisabled = 4 // input disabled + StickInputDisabled = 4, // input disabled + AscendingPriority = 5, // ascending source priority + DescendingPriority = 6 // descending source priority }; 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 0ea367d8c9..53bd82b89f 100644 --- a/src/modules/manual_control/ManualControlSelector.cpp +++ b/src/modules/manual_control/ManualControlSelector.cpp @@ -43,20 +43,16 @@ void ManualControlSelector::updateValidityOfChosenInput(uint64_t now) void ManualControlSelector::updateWithNewInputSample(uint64_t now, const manual_control_setpoint_s &input, int instance) { - if (input.valid) { - 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); 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); // 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)) { + if (isInputValid(input, now) && (update_existing_input || start_using_new_input || is_priority_mode)) { _setpoint = input; _setpoint.valid = true; _setpoint.timestamp = now; // timestamp_sample is preserved @@ -78,32 +74,32 @@ bool ManualControlSelector::isInputValid(const manual_control_setpoint_s &input, bool match = false; switch (_rc_in_mode) { // COM_RC_IN_MODE - case 0: // RC Transmitter only + case RcInMode::RcTransmitterOnly: match = isRc(input.data_source); break; - case 1: // Joystick only + case RcInMode::JoystickOnly: match = isMavlink(input.data_source); break; - case 2: // RC and Joystick with fallback + case RcInMode::RcAndJoystickWithFallback: match = true; break; - case 3: // RC or Joystick keep first + case RcInMode::RcOrJoystickKeepFirst: match = (input.data_source == _first_valid_source) || (_first_valid_source == manual_control_setpoint_s::SOURCE_UNKNOWN); break; - case 5: // RC priority, Joystick fallback - match = isRc(input.data_source) || (now > _timestamp_last_rc + _timeout); + case RcInMode::AscendingPriority: + match = !_setpoint.valid || (input.data_source <= _setpoint.data_source); break; - case 6: // Joystick priority, RC fallback - match = isMavlink(input.data_source) || (now > _timestamp_last_mavlink + _timeout); + case RcInMode::DescendingPriority: + match = !_setpoint.valid || (input.data_source >= _setpoint.data_source); break; - case 4: // Stick input disabled + case RcInMode::StickInputDisabled: default: break; } diff --git a/src/modules/manual_control/ManualControlSelector.hpp b/src/modules/manual_control/ManualControlSelector.hpp index ae9b8662a3..b7f5647021 100644 --- a/src/modules/manual_control/ManualControlSelector.hpp +++ b/src/modules/manual_control/ManualControlSelector.hpp @@ -39,7 +39,7 @@ class ManualControlSelector { public: - void setRcInMode(int32_t rc_in_mode) { _rc_in_mode = rc_in_mode; } + void setRcInMode(int32_t rc_in_mode) { _rc_in_mode = static_cast(rc_in_mode); } void setTimeout(uint64_t timeout) { _timeout = timeout; } void updateValidityOfChosenInput(uint64_t now); void updateWithNewInputSample(uint64_t now, const manual_control_setpoint_s &input, int instance); @@ -51,11 +51,20 @@ private: static bool isRc(uint8_t source); static bool isMavlink(uint8_t source); + // COM_RC_IN_MODE parameter values + enum class RcInMode : int32_t { + RcTransmitterOnly = 0, // RC Transmitter only + JoystickOnly = 1, // Joystick only + 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 + }; + manual_control_setpoint_s _setpoint{}; uint64_t _timeout{0}; - int32_t _rc_in_mode{0}; + RcInMode _rc_in_mode{RcInMode::RcTransmitterOnly}; 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 544f1dae2c..a4850679fd 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, RcMavlinkInputRcPriority) +TEST(ManualControlSelector, AscendingSourcePriority) { ManualControlSelector selector; - selector.setRcInMode(5); // Configure RC priority + selector.setRcInMode(5); // Configure ascending source ID priority (lower number wins) selector.setTimeout(500_ms); uint64_t timestamp = SOME_TIME; - // Valid RC input gets used + // Valid RC input (ID 1) gets used manual_control_setpoint_s input{}; input.data_source = SOURCE_RC; input.valid = true; @@ -373,7 +373,7 @@ TEST(ManualControlSelector, RcMavlinkInputRcPriority) timestamp += 100_ms; - // Now provide input from MAVLink as well which should get ignored + // Now provide input from MAVLink (ID 2) as well which should get ignored (1 < 2) input.data_source = SOURCE_MAVLINK_0; input.timestamp_sample = timestamp; selector.updateWithNewInputSample(timestamp, input, 1); @@ -384,7 +384,7 @@ TEST(ManualControlSelector, RcMavlinkInputRcPriority) timestamp += 500_ms; - // Now we update MAVLink and let RC time out, so it should switch to RC + // Now we update MAVLink and let RC time out, so it should switch to MAVLink input.data_source = SOURCE_MAVLINK_0; input.timestamp_sample = timestamp; selector.updateWithNewInputSample(timestamp, input, 1); @@ -393,7 +393,7 @@ TEST(ManualControlSelector, RcMavlinkInputRcPriority) 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 + // If we get RC back immediately, it should take over with priority (1 < 2) input.data_source = SOURCE_RC; input.timestamp_sample = timestamp; selector.updateWithNewInputSample(timestamp, input, 0); @@ -401,17 +401,69 @@ TEST(ManualControlSelector, RcMavlinkInputRcPriority) EXPECT_TRUE(selector.setpoint().valid); EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC); EXPECT_EQ(selector.instance(), 0); + } -TEST(ManualControlSelector, RcMavlinkInputMavlinkPriority) +TEST(ManualControlSelector, MavlinkTwoInstanceAscendingPriority) { ManualControlSelector selector; - selector.setRcInMode(6); // Configure MAVLink priority + selector.setRcInMode(5); // Configure ascending source ID priority (lower number wins) selector.setTimeout(500_ms); uint64_t timestamp = SOME_TIME; - // Valid MAVLink input gets used + // Valid MAVLink 1 input (ID 3) gets used + manual_control_setpoint_s input{}; + input.data_source = SOURCE_MAVLINK_1; + 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_1); + EXPECT_EQ(selector.instance(), 1); + + timestamp += 100_ms; + + // Now provide input from MAVLink 0 (ID 2), which has higher priority (2 < 3) 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); + + 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); +} + +TEST(ManualControlSelector, DescendingSourcePriority) +{ + ManualControlSelector selector; + selector.setRcInMode(6); // Configure descending source ID priority (higher number wins) + selector.setTimeout(500_ms); + + uint64_t timestamp = SOME_TIME; + + // Valid MAVLink input (ID 2) gets used manual_control_setpoint_s input{}; input.data_source = SOURCE_MAVLINK_0; input.valid = true; @@ -424,7 +476,7 @@ TEST(ManualControlSelector, RcMavlinkInputMavlinkPriority) timestamp += 100_ms; - // Now provide input from RC as well which should get ignored + // Now provide input from RC (ID 1) as well which should get ignored (2 > 1) input.data_source = SOURCE_RC; input.timestamp_sample = timestamp; selector.updateWithNewInputSample(timestamp, input, 1); @@ -444,7 +496,7 @@ TEST(ManualControlSelector, RcMavlinkInputMavlinkPriority) 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 + // 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); @@ -454,15 +506,15 @@ TEST(ManualControlSelector, RcMavlinkInputMavlinkPriority) EXPECT_EQ(selector.instance(), 0); } -TEST(ManualControlSelector, MavlinkTwoInstanceInputMavlinkPriority) +TEST(ManualControlSelector, MavlinkTwoInstanceDescendingPriority) { ManualControlSelector selector; - selector.setRcInMode(6); // Configure MAVLink priority + selector.setRcInMode(6); // Configure descending source ID priority (higher number wins) selector.setTimeout(500_ms); uint64_t timestamp = SOME_TIME; - // Valid MAVLink 0 input gets used + // Valid MAVLink 0 input (ID 2) gets used manual_control_setpoint_s input{}; input.data_source = SOURCE_MAVLINK_0; input.valid = true; @@ -475,18 +527,7 @@ TEST(ManualControlSelector, MavlinkTwoInstanceInputMavlinkPriority) 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 + // Now provide input from MAVLink 1 (ID 3), which has higher priority (3 > 2) and should take over input.data_source = SOURCE_MAVLINK_1; input.timestamp_sample = timestamp; selector.updateWithNewInputSample(timestamp, input, 1); @@ -495,7 +536,7 @@ TEST(ManualControlSelector, MavlinkTwoInstanceInputMavlinkPriority) 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 + // 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); @@ -503,4 +544,15 @@ TEST(ManualControlSelector, MavlinkTwoInstanceInputMavlinkPriority) EXPECT_TRUE(selector.setpoint().valid); EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1); EXPECT_EQ(selector.instance(), 1); + + 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, 0); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0); + EXPECT_EQ(selector.instance(), 0); }