ManualControlSelector: Enable original PX4 default behavior until QGC catches up

src/modules/commander/commander_params.c

@@ -252,12 +252,13 @@ PARAM_DEFINE_INT32(COM_HOME_IN_AIR, 0);
  *
  * @group Commander
  * @min 0
- * @max 2
+ * @max 3
  * @value 0 RC Transmitter only
- * @value 1 Joystick only/No RC Checks
- * @value 2 RC and Joystick are accepted
+ * @value 1 Joystick only
+ * @value 2 RC and Joystick with fallback
+ * @value 3 RC or Joystick keep first
  */
-PARAM_DEFINE_INT32(COM_RC_IN_MODE, 0);
+PARAM_DEFINE_INT32(COM_RC_IN_MODE, 3);
 
 /**
  * RC input arm/disarm command duration

src/modules/manual_control/ManualControlSelector.cpp

@@ -54,6 +54,11 @@ void ManualControlSelector::updateWithNewInputSample(uint64_t now, const manual_
 		_setpoint = input;
 		_setpoint.valid = true;
 		_instance = instance;
+
+		if (_first_valid_source == manual_control_setpoint_s::SOURCE_UNKNOWN) {
+			// initialize first valid source once
+			_first_valid_source = _setpoint.data_source;
+		}
 	}
 }
 
@@ -73,9 +78,12 @@ bool ManualControlSelector::isInputValid(const manual_control_setpoint_s &input,
 					     || input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_4
 					     || input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_5);
 	const bool source_any_matched = (_rc_in_mode == 2);
+	const bool source_first_matched = (_rc_in_mode == 3) &&
+					  (input.data_source == _first_valid_source
+					   || _first_valid_source == manual_control_setpoint_s::SOURCE_UNKNOWN);
 
 	return sample_from_the_past && sample_newer_than_timeout
-	       && (source_rc_matched || source_mavlink_matched || source_any_matched);
+	       && (source_rc_matched || source_mavlink_matched || source_any_matched || source_first_matched);
 }
 
 manual_control_setpoint_s &ManualControlSelector::setpoint()

src/modules/manual_control/ManualControlSelector.hpp

@@ -53,4 +53,5 @@ private:
 	uint64_t _timeout{0};
 	int32_t _rc_in_mode{0};
 	int _instance{-1};
+	uint8_t _first_valid_source{manual_control_setpoint_s::SOURCE_UNKNOWN};
 };

src/modules/manual_control/ManualControlSelectorTest.cpp

@@ -168,6 +168,57 @@ TEST(ManualControlSelector, AutoInput)
 	EXPECT_EQ(selector.instance(), 1);
 }
 
+TEST(ManualControlSelector, FirstInput)
+{
+	ManualControlSelector selector;
+	selector.setRcInMode(3);
+	selector.setTimeout(500_ms);
+
+	uint64_t timestamp = some_time;
+
+	manual_control_setpoint_s input {};
+	input.data_source = manual_control_setpoint_s::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.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.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.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.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_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.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.instance(), 0);
+}
+
 TEST(ManualControlSelector, RcTimeout)
 {
 	ManualControlSelector selector;