Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-29 03:36:07 +08:00
Code Issues Actions 10 Packages Projects Releases Wiki Activity

ManualControlSelector: refactor selection to chose based on a single condition

Browse Source
This commit is contained in:
Matthias Grob
2021-10-06 18:40:32 +02:00
parent 1bd786c71a
commit 396bab52ad
3 changed files with 18 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/manual_control/ManualControlSelector.cpp
+13 -22
View File
@@ -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. // If previous setpoint is timed out, set it invalid, so it can get replaced below.
update_time_only(now); update_time_only(now);
if (_rc_in_mode == 0 && input.data_source == manual_control_input_s::SOURCE_RC) { const bool source_rc_matched = (_rc_in_mode == 0) && (input.data_source == manual_control_input_s::SOURCE_RC);
_setpoint = setpoint_from_input(input); const bool source_mavlink_matched = (_rc_in_mode == 1) &&
_setpoint.valid = true; (input.data_source == manual_control_input_s::SOURCE_MAVLINK_0
_instance = instance; || 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 if (source_rc_matched || source_mavlink_matched || source_any_matched) {
|| input.data_source == manual_control_input_s::SOURCE_MAVLINK_1 if (not_overriding_existing_source) {
|| 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) {
_setpoint = setpoint_from_input(input); _setpoint = setpoint_from_input(input);
_setpoint.valid = true; _setpoint.valid = true;
_instance = instance; _instance = instance;
src/modules/manual_control/ManualControlSelector.hpp
+4 -4
View File
@@ -53,10 +53,10 @@ public:
private: private:
static manual_control_setpoint_s setpoint_from_input(const manual_control_input_s &input); static manual_control_setpoint_s setpoint_from_input(const manual_control_input_s &input);
manual_control_setpoint_s _setpoint {}; manual_control_setpoint_s _setpoint{};
uint64_t _timeout {0}; uint64_t _timeout{0};
int32_t _rc_in_mode {0}; int32_t _rc_in_mode{0};
int _instance {-1}; int _instance{-1};
}; };
} // namespace manual_control } // namespace manual_control
src/modules/manual_control/ManualControlSelectorTest.cpp
+1 -3
View File
@@ -93,11 +93,9 @@ TEST(ManualControlSelector, MavlinkInputOnly)
EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_MAVLINK_3); EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_MAVLINK_3);
EXPECT_EQ(selector.instance(), 1); EXPECT_EQ(selector.instance(), 1);
// But only the first MAVLink source wins, others are too late.
timestamp += 100_ms; 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.data_source = manual_control_input_s::SOURCE_MAVLINK_4;
input.timestamp_sample = timestamp; input.timestamp_sample = timestamp;
selector.update_manual_control_input(timestamp, input, 1); selector.update_manual_control_input(timestamp, input, 1);