diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 1a0668980b..91952b3740 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -84,20 +84,18 @@ void ManualControl::Run() _stick_disarm_hysteresis.set_hysteresis_time_from(false, _param_rc_arm_hyst.get() * 1_ms); _button_hysteresis.set_hysteresis_time_from(false, _param_rc_arm_hyst.get() * 1_ms); - _selector.set_rc_in_mode(_param_com_rc_in_mode.get()); - _selector.set_timeout(_param_com_rc_loss_t.get() * 1_s); + _selector.setRcInMode(_param_com_rc_in_mode.get()); + _selector.setTimeout(_param_com_rc_loss_t.get() * 1_s); } - bool found_at_least_one = false; const hrt_abstime now = hrt_absolute_time(); - const float dt_s = (now - _last_time) / 1e6f; + _selector.updateValidityOfChosenInput(now); for (int i = 0; i < MAX_MANUAL_INPUT_COUNT; i++) { manual_control_input_s manual_control_input; if (_manual_control_input_subs[i].update(&manual_control_input)) { - found_at_least_one = true; - _selector.update_manual_control_input(now, manual_control_input, i); + _selector.updateWithNewInputSample(now, manual_control_input, i); } } @@ -108,10 +106,6 @@ void ManualControl::Run() switches_updated = true; } - if (!found_at_least_one) { - _selector.update_time_only(now); - } - if (_selector.setpoint().valid) { _published_invalid_once = false; @@ -150,6 +144,7 @@ void ManualControl::Run() _previous_disarm_gesture = false; } + const float dt_s = (now - _last_time) / 1e6f; _x_diff.update(_selector.setpoint().chosen_input.x, dt_s); _y_diff.update(_selector.setpoint().chosen_input.y, dt_s); _z_diff.update(_selector.setpoint().chosen_input.z, dt_s); diff --git a/src/modules/manual_control/ManualControlSelector.cpp b/src/modules/manual_control/ManualControlSelector.cpp index 873ee9fbd4..000a270f59 100644 --- a/src/modules/manual_control/ManualControlSelector.cpp +++ b/src/modules/manual_control/ManualControlSelector.cpp @@ -37,7 +37,7 @@ namespace manual_control { -void ManualControlSelector::update_time_only(uint64_t now) +void ManualControlSelector::updateValidityOfChosenInput(uint64_t now) { if (!isInputValid(_setpoint.chosen_input, now)) { _setpoint.valid = false; @@ -45,10 +45,10 @@ void ManualControlSelector::update_time_only(uint64_t now) } } -void ManualControlSelector::update_manual_control_input(uint64_t now, const manual_control_input_s &input, int instance) +void ManualControlSelector::updateWithNewInputSample(uint64_t now, const manual_control_input_s &input, int instance) { - // First check if the setpoint in use got invalid, so it can get replaced below. - update_time_only(now); + // First check if the chosen input got invalid, so it can get replaced + updateValidityOfChosenInput(now); const bool update_existing_input = _setpoint.valid == true && input.data_source == _setpoint.chosen_input.data_source; const bool start_using_new_input = _setpoint.valid == false; diff --git a/src/modules/manual_control/ManualControlSelector.hpp b/src/modules/manual_control/ManualControlSelector.hpp index 5abaa08ede..883ffca077 100644 --- a/src/modules/manual_control/ManualControlSelector.hpp +++ b/src/modules/manual_control/ManualControlSelector.hpp @@ -43,10 +43,10 @@ namespace manual_control class ManualControlSelector { public: - void set_rc_in_mode(int32_t rc_in_mode) { _rc_in_mode = rc_in_mode; } - void set_timeout(uint64_t timeout) { _timeout = timeout; } - void update_time_only(uint64_t now); - void update_manual_control_input(uint64_t now, const manual_control_input_s &input, int instance); + void setRcInMode(int32_t rc_in_mode) { _rc_in_mode = rc_in_mode; } + void setTimeout(uint64_t timeout) { _timeout = timeout; } + void updateValidityOfChosenInput(uint64_t now); + void updateWithNewInputSample(uint64_t now, const manual_control_input_s &input, int instance); manual_control_setpoint_s &setpoint(); int instance() const { return _instance; }; diff --git a/src/modules/manual_control/ManualControlSelectorTest.cpp b/src/modules/manual_control/ManualControlSelectorTest.cpp index fc32644138..12ed384520 100644 --- a/src/modules/manual_control/ManualControlSelectorTest.cpp +++ b/src/modules/manual_control/ManualControlSelectorTest.cpp @@ -43,8 +43,8 @@ static constexpr uint64_t some_time = 12345678; TEST(ManualControlSelector, RcInputContinuous) { ManualControlSelector selector; - selector.set_rc_in_mode(0); - selector.set_timeout(500_ms); + selector.setRcInMode(0); + selector.setTimeout(500_ms); uint64_t timestamp = some_time; @@ -54,7 +54,7 @@ TEST(ManualControlSelector, RcInputContinuous) input.timestamp_sample = timestamp; for (int i = 0; i < 5; i++) { - selector.update_manual_control_input(timestamp, input, 1); + selector.updateWithNewInputSample(timestamp, input, 1); EXPECT_TRUE(selector.setpoint().valid); EXPECT_EQ(selector.setpoint().chosen_input.timestamp_sample, timestamp); EXPECT_EQ(selector.instance(), 1); @@ -67,15 +67,15 @@ TEST(ManualControlSelector, RcInputContinuous) TEST(ManualControlSelector, RcInputOnly) { ManualControlSelector selector; - selector.set_rc_in_mode(0); - selector.set_timeout(500_ms); + selector.setRcInMode(0); + selector.setTimeout(500_ms); uint64_t timestamp = some_time; manual_control_input_s input {}; input.data_source = manual_control_input_s::SOURCE_MAVLINK_0; input.timestamp_sample = timestamp; - selector.update_manual_control_input(timestamp, input, 0); + selector.updateWithNewInputSample(timestamp, input, 0); EXPECT_FALSE(selector.setpoint().valid); @@ -84,7 +84,7 @@ TEST(ManualControlSelector, RcInputOnly) // Now provide input with the correct source. input.data_source = manual_control_input_s::SOURCE_RC; input.timestamp_sample = timestamp; - selector.update_manual_control_input(timestamp, input, 1); + selector.updateWithNewInputSample(timestamp, input, 1); EXPECT_TRUE(selector.setpoint().valid); EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC); @@ -94,15 +94,15 @@ TEST(ManualControlSelector, RcInputOnly) TEST(ManualControlSelector, MavlinkInputOnly) { ManualControlSelector selector; - selector.set_rc_in_mode(1); - selector.set_timeout(500_ms); + selector.setRcInMode(1); + selector.setTimeout(500_ms); uint64_t timestamp = some_time; manual_control_input_s input {}; input.data_source = manual_control_input_s::SOURCE_RC; input.timestamp_sample = timestamp; - selector.update_manual_control_input(timestamp, input, 0); + selector.updateWithNewInputSample(timestamp, input, 0); EXPECT_FALSE(selector.setpoint().valid); @@ -111,7 +111,7 @@ TEST(ManualControlSelector, MavlinkInputOnly) // Now provide input with the correct source. input.data_source = manual_control_input_s::SOURCE_MAVLINK_3; input.timestamp_sample = timestamp; - selector.update_manual_control_input(timestamp, input, 1); + selector.updateWithNewInputSample(timestamp, input, 1); EXPECT_TRUE(selector.setpoint().valid); EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_MAVLINK_3); @@ -122,7 +122,7 @@ TEST(ManualControlSelector, MavlinkInputOnly) // 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); + selector.updateWithNewInputSample(timestamp, input, 1); EXPECT_TRUE(selector.setpoint().valid); EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_MAVLINK_3); @@ -132,15 +132,15 @@ TEST(ManualControlSelector, MavlinkInputOnly) TEST(ManualControlSelector, AutoInput) { ManualControlSelector selector; - selector.set_rc_in_mode(3); - selector.set_timeout(500_ms); + selector.setRcInMode(3); + selector.setTimeout(500_ms); uint64_t timestamp = some_time; manual_control_input_s input {}; input.data_source = manual_control_input_s::SOURCE_RC; input.timestamp_sample = timestamp; - selector.update_manual_control_input(timestamp, input, 0); + selector.updateWithNewInputSample(timestamp, input, 0); EXPECT_TRUE(selector.setpoint().valid); EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC); @@ -151,7 +151,7 @@ TEST(ManualControlSelector, AutoInput) // Now provide input from MAVLink as well which should get ignored. input.data_source = manual_control_input_s::SOURCE_MAVLINK_0; input.timestamp_sample = timestamp; - selector.update_manual_control_input(timestamp, input, 1); + selector.updateWithNewInputSample(timestamp, input, 1); EXPECT_TRUE(selector.setpoint().valid); EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC); @@ -162,7 +162,7 @@ TEST(ManualControlSelector, AutoInput) // Now we'll let RC time out, so it should switch to MAVLINK. input.data_source = manual_control_input_s::SOURCE_MAVLINK_0; input.timestamp_sample = timestamp; - selector.update_manual_control_input(timestamp, input, 1); + selector.updateWithNewInputSample(timestamp, input, 1); EXPECT_TRUE(selector.setpoint().valid); EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_MAVLINK_0); @@ -172,15 +172,15 @@ TEST(ManualControlSelector, AutoInput) TEST(ManualControlSelector, RcTimeout) { ManualControlSelector selector; - selector.set_rc_in_mode(0); - selector.set_timeout(500_ms); + selector.setRcInMode(0); + selector.setTimeout(500_ms); uint64_t timestamp = some_time; manual_control_input_s input {}; input.data_source = manual_control_input_s::SOURCE_RC; input.timestamp_sample = timestamp; - selector.update_manual_control_input(timestamp, input, 0); + selector.updateWithNewInputSample(timestamp, input, 0); EXPECT_TRUE(selector.setpoint().valid); EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC); @@ -189,7 +189,7 @@ TEST(ManualControlSelector, RcTimeout) timestamp += 600_ms; // Update, to make sure it notices the timeout - selector.update_time_only(timestamp); + selector.updateValidityOfChosenInput(timestamp); EXPECT_FALSE(selector.setpoint().valid); EXPECT_EQ(selector.instance(), -1); @@ -198,21 +198,21 @@ TEST(ManualControlSelector, RcTimeout) TEST(ManualControlSelector, RcOutdated) { ManualControlSelector selector; - selector.set_rc_in_mode(0); - selector.set_timeout(500_ms); + selector.setRcInMode(0); + selector.setTimeout(500_ms); uint64_t timestamp = some_time; manual_control_input_s input {}; input.data_source = manual_control_input_s::SOURCE_RC; input.timestamp_sample = timestamp - 600_ms; // First sample is already outdated - selector.update_manual_control_input(timestamp, input, 0); + selector.updateWithNewInputSample(timestamp, input, 0); EXPECT_FALSE(selector.setpoint().valid); EXPECT_EQ(selector.instance(), -1); // If we update again it should still not get accepted - selector.update_manual_control_input(timestamp, input, 0); + selector.updateWithNewInputSample(timestamp, input, 0); EXPECT_FALSE(selector.setpoint().valid); EXPECT_EQ(selector.instance(), -1);