dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ManualControlSelector: refactor selection to chose based on a single condition

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
src/modules/manual_control/ManualControlSelector.cpp
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.
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;

8
src/modules/manual_control/ManualControlSelector.hpp
View File

@ -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

4
src/modules/manual_control/ManualControlSelectorTest.cpp
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_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);