From 8876af91506a7d47df7d90e811c837e8eb1dcd2d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 14 Apr 2021 11:22:29 +0200 Subject: [PATCH] manual_control: add instance to selector This instance is then used to schedule the callback for the subscription. --- src/modules/manual_control/ManualControl.cpp | 30 ++++++++++--------- src/modules/manual_control/ManualControl.hpp | 1 + .../manual_control/ManualControlSelector.cpp | 6 +++- .../manual_control/ManualControlSelector.hpp | 4 ++- .../ManualControlSelectorTest.cpp | 26 ++++++++++------ 5 files changed, 42 insertions(+), 25 deletions(-) diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 7791c83586..7b82ae5f2e 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -92,7 +92,7 @@ void ManualControl::Run() found_at_least_one = true; - _selector.update_manual_control_input(now, manual_control_input); + _selector.update_manual_control_input(now, manual_control_input, i); } } @@ -133,21 +133,23 @@ void ManualControl::Run() _selector.setpoint().timestamp = now; _manual_control_setpoint_pub.publish(_selector.setpoint()); + _manual_control_input_subs[_selector.instance()].registerCallback(); + + if (_last_selected_input != _selector.instance()) { + + PX4_INFO("selected manual_control_input changed %d -> %d", _last_selected_input, _selector.instance()); + _last_selected_input = _selector.instance(); + } + + } else { - _published_invalid_once = true; - _manual_control_setpoint_pub.publish(_selector.setpoint()); + _last_selected_input = -1; + if (!_published_invalid_once) { + _published_invalid_once = true; + _manual_control_setpoint_pub.publish(_selector.setpoint()); + } } - // 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; - //} - // reschedule timeout ScheduleDelayed(200_ms); @@ -198,7 +200,7 @@ int ManualControl::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description - +Module consuming manual_control_inputs publishing one manual_control_setpoint. )DESCR_STR"); diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp index 529dcca0fb..ba91fe2d7d 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -98,6 +98,7 @@ private: ManualControlSelector _selector; bool _published_invalid_once{false}; + int _last_selected_input{-1}; 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")}; diff --git a/src/modules/manual_control/ManualControlSelector.cpp b/src/modules/manual_control/ManualControlSelector.cpp index b65470aed9..97bf101c79 100644 --- a/src/modules/manual_control/ManualControlSelector.cpp +++ b/src/modules/manual_control/ManualControlSelector.cpp @@ -41,10 +41,11 @@ void ManualControlSelector::update_time_only(uint64_t now) { if (_setpoint.timestamp_sample + _timeout < now) { _setpoint.valid = false; + _instance = -1; } } -void ManualControlSelector::update_manual_control_input(uint64_t now, const manual_control_input_s &input) +void ManualControlSelector::update_manual_control_input(uint64_t now, const manual_control_input_s &input, int instance) { // This method requires the current timestamp explicitly in order to prevent weird cases // if the timestamp_sample of some source is invalid/wrong. @@ -55,6 +56,7 @@ void ManualControlSelector::update_manual_control_input(uint64_t now, const manu if (_rc_in_mode == 0 && input.data_source == manual_control_input_s::SOURCE_RC) { _setpoint = setpoint_from_input(input); _setpoint.valid = true; + _instance = instance; } 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 @@ -67,6 +69,7 @@ void ManualControlSelector::update_manual_control_input(uint64_t now, const manu if (_setpoint.data_source == input.data_source || !_setpoint.valid) { _setpoint = setpoint_from_input(input); _setpoint.valid = true; + _instance = instance; } } else if (_rc_in_mode == 2) { // FIXME: what to do in the legacy case? @@ -76,6 +79,7 @@ void ManualControlSelector::update_manual_control_input(uint64_t now, const manu if (_setpoint.data_source == input.data_source || !_setpoint.valid) { _setpoint = setpoint_from_input(input); _setpoint.valid = true; + _instance = instance; } } else { // FIXME: param value unknown, what to do? diff --git a/src/modules/manual_control/ManualControlSelector.hpp b/src/modules/manual_control/ManualControlSelector.hpp index a2e255beec..72a94568f9 100644 --- a/src/modules/manual_control/ManualControlSelector.hpp +++ b/src/modules/manual_control/ManualControlSelector.hpp @@ -45,8 +45,9 @@ 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); + void update_manual_control_input(uint64_t now, const manual_control_input_s &input, int instance); manual_control_setpoint_s &setpoint(); + int instance() const { return _instance; }; private: manual_control_setpoint_s setpoint_from_input(const manual_control_input_s &input); @@ -54,6 +55,7 @@ private: 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 96d6e2e879..0f090b4817 100644 --- a/src/modules/manual_control/ManualControlSelectorTest.cpp +++ b/src/modules/manual_control/ManualControlSelectorTest.cpp @@ -52,7 +52,7 @@ TEST(ManualControlSelector, RcInputOnly) 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); + selector.update_manual_control_input(timestamp, input, 0); EXPECT_FALSE(selector.setpoint().valid); @@ -61,10 +61,11 @@ 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); + selector.update_manual_control_input(timestamp, input, 1); EXPECT_TRUE(selector.setpoint().valid); EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC); + EXPECT_EQ(selector.instance(), 1); } TEST(ManualControlSelector, MavlinkInputOnly) @@ -78,7 +79,7 @@ TEST(ManualControlSelector, MavlinkInputOnly) 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); + selector.update_manual_control_input(timestamp, input, 0); EXPECT_FALSE(selector.setpoint().valid); @@ -87,10 +88,11 @@ 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); + selector.update_manual_control_input(timestamp, input, 1); EXPECT_TRUE(selector.setpoint().valid); EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_3); + EXPECT_EQ(selector.instance(), 1); // But only the first MAVLink source wins, others are too late. @@ -99,10 +101,11 @@ TEST(ManualControlSelector, MavlinkInputOnly) // 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); + selector.update_manual_control_input(timestamp, input, 1); EXPECT_TRUE(selector.setpoint().valid); EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_3); + EXPECT_EQ(selector.instance(), 1); } TEST(ManualControlSelector, AutoInput) @@ -116,30 +119,33 @@ TEST(ManualControlSelector, AutoInput) 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); + selector.update_manual_control_input(timestamp, input, 0); EXPECT_TRUE(selector.setpoint().valid); EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC); + EXPECT_EQ(selector.instance(), 0); 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); + selector.update_manual_control_input(timestamp, input, 1); EXPECT_TRUE(selector.setpoint().valid); EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC); + EXPECT_EQ(selector.instance(), 0); 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); + selector.update_manual_control_input(timestamp, input, 1); EXPECT_TRUE(selector.setpoint().valid); EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_0); + EXPECT_EQ(selector.instance(), 1); } TEST(ManualControlSelector, RcTimeout) @@ -153,10 +159,11 @@ TEST(ManualControlSelector, RcTimeout) 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); + selector.update_manual_control_input(timestamp, input, 0); EXPECT_TRUE(selector.setpoint().valid); EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC); + EXPECT_EQ(selector.instance(), 0); timestamp += 600_ms; @@ -164,4 +171,5 @@ TEST(ManualControlSelector, RcTimeout) selector.update_time_only(timestamp); EXPECT_FALSE(selector.setpoint().valid); + EXPECT_EQ(selector.instance(), -1); }