From e0ec46f06d732041e6b52471134f43433020ce74 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 13 Aug 2020 12:27:43 +1000
Subject: [PATCH] AP_RCProtocol: support a mask of enabled RC protocols

---
 libraries/AP_RCProtocol/AP_RCProtocol.cpp | 37 +++++++++++++++++++++++
 libraries/AP_RCProtocol/AP_RCProtocol.h   | 13 ++++++++
 2 files changed, 50 insertions(+)

diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp
index 3d15457cda..5acca363a7 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp
+++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp
@@ -30,6 +30,7 @@
 #include "AP_RCProtocol_ST24.h"
 #include "AP_RCProtocol_FPort.h"
 #include <AP_Math/AP_Math.h>
+#include <RC_Channel/RC_Channel.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -64,6 +65,16 @@ void AP_RCProtocol::process_pulse(uint32_t width_s0, uint32_t width_s1)
 {
     uint32_t now = AP_HAL::millis();
     bool searching = (now - _last_input_ms >= 200);
+
+#ifndef IOMCU_FW
+    rc_protocols_mask = rc().enabled_protocols();
+#endif
+
+    if (_detected_protocol != AP_RCProtocol::NONE &&
+        !protocol_enabled(_detected_protocol)) {
+        _detected_protocol = AP_RCProtocol::NONE;
+    }
+    
     if (_detected_protocol != AP_RCProtocol::NONE && _detected_with_bytes && !searching) {
         // we're using byte inputs, discard pulses
         return;
@@ -85,6 +96,9 @@ void AP_RCProtocol::process_pulse(uint32_t width_s0, uint32_t width_s1)
             continue;
         }
         if (backend[i] != nullptr) {
+            if (!protocol_enabled(rcprotocol_t(i))) {
+                continue;
+            }
             uint32_t frame_count = backend[i]->get_rc_frame_count();
             uint32_t input_count = backend[i]->get_rc_input_count();
             backend[i]->process_pulse(width_s0, width_s1);
@@ -131,6 +145,16 @@ bool AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate)
 {
     uint32_t now = AP_HAL::millis();
     bool searching = (now - _last_input_ms >= 200);
+
+#ifndef IOMCU_FW
+    rc_protocols_mask = rc().enabled_protocols();
+#endif
+
+    if (_detected_protocol != AP_RCProtocol::NONE &&
+        !protocol_enabled(_detected_protocol)) {
+        _detected_protocol = AP_RCProtocol::NONE;
+    }
+
     if (_detected_protocol != AP_RCProtocol::NONE && !_detected_with_bytes && !searching) {
         // we're using pulse inputs, discard bytes
         return false;
@@ -148,6 +172,9 @@ bool AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate)
     // otherwise scan all protocols
     for (uint8_t i = 0; i < AP_RCProtocol::NONE; i++) {
         if (backend[i] != nullptr) {
+            if (!protocol_enabled(rcprotocol_t(i))) {
+                continue;
+            }
             uint32_t frame_count = backend[i]->get_rc_frame_count();
             uint32_t input_count = backend[i]->get_rc_input_count();
             backend[i]->process_byte(byte, baudrate);
@@ -358,6 +385,16 @@ void AP_RCProtocol::add_uart(AP_HAL::UARTDriver* uart)
     added.baudrate = 115200U;
 }
 
+// return true if a specific protocol is enabled
+bool AP_RCProtocol::protocol_enabled(rcprotocol_t protocol) const
+{
+    if ((rc_protocols_mask & 1) != 0) {
+        // all protocols enabled
+        return true;
+    }
+    return ((1U<<(uint8_t(protocol)+1)) & rc_protocols_mask) != 0;
+}
+
 namespace AP {
     AP_RCProtocol &RC()
     {
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h
index a0715420f5..9ee95cb270 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol.h
+++ b/libraries/AP_RCProtocol/AP_RCProtocol.h
@@ -83,9 +83,19 @@ public:
     // add a UART for RCIN
     void add_uart(AP_HAL::UARTDriver* uart);
 
+#ifdef IOMCU_FW
+    // set allowed RC protocols
+    void set_rc_protocols(uint32_t mask) {
+        rc_protocols_mask = mask;
+    }
+#endif
+
 private:
     void check_added_uart(void);
 
+    // return true if a specific protocol is enabled
+    bool protocol_enabled(enum rcprotocol_t protocol) const;
+
     enum rcprotocol_t _detected_protocol = NONE;
     uint16_t _disabled_for_pulses;
     bool _detected_with_bytes;
@@ -110,6 +120,9 @@ private:
         uint32_t last_baud_change_ms;
         enum config_phase phase;
     } added;
+
+    // allowed RC protocols mask (first bit means "all")
+    uint32_t rc_protocols_mask;
 };
 
 namespace AP {