diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg index a39e1f6911..e421d1d291 100644 --- a/msg/manual_control_setpoint.msg +++ b/msg/manual_control_setpoint.msg @@ -2,6 +2,8 @@ uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp of the raw data (microseconds) +bool valid + uint8 SOURCE_UNKNOWN = 0 uint8 SOURCE_RC = 1 # radio control (input_rc) uint8 SOURCE_MAVLINK_0 = 2 # mavlink instance 0 diff --git a/src/modules/manual_control/CMakeLists.txt b/src/modules/manual_control/CMakeLists.txt index 9a6652768a..c96478afdf 100644 --- a/src/modules/manual_control/CMakeLists.txt +++ b/src/modules/manual_control/CMakeLists.txt @@ -39,6 +39,10 @@ px4_add_module( ManualControl.cpp ManualControl.cpp ManualControl.hpp + ManualControlSelector.hpp + ManualControlSelector.cpp DEPENDS px4_work_queue ) + +px4_add_unit_gtest(SRC ManualControlSelectorTest.cpp LINKLIBS module__manual_control) diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index b4fd18f53e..7791c83586 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -77,108 +77,76 @@ void ManualControl::Run() _stick_arm_hysteresis.set_hysteresis_time_from(false, _param_rc_arm_hyst.get() * 1_ms); _stick_disarm_hysteresis.set_hysteresis_time_from(false, _param_rc_arm_hyst.get() * 1_ms); - } - const hrt_abstime rc_timeout = _param_com_rc_loss_t.get() * 1_s; + _selector.set_rc_in_mode(_param_com_rc_in_mode.get()); + _selector.set_timeout(_param_com_rc_loss_t.get() * 1_s); + } - bool publish_once = false; - int selected_manual_input = -1; + bool found_at_least_one = false; + const hrt_abstime now = hrt_absolute_time(); 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)) { - bool publish = false; - - if ((_param_com_rc_in_mode.get() == 0) - && (manual_control_input.data_source == manual_control_input_s::SOURCE_RC)) { - - publish = true; - - } else if ((_param_com_rc_in_mode.get() == 1) - && (manual_control_input.data_source >= manual_control_input_s::SOURCE_MAVLINK_0)) { - - publish = true; - - } else { - // otherwise, first come, first serve (REVIEW) - publish = true; - } - + found_at_least_one = true; - bool available = (hrt_elapsed_time(&manual_control_input.timestamp) < rc_timeout) - && (hrt_elapsed_time(&_manual_control_input[i].timestamp) < rc_timeout); + _selector.update_manual_control_input(now, manual_control_input); - if (publish && available && !publish_once) { - const float dt_inv = 1e6f / static_cast(manual_control_input.timestamp_sample - - _manual_control_input[i].timestamp_sample); - - manual_control_setpoint_s manual_control_setpoint{}; + } + } - manual_control_setpoint.timestamp_sample = manual_control_input.timestamp_sample; - manual_control_setpoint.x = manual_control_input.x; - manual_control_setpoint.y = manual_control_input.y; - manual_control_setpoint.z = manual_control_input.z; - manual_control_setpoint.r = manual_control_input.r; - manual_control_setpoint.vx = (manual_control_input.x - _manual_control_input[i].x) * dt_inv; - manual_control_setpoint.vy = (manual_control_input.y - _manual_control_input[i].y) * dt_inv; - manual_control_setpoint.vz = (manual_control_input.z - _manual_control_input[i].z) * dt_inv; - manual_control_setpoint.vr = (manual_control_input.r - _manual_control_input[i].r) * dt_inv; - manual_control_setpoint.flaps = manual_control_input.flaps; - manual_control_setpoint.aux1 = manual_control_input.aux1; - manual_control_setpoint.aux2 = manual_control_input.aux2; - manual_control_setpoint.aux3 = manual_control_input.aux3; - manual_control_setpoint.aux4 = manual_control_input.aux4; - manual_control_setpoint.aux5 = manual_control_input.aux5; - manual_control_setpoint.aux6 = manual_control_input.aux6; - manual_control_setpoint.data_source = manual_control_input.data_source; + if (!found_at_least_one) { + _selector.update_time_only(now); + } + if (_selector.setpoint().valid) { + _published_invalid_once = false; - // user arm/disarm gesture - const bool right_stick_centered = (fabsf(manual_control_input.x) < 0.1f) && (fabsf(manual_control_input.y) < 0.1f); - const bool stick_lower_left = (manual_control_input.z < 0.1f) && (manual_control_input.r < -0.9f); - const bool stick_lower_right = (manual_control_input.z < 0.1f) && (manual_control_input.r > 0.9f); + // user arm/disarm gesture + const bool right_stick_centered = (fabsf(_selector.setpoint().x) < 0.1f) && (fabsf(_selector.setpoint().y) < 0.1f); + const bool stick_lower_left = (_selector.setpoint().z < 0.1f) && (_selector.setpoint().r < -0.9f); + const bool stick_lower_right = (_selector.setpoint().z < 0.1f) && (_selector.setpoint().r > 0.9f); - _stick_arm_hysteresis.set_state_and_update(stick_lower_right && right_stick_centered, manual_control_input.timestamp); - _stick_disarm_hysteresis.set_state_and_update(stick_lower_left && right_stick_centered, manual_control_input.timestamp); - manual_control_setpoint.arm_gesture = _stick_arm_hysteresis.get_state(); - manual_control_setpoint.disarm_gesture = _stick_disarm_hysteresis.get_state(); + _stick_arm_hysteresis.set_state_and_update(stick_lower_right && right_stick_centered, _selector.setpoint().timestamp); + _stick_disarm_hysteresis.set_state_and_update(stick_lower_left && right_stick_centered, _selector.setpoint().timestamp); + _selector.setpoint().arm_gesture = _stick_arm_hysteresis.get_state(); + _selector.setpoint().disarm_gesture = _stick_disarm_hysteresis.get_state(); - // user wants override - const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get(); - const bool rpy_moved = (fabsf(manual_control_input.x - _manual_control_input[i].x) > minimum_stick_change) - || (fabsf(manual_control_input.y - _manual_control_input[i].y) > minimum_stick_change) - || (fabsf(manual_control_input.r - _manual_control_input[i].r) > minimum_stick_change); + // user wants override + //const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get(); - // Throttle change value doubled to achieve the same scaling even though the range is [0,1] instead of [-1,1] - const bool throttle_moved = (fabsf(manual_control_input.z - _manual_control_input[i].z) * 2.f > minimum_stick_change); - const bool use_throttle = !(_param_rc_override.get() & OverrideBits::OVERRIDE_IGNORE_THROTTLE_BIT); + // FIXME: we need previous + //const bool rpy_moved = (fabsf(_selector.setpoint().x - _manual_control_input[i].x) > minimum_stick_change) + // || (fabsf(_selector.setpoint().y - _manual_control_input[i].y) > minimum_stick_change) + // || (fabsf(_selector.setpoint().r - _manual_control_input[i].r) > minimum_stick_change); - manual_control_setpoint.user_override = rpy_moved || (use_throttle && throttle_moved); + // Throttle change value doubled to achieve the same scaling even though the range is [0,1] instead of [-1,1] + //const bool throttle_moved = (fabsf(_selector.setpoint().z - _manual_control_input[i].z) * 2.f > minimum_stick_change); + //const bool use_throttle = !(_param_rc_override.get() & OverrideBits::OVERRIDE_IGNORE_THROTTLE_BIT); + //_selector.setpoint().user_override = rpy_moved || (use_throttle && throttle_moved); - manual_control_setpoint.timestamp = hrt_absolute_time(); - _manual_control_setpoint_pub.publish(manual_control_setpoint); - publish_once = true; - selected_manual_input = i; - } + _selector.setpoint().timestamp = now; + _manual_control_setpoint_pub.publish(_selector.setpoint()); - _manual_control_input[i] = manual_control_input; - _available[i] = available; - } + } else { + _published_invalid_once = true; + _manual_control_setpoint_pub.publish(_selector.setpoint()); } - if ((selected_manual_input >= 0) && (selected_manual_input != _selected_manual_input)) { - if (_selected_manual_input >= 0) { - PX4_INFO("selected manual_control_input changed %d -> %d", _selected_manual_input, selected_manual_input); - } + // FIXME: get from selector + //if ((selected_manual_input >= 0) && (selected_manual_input != _selected_manual_input)) { + // if (_selected_manual_input >= 0) { + // PX4_INFO("selected manual_control_input changed %d -> %d", _selected_manual_input, selected_manual_input); + // } - _manual_control_input_subs[selected_manual_input].registerCallback(); - _selected_manual_input = selected_manual_input; - } + // _manual_control_input_subs[selected_manual_input].registerCallback(); + // _selected_manual_input = selected_manual_input; + //} // reschedule timeout ScheduleDelayed(200_ms); diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp index 5f9a806887..529dcca0fb 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -46,6 +46,7 @@ #include #include #include +#include "ManualControlSelector.hpp" using namespace time_literals; @@ -92,15 +93,12 @@ private: {this, ORB_ID(manual_control_input), 2}, }; - manual_control_input_s _manual_control_input[MAX_MANUAL_INPUT_COUNT] {}; - - bool _available[MAX_MANUAL_INPUT_COUNT] {}; - - int8_t _selected_manual_input{-1}; - systemlib::Hysteresis _stick_arm_hysteresis{false}; systemlib::Hysteresis _stick_disarm_hysteresis{false}; + ManualControlSelector _selector; + bool _published_invalid_once{false}; + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; @@ -112,4 +110,4 @@ private: (ParamInt) _param_rc_arm_hyst ) }; -}; // namespace manual_control +} // namespace manual_control diff --git a/src/modules/manual_control/ManualControlSelector.cpp b/src/modules/manual_control/ManualControlSelector.cpp new file mode 100644 index 0000000000..b65470aed9 --- /dev/null +++ b/src/modules/manual_control/ManualControlSelector.cpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "ManualControlSelector.hpp" + + +namespace manual_control +{ + +void ManualControlSelector::update_time_only(uint64_t now) +{ + if (_setpoint.timestamp_sample + _timeout < now) { + _setpoint.valid = false; + } +} + +void ManualControlSelector::update_manual_control_input(uint64_t now, const manual_control_input_s &input) +{ + // This method requires the current timestamp explicitly in order to prevent weird cases + // if the timestamp_sample of some source is invalid/wrong. + + // 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; + + } 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.data_source == input.data_source || !_setpoint.valid) { + _setpoint = setpoint_from_input(input); + _setpoint.valid = true; + } + } else if (_rc_in_mode == 2) { + // FIXME: what to do in the legacy case? + } else if (_rc_in_mode == 3) { + + // We only stick to the first discovered mavlink channel. + if (_setpoint.data_source == input.data_source || !_setpoint.valid) { + _setpoint = setpoint_from_input(input); + _setpoint.valid = true; + } + } else { + // FIXME: param value unknown, what to do? + } +} + +manual_control_setpoint_s ManualControlSelector::setpoint_from_input(const manual_control_input_s &input) +{ + manual_control_setpoint_s setpoint; + setpoint.timestamp_sample = input.timestamp_sample; + setpoint.x = input.x; + setpoint.y = input.y; + setpoint.z = input.z; + setpoint.r = input.r; + // FIXME: what's that? + //setpoint.vx = (input.x - _manual_control_input[i].x) * dt_inv; + //setpoint.vy = (input.y - _manual_control_input[i].y) * dt_inv; + //setpoint.vz = (input.z - _manual_control_input[i].z) * dt_inv; + //setpoint.vr = (input.r - _manual_control_input[i].r) * dt_inv; + setpoint.flaps = input.flaps; + setpoint.aux1 = input.aux1; + setpoint.aux2 = input.aux2; + setpoint.aux3 = input.aux3; + setpoint.aux4 = input.aux4; + setpoint.aux5 = input.aux5; + setpoint.aux6 = input.aux6; + setpoint.data_source = input.data_source; + + return setpoint; +} + +manual_control_setpoint_s &ManualControlSelector::setpoint() +{ + return _setpoint; +} + +} // namespace manual_control diff --git a/src/modules/manual_control/ManualControlSelector.hpp b/src/modules/manual_control/ManualControlSelector.hpp new file mode 100644 index 0000000000..a2e255beec --- /dev/null +++ b/src/modules/manual_control/ManualControlSelector.hpp @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include + +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); + manual_control_setpoint_s &setpoint(); + +private: + 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}; +}; + +} // namespace manual_control diff --git a/src/modules/manual_control/ManualControlSelectorTest.cpp b/src/modules/manual_control/ManualControlSelectorTest.cpp new file mode 100644 index 0000000000..96d6e2e879 --- /dev/null +++ b/src/modules/manual_control/ManualControlSelectorTest.cpp @@ -0,0 +1,167 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "ManualControlSelector.hpp" +#include +#include + +using namespace time_literals; +using namespace manual_control; + +static constexpr uint64_t some_time = 1234568; + + +TEST(ManualControlSelector, RcInputOnly) +{ + ManualControlSelector selector; + selector.set_rc_in_mode(0); + selector.set_timeout(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); + + EXPECT_FALSE(selector.setpoint().valid); + + timestamp += 100_ms; + + // 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); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC); +} + +TEST(ManualControlSelector, MavlinkInputOnly) +{ + ManualControlSelector selector; + selector.set_rc_in_mode(1); + selector.set_timeout(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); + + EXPECT_FALSE(selector.setpoint().valid); + + timestamp += 100_ms; + + // 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); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_3); + + // But only the first MAVLink source wins, others are too late. + + timestamp += 100_ms; + + // Now provide input with the correct source. + input.data_source = manual_control_input_s::SOURCE_MAVLINK_4; + input.timestamp_sample = timestamp; + selector.update_manual_control_input(timestamp, input); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_3); +} + +TEST(ManualControlSelector, AutoInput) +{ + ManualControlSelector selector; + selector.set_rc_in_mode(3); + selector.set_timeout(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); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC); + + timestamp += 100_ms; + + // 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); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC); + + timestamp += 500_ms; + + // 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); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_0); +} + +TEST(ManualControlSelector, RcTimeout) +{ + ManualControlSelector selector; + selector.set_rc_in_mode(0); + selector.set_timeout(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); + + EXPECT_TRUE(selector.setpoint().valid); + EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC); + + timestamp += 600_ms; + + // Update, to make sure it notices the timeout + selector.update_time_only(timestamp); + + EXPECT_FALSE(selector.setpoint().valid); +}