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

feat: extend COM_RC_IN_MODE 5 and 6 to source ID ascending and descending priority modes

Browse Source
This commit is contained in:
Tobias Büchli
2025-09-23 23:18:50 +02:00
parent 6c69d86d7e
commit 25c66a7ee5
6 changed files with 114 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/manual_control/ManualControlSelectorTest.cpp
+79 -27
View File
@@ -352,15 +352,15 @@ TEST(ManualControlSelector, RcOutdated)
EXPECT_EQ(selector.instance(), -1);
}
TEST(ManualControlSelector, RcMavlinkInputRcPriority)
TEST(ManualControlSelector, AscendingSourcePriority)
{
ManualControlSelector selector;
selector.setRcInMode(5); // Configure RC priority
selector.setRcInMode(5); // Configure ascending source ID priority (lower number wins)
selector.setTimeout(500_ms);
uint64_t timestamp = SOME_TIME;
// Valid RC input gets used
// Valid RC input (ID 1) gets used
manual_control_setpoint_s input{};
input.data_source = SOURCE_RC;
input.valid = true;
@@ -373,7 +373,7 @@ TEST(ManualControlSelector, RcMavlinkInputRcPriority)
timestamp += 100_ms;
// Now provide input from MAVLink as well which should get ignored
// Now provide input from MAVLink (ID 2) as well which should get ignored (1 < 2)
input.data_source = SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
@@ -384,7 +384,7 @@ TEST(ManualControlSelector, RcMavlinkInputRcPriority)
timestamp += 500_ms;
// Now we update MAVLink and let RC time out, so it should switch to RC
// Now we update MAVLink and let RC time out, so it should switch to MAVLink
input.data_source = SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
@@ -393,7 +393,7 @@ TEST(ManualControlSelector, RcMavlinkInputRcPriority)
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
// If we get RC back immediately, it should take over with priority (1 < 2)
input.data_source = SOURCE_RC;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0);
@@ -401,17 +401,69 @@ TEST(ManualControlSelector, RcMavlinkInputRcPriority)
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC);
EXPECT_EQ(selector.instance(), 0);
}
TEST(ManualControlSelector, RcMavlinkInputMavlinkPriority)
TEST(ManualControlSelector, MavlinkTwoInstanceAscendingPriority)
{
ManualControlSelector selector;
selector.setRcInMode(6); // Configure MAVLink priority
selector.setRcInMode(5); // Configure ascending source ID priority (lower number wins)
selector.setTimeout(500_ms);
uint64_t timestamp = SOME_TIME;
// Valid MAVLink input gets used
// Valid MAVLink 1 input (ID 3) gets used
manual_control_setpoint_s input{};
input.data_source = SOURCE_MAVLINK_1;
input.valid = true;
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);
timestamp += 100_ms;
// Now provide input from MAVLink 0 (ID 2), which has higher priority (2 < 3) and should take over
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);
// If we get MAVLink 1 back immediately, it should be ignored since MAVLink 0 has higher priority (2 < 3)
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);
}
TEST(ManualControlSelector, DescendingSourcePriority)
{
ManualControlSelector selector;
selector.setRcInMode(6); // Configure descending source ID priority (higher number wins)
selector.setTimeout(500_ms);
uint64_t timestamp = SOME_TIME;
// Valid MAVLink input (ID 2) gets used
manual_control_setpoint_s input{};
input.data_source = SOURCE_MAVLINK_0;
input.valid = true;
@@ -424,7 +476,7 @@ TEST(ManualControlSelector, RcMavlinkInputMavlinkPriority)
timestamp += 100_ms;
// Now provide input from RC as well which should get ignored
// Now provide input from RC (ID 1) as well which should get ignored (2 > 1)
input.data_source = SOURCE_RC;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
@@ -444,7 +496,7 @@ TEST(ManualControlSelector, RcMavlinkInputMavlinkPriority)
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
// If we get MAVLink back immediately, it should take over with priority (2 > 1)
input.data_source = SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0);
@@ -454,15 +506,15 @@ TEST(ManualControlSelector, RcMavlinkInputMavlinkPriority)
EXPECT_EQ(selector.instance(), 0);
}
TEST(ManualControlSelector, MavlinkTwoInstanceInputMavlinkPriority)
TEST(ManualControlSelector, MavlinkTwoInstanceDescendingPriority)
{
ManualControlSelector selector;
selector.setRcInMode(6); // Configure MAVLink priority
selector.setRcInMode(6); // Configure descending source ID priority (higher number wins)
selector.setTimeout(500_ms);
uint64_t timestamp = SOME_TIME;
// Valid MAVLink 0 input gets used
// Valid MAVLink 0 input (ID 2) gets used
manual_control_setpoint_s input{};
input.data_source = SOURCE_MAVLINK_0;
input.valid = true;
@@ -475,18 +527,7 @@ TEST(ManualControlSelector, MavlinkTwoInstanceInputMavlinkPriority)
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
// Now provide input from MAVLink 1 (ID 3), which has higher priority (3 > 2) and should take over
input.data_source = SOURCE_MAVLINK_1;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
@@ -495,7 +536,7 @@ TEST(ManualControlSelector, MavlinkTwoInstanceInputMavlinkPriority)
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
// If we get MAVLink 0 back immediately, it should be ignored since MAVLink 1 has higher priority (3 > 2)
input.data_source = SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0);
@@ -503,4 +544,15 @@ TEST(ManualControlSelector, MavlinkTwoInstanceInputMavlinkPriority)
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1);
EXPECT_EQ(selector.instance(), 1);
timestamp += 500_ms;
// Now we update MAVLink 0 and let MAVLink 1 time out, so it should switch to MAVLink 0
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);
}