From 396bab52ade1f8ae7368993c39232693f8d7c548 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 6 Oct 2021 18:40:32 +0200 Subject: [PATCH] ManualControlSelector: refactor selection to chose based on a single condition --- .../manual_control/ManualControlSelector.cpp | 35 +++++++------------ .../manual_control/ManualControlSelector.hpp | 8 ++--- .../ManualControlSelectorTest.cpp | 4 +-- 3 files changed, 18 insertions(+), 29 deletions(-) diff --git a/src/modules/manual_control/ManualControlSelector.cpp b/src/modules/manual_control/ManualControlSelector.cpp index 9029e43144..df6f8544b8 100644 --- a/src/modules/manual_control/ManualControlSelector.cpp +++ b/src/modules/manual_control/ManualControlSelector.cpp @@ -57,29 +57,20 @@ void ManualControlSelector::update_manual_control_input(uint64_t now, const manu // If previous setpoint is timed out, set it invalid, so it can get replaced below. update_time_only(now); - if (_rc_in_mode == 0 && input.data_source == manual_control_input_s::SOURCE_RC) { - _setpoint = setpoint_from_input(input); - _setpoint.valid = true; - _instance = instance; + const bool source_rc_matched = (_rc_in_mode == 0) && (input.data_source == manual_control_input_s::SOURCE_RC); + const bool source_mavlink_matched = (_rc_in_mode == 1) && + (input.data_source == manual_control_input_s::SOURCE_MAVLINK_0 + || input.data_source == manual_control_input_s::SOURCE_MAVLINK_1 + || input.data_source == manual_control_input_s::SOURCE_MAVLINK_2 + || input.data_source == manual_control_input_s::SOURCE_MAVLINK_3 + || input.data_source == manual_control_input_s::SOURCE_MAVLINK_4 + || input.data_source == manual_control_input_s::SOURCE_MAVLINK_5); + const bool source_any_matched = (_rc_in_mode == 3); + const bool not_overriding_existing_source = !_setpoint.valid + || (_setpoint.chosen_input.data_source == input.data_source); - } else if (_rc_in_mode == 1 && (input.data_source == manual_control_input_s::SOURCE_MAVLINK_0 - || input.data_source == manual_control_input_s::SOURCE_MAVLINK_1 - || input.data_source == manual_control_input_s::SOURCE_MAVLINK_2 - || input.data_source == manual_control_input_s::SOURCE_MAVLINK_3 - || input.data_source == manual_control_input_s::SOURCE_MAVLINK_4 - || input.data_source == manual_control_input_s::SOURCE_MAVLINK_5)) { - - // We only stick to the first discovered mavlink channel. - if (_setpoint.chosen_input.data_source == input.data_source || !_setpoint.valid) { - _setpoint = setpoint_from_input(input); - _setpoint.valid = true; - _instance = instance; - } - - } else if (_rc_in_mode == 3) { - - // We only stick to the first discovered mavlink channel. - if (_setpoint.chosen_input.data_source == input.data_source || !_setpoint.valid) { + if (source_rc_matched || source_mavlink_matched || source_any_matched) { + if (not_overriding_existing_source) { _setpoint = setpoint_from_input(input); _setpoint.valid = true; _instance = instance; diff --git a/src/modules/manual_control/ManualControlSelector.hpp b/src/modules/manual_control/ManualControlSelector.hpp index 6646fd3293..fcf5005355 100644 --- a/src/modules/manual_control/ManualControlSelector.hpp +++ b/src/modules/manual_control/ManualControlSelector.hpp @@ -53,10 +53,10 @@ public: private: static manual_control_setpoint_s setpoint_from_input(const manual_control_input_s &input); - manual_control_setpoint_s _setpoint {}; - uint64_t _timeout {0}; - int32_t _rc_in_mode {0}; - int _instance {-1}; + manual_control_setpoint_s _setpoint{}; + uint64_t _timeout{0}; + int32_t _rc_in_mode{0}; + int _instance{-1}; }; } // namespace manual_control diff --git a/src/modules/manual_control/ManualControlSelectorTest.cpp b/src/modules/manual_control/ManualControlSelectorTest.cpp index c293a5c39d..a0ba5ca2e1 100644 --- a/src/modules/manual_control/ManualControlSelectorTest.cpp +++ b/src/modules/manual_control/ManualControlSelectorTest.cpp @@ -93,11 +93,9 @@ TEST(ManualControlSelector, MavlinkInputOnly) EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_MAVLINK_3); EXPECT_EQ(selector.instance(), 1); - // But only the first MAVLink source wins, others are too late. - timestamp += 100_ms; - // Now provide input with the correct source. + // But only the first MAVLink source wins, others are too late. input.data_source = manual_control_input_s::SOURCE_MAVLINK_4; input.timestamp_sample = timestamp; selector.update_manual_control_input(timestamp, input, 1);