feat: expand to 4 modes with more priority options

This commit is contained in:
Tobias Büchli
2025-10-01 18:31:12 +02:00
parent 25c66a7ee5
commit 2c062a45ba
6 changed files with 375 additions and 98 deletions
@@ -48,8 +48,10 @@ void ManualControlSelector::updateWithNewInputSample(uint64_t now, const manual_
const bool update_existing_input = _setpoint.valid && (input.data_source == _setpoint.data_source);
const bool start_using_new_input = !_setpoint.valid;
const bool is_priority_mode = (_rc_in_mode == RcInMode::AscendingPriority
|| _rc_in_mode == RcInMode::DescendingPriority);
const bool is_priority_mode = (_rc_in_mode == RcInMode::RcThenJoystickAscendingPriority
|| _rc_in_mode == RcInMode::JoystickAscendingThenRcPriority
|| _rc_in_mode == RcInMode::RcThenJoystickDescendingPriority
|| _rc_in_mode == RcInMode::JoystickDescendingThenRcPriority);
// Switch to new input if it's valid and we don't already have a valid one
if (isInputValid(input, now) && (update_existing_input || start_using_new_input || is_priority_mode)) {
@@ -91,12 +93,68 @@ bool ManualControlSelector::isInputValid(const manual_control_setpoint_s &input,
|| (_first_valid_source == manual_control_setpoint_s::SOURCE_UNKNOWN);
break;
case RcInMode::AscendingPriority:
match = !_setpoint.valid || (input.data_source <= _setpoint.data_source);
case RcInMode::RcThenJoystickAscendingPriority:
if (isRc(input.data_source)) {
match = true;
} else if (isMavlink(input.data_source)) {
if (!_setpoint.valid) {
match = true;
} else if (isMavlink(_setpoint.data_source)) {
match = input.data_source <= _setpoint.data_source;
}
}
break;
case RcInMode::DescendingPriority:
match = !_setpoint.valid || (input.data_source >= _setpoint.data_source);
case RcInMode::JoystickAscendingThenRcPriority:
if (isMavlink(input.data_source)) {
if (!_setpoint.valid || isRc(_setpoint.data_source)) {
match = true;
} else if (isMavlink(_setpoint.data_source)) {
match = input.data_source <= _setpoint.data_source;
}
} else if (isRc(input.data_source)) {
if (!_setpoint.valid || isRc(_setpoint.data_source)) {
match = true;
}
}
break;
case RcInMode::RcThenJoystickDescendingPriority:
if (isRc(input.data_source)) {
match = true;
} else if (isMavlink(input.data_source)) {
if (!_setpoint.valid) {
match = true;
} else if (isMavlink(_setpoint.data_source)) {
match = input.data_source >= _setpoint.data_source;
}
}
break;
case RcInMode::JoystickDescendingThenRcPriority:
if (isMavlink(input.data_source)) {
if (!_setpoint.valid || isRc(_setpoint.data_source)) {
match = true;
} else if (isMavlink(_setpoint.data_source)) {
match = input.data_source >= _setpoint.data_source;
}
} else if (isRc(input.data_source)) {
if (!_setpoint.valid || isRc(_setpoint.data_source)) {
match = true;
}
}
break;
case RcInMode::StickInputDisabled:
@@ -58,8 +58,10 @@ private:
RcAndJoystickWithFallback = 2, // RC And Joystick with fallback
RcOrJoystickKeepFirst = 3, // RC or Joystick keep first
StickInputDisabled = 4, // input disabled
AscendingPriority = 5, // ascending source priority
DescendingPriority = 6 // descending source priority
RcThenJoystickAscendingPriority = 5, // RC, then Joystick ascending prio
JoystickAscendingThenRcPriority = 6, // Joystick ascending prio, then RC
RcThenJoystickDescendingPriority = 7, // RC, then Joystick descending prio
JoystickDescendingThenRcPriority = 8 // Joystick descending prio, then RC
};
manual_control_setpoint_s _setpoint{};
@@ -352,15 +352,15 @@ TEST(ManualControlSelector, RcOutdated)
EXPECT_EQ(selector.instance(), -1);
}
TEST(ManualControlSelector, AscendingSourcePriority)
TEST(ManualControlSelector, RcThenJoystickAscendingPriority)
{
ManualControlSelector selector;
selector.setRcInMode(5); // Configure ascending source ID priority (lower number wins)
selector.setRcInMode(5); // Configure RC, then Joystick ascending
selector.setTimeout(500_ms);
uint64_t timestamp = SOME_TIME;
// Valid RC input (ID 1) gets used
// Valid RC input gets used
manual_control_setpoint_s input{};
input.data_source = SOURCE_RC;
input.valid = true;
@@ -373,7 +373,7 @@ TEST(ManualControlSelector, AscendingSourcePriority)
timestamp += 100_ms;
// Now provide input from MAVLink (ID 2) as well which should get ignored (1 < 2)
// Now provide input from MAVLink 0 as well which should get ignored
input.data_source = SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
@@ -384,7 +384,7 @@ TEST(ManualControlSelector, AscendingSourcePriority)
timestamp += 500_ms;
// Now we update MAVLink and let RC time out, so it should switch to MAVLink
// Now we update MAVLink 0 and let RC time out, so it should switch to MAVLink 0
input.data_source = SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
@@ -393,7 +393,7 @@ TEST(ManualControlSelector, AscendingSourcePriority)
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0);
EXPECT_EQ(selector.instance(), 1);
// If we get RC back immediately, it should take over with priority (1 < 2)
// If we get RC back immediately, it should take over with priority
input.data_source = SOURCE_RC;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0);
@@ -402,157 +402,368 @@ TEST(ManualControlSelector, AscendingSourcePriority)
EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC);
EXPECT_EQ(selector.instance(), 0);
}
timestamp += 100_ms;
TEST(ManualControlSelector, MavlinkTwoInstanceAscendingPriority)
{
ManualControlSelector selector;
selector.setRcInMode(5); // Configure ascending source ID priority (lower number wins)
selector.setTimeout(500_ms);
uint64_t timestamp = SOME_TIME;
// Valid MAVLink 1 input (ID 3) gets used
manual_control_setpoint_s input{};
// Now provide input from MAVLink 1, which should get ignored
input.data_source = SOURCE_MAVLINK_1;
input.valid = true;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
selector.updateWithNewInputSample(timestamp, input, 2);
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 1 and let RC time out, so it should switch to MAVLink 1
input.data_source = SOURCE_MAVLINK_1;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 2);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1);
EXPECT_EQ(selector.instance(), 1);
EXPECT_EQ(selector.instance(), 2);
timestamp += 100_ms;
// Now provide input from MAVLink 0 (ID 2), which has higher priority (2 < 3) and should take over
// Now provide input from MAVLink 0, which has higher priority 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);
EXPECT_EQ(selector.instance(), 1);
// If we get MAVLink 1 back immediately, it should be ignored since MAVLink 0 has higher priority
input.data_source = SOURCE_MAVLINK_1;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 2);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0);
EXPECT_EQ(selector.instance(), 1);
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);
selector.updateWithNewInputSample(timestamp, input, 2);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1);
EXPECT_EQ(selector.instance(), 1);
EXPECT_EQ(selector.instance(), 2);
}
TEST(ManualControlSelector, DescendingSourcePriority)
TEST(ManualControlSelector, JoystickAscendingThenRcPriority)
{
ManualControlSelector selector;
selector.setRcInMode(6); // Configure descending source ID priority (higher number wins)
selector.setRcInMode(6); // Configure Joystick ascending, then RC
selector.setTimeout(500_ms);
uint64_t timestamp = SOME_TIME;
// Valid MAVLink input (ID 2) gets used
// 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, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0);
EXPECT_EQ(selector.instance(), 1);
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, 2);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0);
EXPECT_EQ(selector.instance(), 1);
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, 2);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1);
EXPECT_EQ(selector.instance(), 2);
// If we get MAVLink 0 back immediately, it should take over with priority
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);
timestamp += 100_ms;
// Now provide input from RC, which should get ignored
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_MAVLINK_0);
EXPECT_EQ(selector.instance(), 1);
timestamp += 500_ms;
// Now we update RC and let MAVLink 0 time out, so it should switch to RC
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);
timestamp += 100_ms;
// Now provide input from RC (ID 1) as well which should get ignored (2 > 1)
input.data_source = SOURCE_RC;
// Now provide input from MAVLink 1, which has higher priority and should take over
input.data_source = SOURCE_MAVLINK_1;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1);
selector.updateWithNewInputSample(timestamp, input, 2);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1);
EXPECT_EQ(selector.instance(), 2);
// If we get RC back immediately, it should be ignored since MAVLink 1 has higher 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_MAVLINK_1);
EXPECT_EQ(selector.instance(), 2);
timestamp += 500_ms;
// Now we update RC and let MAVLink 1 time out, so it should switch to RC
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, RcThenJoystickDescendingPriority)
{
ManualControlSelector selector;
selector.setRcInMode(7); // Configure RC, then Joystick descending
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 1 as well which should get ignored
input.data_source = SOURCE_MAVLINK_1;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 2);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC);
EXPECT_EQ(selector.instance(), 0);
timestamp += 500_ms;
// Now we update RC and let MAVLink time out, so it should switch to RC
// Now we update MAVLink 0 and let RC time out, so it should switch to MAVLink 0
input.data_source = SOURCE_MAVLINK_1;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 2);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1);
EXPECT_EQ(selector.instance(), 2);
// If we get RC back immediately, it should take over 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);
timestamp += 100_ms;
// Now provide input from MAVLink 0, 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 0 and let RC time out, so it should switch to MAVLink 0
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 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);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0);
EXPECT_EQ(selector.instance(), 0);
}
TEST(ManualControlSelector, MavlinkTwoInstanceDescendingPriority)
{
ManualControlSelector selector;
selector.setRcInMode(6); // Configure descending source ID priority (higher number wins)
selector.setTimeout(500_ms);
uint64_t timestamp = SOME_TIME;
// Valid MAVLink 0 input (ID 2) 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 (ID 3), which has higher priority (3 > 2) and should take over
// Now provide input from MAVLink 1, which has higher priority and should take over
input.data_source = SOURCE_MAVLINK_1;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 2);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1);
EXPECT_EQ(selector.instance(), 2);
// If we get MAVLink 0 back immediately, it should be ignored since MAVLink 1 has higher priority
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_1);
EXPECT_EQ(selector.instance(), 1);
// 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);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1);
EXPECT_EQ(selector.instance(), 1);
EXPECT_EQ(selector.instance(), 2);
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, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0);
EXPECT_EQ(selector.instance(), 1);
}
TEST(ManualControlSelector, JoystickDescendingThenRcPriority)
{
ManualControlSelector selector;
selector.setRcInMode(8); // Configure Joystick descending, then RC
selector.setTimeout(500_ms);
uint64_t timestamp = SOME_TIME;
// Valid MAVLink 1 input gets used
manual_control_setpoint_s input{};
input.data_source = SOURCE_MAVLINK_1;
input.valid = true;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 2);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1);
EXPECT_EQ(selector.instance(), 2);
timestamp += 100_ms;
// Now provide input from MAVLink 0 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_MAVLINK_1);
EXPECT_EQ(selector.instance(), 2);
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, 1);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0);
EXPECT_EQ(selector.instance(), 1);
// If we get MAVLink 1 back immediately, it should take over with priority
input.data_source = SOURCE_MAVLINK_1;
input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 2);
EXPECT_TRUE(selector.setpoint().valid);
EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_1);
EXPECT_EQ(selector.instance(), 2);
timestamp += 100_ms;
// Now provide input from RC, which should get ignored
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_MAVLINK_1);
EXPECT_EQ(selector.instance(), 2);
timestamp += 500_ms;
// Now we update RC and let MAVLink 1 time out, so it should switch to RC
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);
timestamp += 100_ms;
// Now provide input from MAVLink 0, which has higher priority and should take over
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 be ignored since MAVLink 0 has higher 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_MAVLINK_0);
EXPECT_EQ(selector.instance(), 1);
timestamp += 500_ms;
// Now we update RC and let MAVLink 0 time out, so it should switch to RC
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);
}