From 28a29ad0508677915a9caf1ccd00e965b47f02b5 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Thu, 7 Oct 2021 17:53:33 +0200
Subject: [PATCH] ManualControlSelector: revamp logic to switch when
 configuration is changed

---
 .../manual_control/ManualControlSelector.cpp  | 44 +++++++++----------
 .../manual_control/ManualControlSelector.hpp  |  1 +
 .../ManualControlSelectorTest.cpp             | 24 ++++++++++
 3 files changed, 47 insertions(+), 22 deletions(-)

diff --git a/src/modules/manual_control/ManualControlSelector.cpp b/src/modules/manual_control/ManualControlSelector.cpp
index df6f8544b8..873ee9fbd4 100644
--- a/src/modules/manual_control/ManualControlSelector.cpp
+++ b/src/modules/manual_control/ManualControlSelector.cpp
@@ -39,7 +39,7 @@ namespace manual_control
 
 void ManualControlSelector::update_time_only(uint64_t now)
 {
-	if (now - _setpoint.chosen_input.timestamp_sample > _timeout) {
+	if (!isInputValid(_setpoint.chosen_input, now)) {
 		_setpoint.valid = false;
 		_instance = -1;
 	}
@@ -47,16 +47,27 @@ void ManualControlSelector::update_time_only(uint64_t now)
 
 void ManualControlSelector::update_manual_control_input(uint64_t now, const manual_control_input_s &input, int instance)
 {
-	// This method requires the current timestamp explicitly in order to prevent the case
-	// where the timestamp_sample of some source is already outdated.
-
-	if (now >= input.timestamp_sample && now - input.timestamp_sample > _timeout) {
-		return;
-	}
-
-	// If previous setpoint is timed out, set it invalid, so it can get replaced below.
+	// First check if the setpoint in use got invalid, so it can get replaced below.
 	update_time_only(now);
 
+	const bool update_existing_input = _setpoint.valid == true && input.data_source == _setpoint.chosen_input.data_source;
+	const bool start_using_new_input = _setpoint.valid == false;
+
+	// 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)) {
+		_setpoint = setpoint_from_input(input);
+		_setpoint.valid = true;
+		_instance = instance;
+	}
+}
+
+bool ManualControlSelector::isInputValid(const manual_control_input_s &input, uint64_t now) const
+{
+	// Check for timeout
+	const bool sample_from_the_past = now >= input.timestamp_sample;
+	const bool sample_newer_than_timeout = now - input.timestamp_sample < _timeout;
+
+	// Check if source matches the configuration
 	const bool source_rc_matched = (_rc_in_mode == 0) && (input.data_source == manual_control_input_s::SOURCE_RC);
 	const bool source_mavlink_matched = (_rc_in_mode == 1) &&
 					    (input.data_source == manual_control_input_s::SOURCE_MAVLINK_0
@@ -66,20 +77,9 @@ void ManualControlSelector::update_manual_control_input(uint64_t now, const manu
 					     || input.data_source == manual_control_input_s::SOURCE_MAVLINK_4
 					     || input.data_source == manual_control_input_s::SOURCE_MAVLINK_5);
 	const bool source_any_matched = (_rc_in_mode == 3);
-	const bool not_overriding_existing_source = !_setpoint.valid
-			|| (_setpoint.chosen_input.data_source == input.data_source);
 
-	if (source_rc_matched || source_mavlink_matched || source_any_matched) {
-		if (not_overriding_existing_source) {
-			_setpoint = setpoint_from_input(input);
-			_setpoint.valid = true;
-			_instance = instance;
-		}
-
-	} else {
-		_setpoint.valid = false;
-		_instance = -1;
-	}
+	return sample_from_the_past && sample_newer_than_timeout
+	       && (source_rc_matched || source_mavlink_matched || source_any_matched);
 }
 
 manual_control_setpoint_s ManualControlSelector::setpoint_from_input(const manual_control_input_s &input)
diff --git a/src/modules/manual_control/ManualControlSelector.hpp b/src/modules/manual_control/ManualControlSelector.hpp
index fcf5005355..5abaa08ede 100644
--- a/src/modules/manual_control/ManualControlSelector.hpp
+++ b/src/modules/manual_control/ManualControlSelector.hpp
@@ -51,6 +51,7 @@ public:
 	int instance() const { return _instance; };
 
 private:
+	bool isInputValid(const manual_control_input_s &input, uint64_t now) const;
 	static manual_control_setpoint_s setpoint_from_input(const manual_control_input_s &input);
 
 	manual_control_setpoint_s _setpoint{};
diff --git a/src/modules/manual_control/ManualControlSelectorTest.cpp b/src/modules/manual_control/ManualControlSelectorTest.cpp
index a0ba5ca2e1..fc32644138 100644
--- a/src/modules/manual_control/ManualControlSelectorTest.cpp
+++ b/src/modules/manual_control/ManualControlSelectorTest.cpp
@@ -40,6 +40,30 @@ using namespace manual_control;
 
 static constexpr uint64_t some_time = 12345678;
 
+TEST(ManualControlSelector, RcInputContinuous)
+{
+	ManualControlSelector selector;
+	selector.set_rc_in_mode(0);
+	selector.set_timeout(500_ms);
+
+	uint64_t timestamp = some_time;
+
+	// Now provide input with the correct source.
+	manual_control_input_s input {};
+	input.data_source = manual_control_input_s::SOURCE_RC;
+	input.timestamp_sample = timestamp;
+
+	for (int i = 0; i < 5; i++) {
+		selector.update_manual_control_input(timestamp, input, 1);
+		EXPECT_TRUE(selector.setpoint().valid);
+		EXPECT_EQ(selector.setpoint().chosen_input.timestamp_sample, timestamp);
+		EXPECT_EQ(selector.instance(), 1);
+		EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC);
+		timestamp += 100_ms;
+		input.timestamp_sample = timestamp;
+	}
+}
+
 TEST(ManualControlSelector, RcInputOnly)
 {
 	ManualControlSelector selector;