Skip to content

Commit

Permalink
ManualControlSelectorTest: adapt to using input validity flag
Browse files Browse the repository at this point in the history
  • Loading branch information
@MaEtUgR
MaEtUgR committed May 10, 2023
1 parent f4c814c commit 4b76c3d
Showing 1 changed file with 89 additions and 45 deletions.
134 changes: 89 additions & 45 deletions src/modules/manual_control/ManualControlSelectorTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,27 +37,64 @@

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)
{
ManualControlSelector selector;
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++) {
selector.updateWithNewInputSample(timestamp, input, 1);
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;
}
Expand All @@ -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);

Expand All @@ -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);

Expand All @@ -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);

Expand All @@ -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);

Expand All @@ -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;
Expand All @@ -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);

Expand Down

0 comments on commit 4b76c3d

Please sign in to comment.