From 2492fb35e46aa49a5ce29f953be97dcbde95cd95 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 15 Apr 2021 10:57:36 -0400
Subject: [PATCH] rc_update: require consecutive valid input_rc before
 publishing

---
 src/modules/rc_update/rc_update.cpp | 50 ++++++++++++++++-------------
 1 file changed, 27 insertions(+), 23 deletions(-)

diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp
index 590f4ebfed..d259496866 100644
--- a/src/modules/rc_update/rc_update.cpp
+++ b/src/modules/rc_update/rc_update.cpp
@@ -424,35 +424,39 @@ void RCUpdate::Run()
 		/* publish rc_channels topic even if signal is invalid, for debug */
 		_rc_channels_pub.publish(_rc);
 
-		/* only publish manual control if the signal is present */
-		if (input_source_stable && channel_count_stable && !signal_lost
-		    && (input_rc.timestamp_last_signal > _last_timestamp_signal)) {
+		// only publish manual control if the signal is present and regularly updating
+		if (input_source_stable && channel_count_stable && !signal_lost) {
 
-			_last_timestamp_signal = input_rc.timestamp_last_signal;
-			perf_count(_valid_data_interval_perf);
+			if ((input_rc.timestamp_last_signal > _last_timestamp_signal)
+			    && (input_rc.timestamp_last_signal - _last_timestamp_signal < 1_s)) {
 
-			// check if channels actually updated
-			bool rc_updated = false;
+				perf_count(_valid_data_interval_perf);
 
-			for (unsigned i = 0; i < channel_count_limited; i++) {
-				if (_rc_values_previous[i] != input_rc.values[i]) {
-					rc_updated = true;
-					break;
+				// check if channels actually updated
+				bool rc_updated = false;
+
+				for (unsigned i = 0; i < channel_count_limited; i++) {
+					if (_rc_values_previous[i] != input_rc.values[i]) {
+						rc_updated = true;
+						break;
+					}
+				}
+
+				// limit processing if there's no update
+				if (rc_updated || (hrt_elapsed_time(&_last_manual_control_setpoint_publish) > 300_ms)) {
+					UpdateManualSetpoint(input_rc.timestamp_last_signal);
+				}
+
+				UpdateManualSwitches(input_rc.timestamp_last_signal);
+
+				/* Update parameters from RC Channels (tuning with RC) if activated */
+				if (hrt_elapsed_time(&_last_rc_to_param_map_time) > 1_s) {
+					set_params_from_rc();
+					_last_rc_to_param_map_time = hrt_absolute_time();
 				}
 			}
 
-			// limit processing if there's no update
-			if (rc_updated || (hrt_elapsed_time(&_last_manual_control_setpoint_publish) > 300_ms)) {
-				UpdateManualSetpoint(input_rc.timestamp_last_signal);
-			}
-
-			UpdateManualSwitches(input_rc.timestamp_last_signal);
-
-			/* Update parameters from RC Channels (tuning with RC) if activated */
-			if (hrt_elapsed_time(&_last_rc_to_param_map_time) > 1_s) {
-				set_params_from_rc();
-				_last_rc_to_param_map_time = hrt_absolute_time();
-			}
+			_last_timestamp_signal = input_rc.timestamp_last_signal;
 		}
 
 		memcpy(_rc_values_previous, input_rc.values, sizeof(input_rc.values[0]) * channel_count_limited);