fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ManualControl: add option to prioritize RC or MAVLink but with fallback

Browse Source
This commit is contained in:
Matthias Grob 2025-07-29 14:22:57 +02:00
parent b876aa5b45
commit a09352c079
4 changed files with 202 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/commander/commander_params.c
View File

@ -173,6 +173,8 @@ PARAM_DEFINE_INT32(COM_HOME_IN_AIR, 0);
* A value of 2 allows either RC Transmitter or Joystick input. The first valid input is used, will fallback to other sources if the input stream becomes invalid.
* A value of 3 allows either input from RC or joystick. The first available source is selected and used until reboot.
* A value of 4 ignores any stick input.
* A value of 5 allows either RC Transmitter or Joystick input. But RC has priority and whenever avaiable is immedietely used.
* A value of 6 allows either RC Transmitter or Joystick input. But Joystick has priority and whenever avaiable is immedietely used.
*
* @group Commander
* @min 0
@ -182,6 +184,8 @@ PARAM_DEFINE_INT32(COM_HOME_IN_AIR, 0);
* @value 2 RC and Joystick with fallback
* @value 3 RC or Joystick keep first
* @value 4 Stick input disabled
* @value 5 RC priority, Joystick fallback
* @value 6 Joystick priority, RC fallback
*/
PARAM_DEFINE_INT32(COM_RC_IN_MODE, 3);

36
src/modules/manual_control/ManualControlSelector.cpp
View File

@ -43,6 +43,10 @@ void ManualControlSelector::updateValidityOfChosenInput(uint64_t now)
void ManualControlSelector::updateWithNewInputSample(uint64_t now, const manual_control_setpoint_s &input, int instance)
{
if (isRc(input.data_source)) { _timestamp_last_rc = input.timestamp_sample; }
if (isMavlink(input.data_source)) { _timestamp_last_mavlink = input.timestamp_sample; }
// First check if the chosen input got invalid, so it can get replaced
updateValidityOfChosenInput(now);
@ -70,24 +74,38 @@ bool ManualControlSelector::isInputValid(const manual_control_setpoint_s &input,
const bool sample_newer_than_timeout = now < input.timestamp_sample + _timeout;
// Check if source matches the configuration
const bool source_rc_matched = (_rc_in_mode == 0) && (input.data_source == manual_control_setpoint_s::SOURCE_RC);
const bool source_mavlink_matched = (_rc_in_mode == 1) &&
(input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_0
|| input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_1
|| input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_2
|| input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_3
|| input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_4
|| input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_5);
const bool source_rc_matched = (_rc_in_mode == 0) && isRc(input.data_source);
const bool source_mavlink_matched = (_rc_in_mode == 1) && isMavlink(input.data_source);
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);
const bool source_rc_priority = (_rc_in_mode == 5)
&& (isRc(input.data_source) || (now > _timestamp_last_rc + _timeout));
const bool source_mavlink_priority = (_rc_in_mode == 6)
&& (isMavlink(input.data_source) || (now > _timestamp_last_mavlink + _timeout));
return sample_from_the_past && sample_newer_than_timeout && input.valid
&& (source_rc_matched || source_mavlink_matched || source_any_matched || source_first_matched);
&& (source_rc_matched || source_mavlink_matched || source_any_matched || source_first_matched || source_rc_priority
|| source_mavlink_priority);
}
manual_control_setpoint_s &ManualControlSelector::setpoint()
{
return _setpoint;
}
bool ManualControlSelector::isRc(uint8_t source)
{
return source == manual_control_setpoint_s::SOURCE_RC;
}
bool ManualControlSelector::isMavlink(uint8_t source)
{
return (source == manual_control_setpoint_s::SOURCE_MAVLINK_0
|| source == manual_control_setpoint_s::SOURCE_MAVLINK_1
|| source == manual_control_setpoint_s::SOURCE_MAVLINK_2
|| source == manual_control_setpoint_s::SOURCE_MAVLINK_3
|| source == manual_control_setpoint_s::SOURCE_MAVLINK_4
|| source == manual_control_setpoint_s::SOURCE_MAVLINK_5);
}

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

@ -48,10 +48,14 @@ public:
private:
bool isInputValid(const manual_control_setpoint_s &input, uint64_t now) const;
static bool isRc(uint8_t source);
static bool isMavlink(uint8_t source);
manual_control_setpoint_s _setpoint{};
uint64_t _timeout{0};
int32_t _rc_in_mode{0};
int _instance{-1};
uint8_t _first_valid_source{manual_control_setpoint_s::SOURCE_UNKNOWN};
uint64_t _timestamp_last_rc{0};
uint64_t _timestamp_last_mavlink{0};
};

169
src/modules/manual_control/ManualControlSelectorTest.cpp
View File

