feat: expand to 4 modes with more priority options

src/modules/commander/HealthAndArmingChecks/checks/rcCalibrationCheck.cpp

@@ -75,8 +75,8 @@ void RcCalibrationChecks::checkAndReport(const Context &context, Report &reporte
 		return;
 	}
 
-	if (_param_com_rc_in_mode.get() != 0 && _param_com_rc_in_mode.get() != 2 && _param_com_rc_in_mode.get() != 3
-	    && _param_com_rc_in_mode.get() != 5 && _param_com_rc_in_mode.get() != 6) {
+	// return early when joystick only or stick input disabled
+	if (_param_com_rc_in_mode.get() == 1 || _param_com_rc_in_mode.get() == 4) {
 		return;
 	}
 

src/modules/commander/commander_params.c

@@ -173,19 +173,23 @@ 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, prioritizing by ascending source ID, giving an RC transmitter priority over MAVLink joysticks, and lower MAVLink instance numbers priority over higher ones.
- * A value of 6 allows either RC Transmitter or Joystick input, prioritizing by descending source ID, giving MAVLink joysticks priority over an RC transmitter, and higher MAVLink instance numbers priority over lower ones.
+ * A value of 5 allows either RC Transmitter or Joystick input, giving an RC transmitter priority over MAVLink joysticks, and lower MAVLink instance numbers priority over higher ones.
+ * A value of 6 allows either RC Transmitter or Joystick input, giving MAVLink joysticks priority over an RC transmitter, and lower MAVLink instance numbers priority over higher ones.
+ * A value of 7 allows either RC Transmitter or Joystick input, giving an RC transmitter priority over MAVLink joysticks, and higher MAVLink instance numbers priority over lower ones.
+ * A value of 8 allows either RC Transmitter or Joystick input, giving MAVLink joysticks priority over an RC transmitter, and higher MAVLink instance numbers priority over lower ones.
  *
  * @group Commander
  * @min 0
- * @max 6
+ * @max 8
  * @value 0 RC Transmitter only
  * @value 1 Joystick only
  * @value 2 RC and Joystick with fallback
  * @value 3 RC or Joystick keep first
  * @value 4 Stick input disabled
- * @value 5 Ascending priority
- * @value 6 Descending priority
+ * @value 5 RC, then Joystick ascending prio
+ * @value 6 Joystick ascending prio, then RC
+ * @value 7 RC, then Joystick descending prio
+ * @value 8 Joystick descending prio, then RC
  */
 PARAM_DEFINE_INT32(COM_RC_IN_MODE, 3);
 

src/modules/commander/failsafe/failsafe.h

@@ -133,8 +133,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
 	};
 
 	enum class command_after_high_wind_failsafe : int32_t {

src/modules/manual_control/ManualControlSelector.cpp

@@ -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:

src/modules/manual_control/ManualControlSelector.hpp

@@ -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{};

src/modules/manual_control/ManualControlSelectorTest.cpp

@@ -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);
 }