From 300e43914497fbde3200efd0b02f7f6b6e457471 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Thu, 9 Dec 2021 11:15:07 +0100
Subject: [PATCH] ManualControlSelector: Enable original PX4 default behavior
 until QGC catches up

---
 src/modules/commander/commander_params.c      |  9 ++--
 .../manual_control/ManualControlSelector.cpp  | 10 +++-
 .../manual_control/ManualControlSelector.hpp  |  1 +
 .../ManualControlSelectorTest.cpp             | 51 +++++++++++++++++++
 4 files changed, 66 insertions(+), 5 deletions(-)

diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index dc2504ae42..006603dcc1 100644
--- a/src/modules/commander/commander_params.c
+++ b/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
diff --git a/src/modules/manual_control/ManualControlSelector.cpp b/src/modules/manual_control/ManualControlSelector.cpp
index 8b31b66862..c1f0280a75 100644
--- a/src/modules/manual_control/ManualControlSelector.cpp
+++ b/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()
diff --git a/src/modules/manual_control/ManualControlSelector.hpp b/src/modules/manual_control/ManualControlSelector.hpp
index 0a11e0b60b..ab6dfbd9e4 100644
--- a/src/modules/manual_control/ManualControlSelector.hpp
+++ b/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};
 };
diff --git a/src/modules/manual_control/ManualControlSelectorTest.cpp b/src/modules/manual_control/ManualControlSelectorTest.cpp
index e860128dc7..beba41dbca 100644
--- a/src/modules/manual_control/ManualControlSelectorTest.cpp
+++ b/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;