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

ManualControlSelectorTest: adapt to using input validity flag

Browse Source
This commit is contained in:
Matthias Grob
2023-05-08 11:31:41 +02:00
parent 7b6f45079b
commit c903288f4c
1 changed files with 89 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/manual_control/ManualControlSelectorTest.cpp
+89 -45
View File
@@ -37,7 +37,43 @@
using namespace time_literals;
static constexpr uint64_t some_time = 12345678;
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_3 = manual_control_setpoint_s::SOURCE_MAVLINK_3;
static constexpr uint8_t SOURCE_MAVLINK_4 = manual_control_setpoint_s::SOURCE_MAVLINK_4;
TEST(ManualControlSelector, RcInputInvalidValid)
{
ManualControlSelector selector;
selector.setRcInMode(0);
selector.setTimeout(500_ms);
uint64_t timestamp = SOME_TIME;
// Now provide input with the correct source flagged invalid
manual_control_setpoint_s input {};
input.data_source = SOURCE_RC;
input.valid = false;
input.timestamp_sample = timestamp;
for (int i = 0; i < 2; i++) {
selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_FALSE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().timestamp_sample, 0);
EXPECT_EQ(selector.instance(), -1);
EXPECT_EQ(selector.setpoint().data_source, 0);
timestamp += 100_ms;
input.timestamp_sample = timestamp;
}
input.valid = true;
selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().timestamp_sample, timestamp);
EXPECT_EQ(selector.instance(), 1);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC);
}
TEST(ManualControlSelector, RcInputContinuous)
{
@@ -45,11 +81,12 @@ TEST(ManualControlSelector, RcInputContinuous)
selector.setRcInMode(0);
selector.setTimeout(500_ms);
uint64_t timestamp = some_time;
uint64_t timestamp = SOME_TIME;
// Now provide input with the correct source.
manual_control_setpoint_s input {};
input.data_source = manual_control_setpoint_s::SOURCE_RC;
input.data_source = SOURCE_RC;
input.valid = true;
input.timestamp_sample = timestamp;
for (int i = 0; i < 5; i++) {
@@ -57,7 +94,7 @@ TEST(ManualControlSelector, RcInputContinuous)
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().timestamp_sample, timestamp);
EXPECT_EQ(selector.instance(), 1);
EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC);
timestamp += 100_ms;
input.timestamp_sample = timestamp;
}
@@ -66,13 +103,14 @@ TEST(ManualControlSelector, RcInputContinuous)
TEST(ManualControlSelector, RcInputOnly)
{
ManualControlSelector selector;
selector.setRcInMode(0);
selector.setRcInMode(0); // Configure RC input only
selector.setTimeout(500_ms);
uint64_t timestamp = some_time;
uint64_t timestamp = SOME_TIME;
manual_control_setpoint_s input {};
input.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0;
input.data_source = SOURCE_MAVLINK_0;
input.valid = true;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0);
@@ -81,25 +119,26 @@ TEST(ManualControlSelector, RcInputOnly)
timestamp += 100_ms;
// Now provide input with the correct source.
input.data_source = manual_control_setpoint_s::SOURCE_RC;
input.data_source = SOURCE_RC;
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.setpoint().data_source, SOURCE_RC);
EXPECT_EQ(selector.instance(), 1);
}
TEST(ManualControlSelector, MavlinkInputOnly)
{
ManualControlSelector selector;
selector.setRcInMode(1);
selector.setRcInMode(1); // Configure MAVLink input only
selector.setTimeout(500_ms);
uint64_t timestamp = some_time;
uint64_t timestamp = SOME_TIME;
manual_control_setpoint_s input {};
input.data_source = manual_control_setpoint_s::SOURCE_RC;
input.data_source = SOURCE_RC;
input.valid = true;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0);
@@ -108,128 +147,131 @@ TEST(ManualControlSelector, MavlinkInputOnly)
timestamp += 100_ms;
// Now provide input with the correct source.
input.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_3;
input.data_source = SOURCE_MAVLINK_3;
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_MAVLINK_3);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_3);
EXPECT_EQ(selector.instance(), 1);
timestamp += 100_ms;
// But only the first MAVLink source wins, others are too late.
input.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_4;
input.data_source = SOURCE_MAVLINK_4;
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_MAVLINK_3);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_3);
EXPECT_EQ(selector.instance(), 1);
}
TEST(ManualControlSelector, AutoInput)
TEST(ManualControlSelector, RcMavlinkInputFallback)
{
ManualControlSelector selector;
selector.setRcInMode(2);
selector.setRcInMode(2); // Configure fallback
selector.setTimeout(500_ms);
uint64_t timestamp = some_time;
uint64_t timestamp = SOME_TIME;
manual_control_setpoint_s input {};
input.data_source = manual_control_setpoint_s::SOURCE_RC;
input.data_source = SOURCE_RC;
input.valid = true;
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.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 = manual_control_setpoint_s::SOURCE_MAVLINK_0;
input.data_source = 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.setpoint().data_source, SOURCE_RC);
EXPECT_EQ(selector.instance(), 0);
timestamp += 500_ms;
// Now we'll let RC time out, so it should switch to MAVLINK.
input.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0;
input.data_source = 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_MAVLINK_0);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0);
EXPECT_EQ(selector.instance(), 1);
}
TEST(ManualControlSelector, FirstInput)
TEST(ManualControlSelector, RcMavlinkInputKeepFirst)
{
ManualControlSelector selector;
selector.setRcInMode(3);
selector.setRcInMode(3); // Configure keep first input
selector.setTimeout(500_ms);
uint64_t timestamp = some_time;
uint64_t timestamp = SOME_TIME;
manual_control_setpoint_s input {};
input.data_source = manual_control_setpoint_s::SOURCE_RC;
input.data_source = SOURCE_RC;
input.valid = true;
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.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 = manual_control_setpoint_s::SOURCE_MAVLINK_0;
input.data_source = 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.setpoint().data_source, 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.data_source = 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_NE(selector.setpoint().data_source, 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.data_source = 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.setpoint().data_source, SOURCE_RC);
EXPECT_EQ(selector.instance(), 0);
}
TEST(ManualControlSelector, DisabledInput)
{
ManualControlSelector selector;
selector.setRcInMode(4);
selector.setRcInMode(4); // Configure disabled stick input
selector.setTimeout(500_ms);
uint64_t timestamp = some_time;
uint64_t timestamp = SOME_TIME;
manual_control_setpoint_s input {};
// Reject MAVLink stick input
input.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0;
input.data_source = SOURCE_MAVLINK_0;
input.valid = true;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0);
@@ -239,7 +281,7 @@ TEST(ManualControlSelector, DisabledInput)
timestamp += 100_ms;
// Reject RC stick input
input.data_source = manual_control_setpoint_s::SOURCE_RC;
input.data_source = SOURCE_RC;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
@@ -253,15 +295,16 @@ TEST(ManualControlSelector, RcTimeout)
selector.setRcInMode(0);
selector.setTimeout(500_ms);
uint64_t timestamp = some_time;
uint64_t timestamp = SOME_TIME;
manual_control_setpoint_s input {};
input.data_source = manual_control_setpoint_s::SOURCE_RC;
input.data_source = SOURCE_RC;
input.valid = true;
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.setpoint().data_source, SOURCE_RC);
EXPECT_EQ(selector.instance(), 0);
timestamp += 600_ms;
@@ -279,10 +322,11 @@ TEST(ManualControlSelector, RcOutdated)
selector.setRcInMode(0);
selector.setTimeout(500_ms);
uint64_t timestamp = some_time;
uint64_t timestamp = SOME_TIME;
manual_control_setpoint_s input {};
input.data_source = manual_control_setpoint_s::SOURCE_RC;
input.data_source = SOURCE_RC;
input.valid = true;
input.timestamp_sample = timestamp - 600_ms; // First sample is already outdated
selector.updateWithNewInputSample(timestamp, input, 0);