@ -40,6 +40,8 @@ using namespace time_literals;
static constexpr uint64_t SOME_TIME = 12345678;
static constexpr uint8_t SOURCE_RC = manual_control_setpoint_s::SOURCE_RC;
static constexpr uint8_t SOURCE_MAVLINK_0 = manual_control_setpoint_s::SOURCE_MAVLINK_0;
static constexpr uint8_t SOURCE_MAVLINK_1 = manual_control_setpoint_s::SOURCE_MAVLINK_1;
static constexpr uint8_t SOURCE_MAVLINK_2 = manual_control_setpoint_s::SOURCE_MAVLINK_2;
static constexpr uint8_t SOURCE_MAVLINK_3 = manual_control_setpoint_s::SOURCE_MAVLINK_3;
static constexpr uint8_t SOURCE_MAVLINK_4 = manual_control_setpoint_s::SOURCE_MAVLINK_4;
@ -175,6 +177,7 @@ TEST(ManualControlSelector, RcMavlinkInputFallback)
uint64_t timestamp = SOME_TIME;
// Valid RC input gets used
manual_control_setpoint_s input {};
input.data_source = SOURCE_RC;
input.valid = true;
@ -187,7 +190,7 @@ TEST(ManualControlSelector, RcMavlinkInputFallback)
timestamp += 100_ms;
// Now provide input from MAVLink as well which should get ignored.
// Now provide input from MAVLink as well which should get ignored
input.data_source = SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
@ -198,7 +201,7 @@ TEST(ManualControlSelector, RcMavlinkInputFallback)
timestamp += 500_ms;
// Now we'll let RC time out, so it should switch to MAVLINK.
// Now we update MAVLink and let RC time out, so it should switch to RC
input.data_source = SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
@ -206,6 +209,15 @@ TEST(ManualControlSelector, RcMavlinkInputFallback)
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0);
EXPECT_EQ(selector.instance(), 1);
// If we get RC back immediately, it stays with MAVLink (until RC is lost again)
input.data_source = SOURCE_RC;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0);
EXPECT_EQ(selector.instance(), 1);
}
TEST(ManualControlSelector, RcMavlinkInputKeepFirst)
@ -339,3 +351,156 @@ TEST(ManualControlSelector, RcOutdated)
EXPECT_FALSE(selector.setpoint().valid);
EXPECT_EQ(selector.instance(), -1);
}
TEST(ManualControlSelector, RcMavlinkInputRcPriority)
{
ManualControlSelector selector;
selector.setRcInMode(5); // Configure RC priority
selector.setTimeout(500_ms);
uint64_t timestamp = SOME_TIME;
// Valid RC input gets used
manual_control_setpoint_s input{};
input.data_source = SOURCE_RC;
input.valid = true;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC);
EXPECT_EQ(selector.instance(), 0);
timestamp += 100_ms;
// Now provide input from MAVLink as well which should get ignored
input.data_source = SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC);
EXPECT_EQ(selector.instance(), 0);
timestamp += 500_ms;
// Now we update MAVLink and let RC time out, so it should switch to RC
input.data_source = SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0);
EXPECT_EQ(selector.instance(), 1);
// If we get RC back immediately, it should use it with priority
input.data_source = SOURCE_RC;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC);
EXPECT_EQ(selector.instance(), 0);
}
TEST(ManualControlSelector, RcMavlinkInputMavlinkPriority)
{
ManualControlSelector selector;
selector.setRcInMode(6); // Configure MAVLink priority
selector.setTimeout(500_ms);
uint64_t timestamp = SOME_TIME;
// Valid MAVLink input gets used
manual_control_setpoint_s input{};
input.data_source = SOURCE_MAVLINK_0;
input.valid = true;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0);
EXPECT_EQ(selector.instance(), 0);
timestamp += 100_ms;
// Now provide input from RC as well which should get ignored
input.data_source = SOURCE_RC;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0);
EXPECT_EQ(selector.instance(), 0);
timestamp += 500_ms;
// Now we update RC and let MAVLink time out, so it should switch to RC
input.data_source = SOURCE_RC;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC);
EXPECT_EQ(selector.instance(), 1);
// If we get MAVLink back immediately, it should use it with priority
input.data_source = SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0);
EXPECT_EQ(selector.instance(), 0);
}
TEST(ManualControlSelector, MavlinkTwoInstanceInputMavlinkPriority)
{
ManualControlSelector selector;
selector.setRcInMode(6); // Configure MAVLink priority
selector.setTimeout(500_ms);
uint64_t timestamp = SOME_TIME;
// Valid MAVLink 0 input gets used
manual_control_setpoint_s input{};
input.data_source = SOURCE_MAVLINK_0;
input.valid = true;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0);
EXPECT_EQ(selector.instance(), 0);
timestamp += 100_ms;
// Now provide input from MAVLink 1 as well which should get ignored
input.data_source = SOURCE_MAVLINK_1;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0);
EXPECT_EQ(selector.instance(), 0);
timestamp += 500_ms;
// Now we update MAVLink 1 and let MAVLink 0 time out, so it should switch to MAVLink 1
input.data_source = SOURCE_MAVLINK_1;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1);
EXPECT_EQ(selector.instance(), 1);
// If we get MAVLink 0 back immediately, it should not switch since MAVLink 1 has the same priority
input.data_source = SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1);
EXPECT_EQ(selector.instance(), 1);
}