From df9679c73a483fa7ff2016ab259c1d2533bdb403 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 17 Mar 2022 18:01:03 +1100
Subject: [PATCH] HAL_ChibiOS: prevent conflicting RC input

when we have RC from both IOMCU and from rcprotocol (eg. from
SERIALn_PROTOCOL=23) we need to only process one of them. This
prioritises IOMCU input
---
 libraries/AP_HAL_ChibiOS/RCInput.cpp | 9 ++++++++-
 libraries/AP_HAL_ChibiOS/RCInput.h   | 1 +
 2 files changed, 9 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_ChibiOS/RCInput.cpp b/libraries/AP_HAL_ChibiOS/RCInput.cpp
index cbaaf03c7b..a82b3fdb1b 100644
--- a/libraries/AP_HAL_ChibiOS/RCInput.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCInput.cpp
@@ -175,7 +175,13 @@ void RCInput::_timer_tick(void)
     }
 #endif
 
-    if (rcprot.new_input()) {
+    uint32_t now = AP_HAL::millis();
+    const bool have_iocmu_rc = (_rcin_last_iomcu_ms != 0 && now - _rcin_last_iomcu_ms < 400);
+    if (!have_iocmu_rc) {
+        _rcin_last_iomcu_ms = 0;
+    }
+
+    if (!have_iocmu_rc && rcprot.new_input()) {
         WITH_SEMAPHORE(rcin_mutex);
         _rcin_timestamp_last_signal = AP_HAL::micros();
         _num_channels = rcprot.num_channels();
@@ -208,6 +214,7 @@ void RCInput::_timer_tick(void)
         if (AP_BoardConfig::io_enabled() &&
             iomcu.check_rcinput(last_iomcu_us, _num_channels, _rc_values, RC_INPUT_MAX_CHANNELS)) {
             _rcin_timestamp_last_signal = last_iomcu_us;
+            _rcin_last_iomcu_ms = now;
 #ifndef HAL_NO_UARTDRIVER
             rc_protocol = iomcu.get_rc_protocol();
             _rssi = iomcu.get_RSSI();
diff --git a/libraries/AP_HAL_ChibiOS/RCInput.h b/libraries/AP_HAL_ChibiOS/RCInput.h
index 6a35ab0598..48eff5767b 100644
--- a/libraries/AP_HAL_ChibiOS/RCInput.h
+++ b/libraries/AP_HAL_ChibiOS/RCInput.h
@@ -69,6 +69,7 @@ private:
     int16_t _rssi = -1;
     int16_t _rx_link_quality = -1;
     uint32_t _rcin_timestamp_last_signal;
+    uint32_t _rcin_last_iomcu_ms;
     bool _init;
     const char *last_protocol;
     bool pulse_input_enabled;