fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 17:58:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

ManualControlSelector: Enable original PX4 default behavior until QGC catches up

Browse Source
This commit is contained in:
Matthias Grob
2021-12-10 09:11:08 -05:00
committed by Daniel Agar
parent d961a067b9
commit 300e439144
4 changed files with 66 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander_params.c
+5 -4
View File
@@ -252,12 +252,13 @@ PARAM_DEFINE_INT32(COM_HOME_IN_AIR, 0);
*
* @group Commander
* @min 0
* @max 2
* @max 3
* @value 0 RC Transmitter only
* @value 1 Joystick only/No RC Checks
* @value 2 RC and Joystick are accepted
* @value 1 Joystick only
* @value 2 RC and Joystick with fallback
* @value 3 RC or Joystick keep first
*/
PARAM_DEFINE_INT32(COM_RC_IN_MODE, 0);
PARAM_DEFINE_INT32(COM_RC_IN_MODE, 3);
/**
* RC input arm/disarm command duration
src/modules/manual_control/ManualControlSelector.cpp
+9 -1
View File
@@ -54,6 +54,11 @@ void ManualControlSelector::updateWithNewInputSample(uint64_t now, const manual_
_setpoint = input;
_setpoint.valid = true;
_instance = instance;
if (_first_valid_source == manual_control_setpoint_s::SOURCE_UNKNOWN) {
// initialize first valid source once
_first_valid_source = _setpoint.data_source;
}
}
}
@@ -73,9 +78,12 @@ bool ManualControlSelector::isInputValid(const manual_control_setpoint_s &input,
|| input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_4
|| input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_5);
const bool source_any_matched = (_rc_in_mode == 2);
const bool source_first_matched = (_rc_in_mode == 3) &&
(input.data_source == _first_valid_source
|| _first_valid_source == manual_control_setpoint_s::SOURCE_UNKNOWN);
return sample_from_the_past && sample_newer_than_timeout
&& (source_rc_matched || source_mavlink_matched || source_any_matched);
&& (source_rc_matched || source_mavlink_matched || source_any_matched || source_first_matched);
}
manual_control_setpoint_s &ManualControlSelector::setpoint()
src/modules/manual_control/ManualControlSelector.hpp
+1
View File
@@ -53,4 +53,5 @@ private:
uint64_t _timeout{0};
int32_t _rc_in_mode{0};
int _instance{-1};
uint8_t _first_valid_source{manual_control_setpoint_s::SOURCE_UNKNOWN};
};
src/modules/manual_control/ManualControlSelectorTest.cpp
+51
View File
@@ -168,6 +168,57 @@ TEST(ManualControlSelector, AutoInput)
EXPECT_EQ(selector.instance(), 1);
}
TEST(ManualControlSelector, FirstInput)
{
ManualControlSelector selector;
selector.setRcInMode(3);
selector.setTimeout(500_ms);
uint64_t timestamp = some_time;
manual_control_setpoint_s input {};
input.data_source = manual_control_setpoint_s::SOURCE_RC;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(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_setpoint_s::SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(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, but it should NOT switch to MAVLINK because RC was first
input.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_FALSE(selector.setpoint().valid);
EXPECT_FALSE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_0);
EXPECT_EQ(selector.instance(), -1);
timestamp += 100_ms;
// Provide input from RC again and it should get accepted because it was the first.
input.data_source = manual_control_setpoint_s::SOURCE_RC;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(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);
}
TEST(ManualControlSelector, RcTimeout)
{
ManualControlSelector selector;