From 8e67c3459a052605e9c9b0ae52062c6175bea928 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 29 Jan 2020 13:16:54 +1100
Subject: [PATCH] AP_RCProtocol: added multi-channel read() API

and use pulse_input_enable() to disable pulse input when not needed
---
 libraries/AP_RCProtocol/AP_RCProtocol.cpp         | 12 ++++++++++++
 libraries/AP_RCProtocol/AP_RCProtocol.h           |  1 +
 libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp |  8 ++++++++
 libraries/AP_RCProtocol/AP_RCProtocol_Backend.h   |  1 +
 4 files changed, 22 insertions(+)

diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp
index 210cb3b8fe..035c5a1cb9 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp
+++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp
@@ -26,6 +26,8 @@
 #include "AP_RCProtocol_FPort.h"
 #include <AP_Math/AP_Math.h>
 
+extern const AP_HAL::HAL& hal;
+
 void AP_RCProtocol::init()
 {
     backend[AP_RCProtocol::PPM] = new AP_RCProtocol_PPMSum(*this);
@@ -150,6 +152,9 @@ bool AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate)
                 memset(_good_frames, 0, sizeof(_good_frames));
                 _last_input_ms = now;
                 _detected_with_bytes = true;
+
+                // stop decoding pulses to save CPU
+                hal.rcin->pulse_input_enable(false);
                 break;
             }
         }
@@ -257,6 +262,13 @@ uint16_t AP_RCProtocol::read(uint8_t chan)
     return 0;
 }
 
+void AP_RCProtocol::read(uint16_t *pwm, uint8_t n)
+{
+    if (_detected_protocol != AP_RCProtocol::NONE) {
+        backend[_detected_protocol]->read(pwm, n);
+    }
+}
+
 int16_t AP_RCProtocol::get_RSSI(void) const
 {
     if (_detected_protocol != AP_RCProtocol::NONE) {
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h
index c150b85bd8..9db14c9dc2 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol.h
+++ b/libraries/AP_RCProtocol/AP_RCProtocol.h
@@ -62,6 +62,7 @@ public:
 
     uint8_t num_channels();
     uint16_t read(uint8_t chan);
+    void read(uint16_t *pwm, uint8_t n);
     bool new_input();
     void start_bind(void);
     int16_t get_RSSI(void) const;
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp
index f4b6fb393f..86a8a7315a 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp
+++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp
@@ -46,6 +46,14 @@ uint16_t AP_RCProtocol_Backend::read(uint8_t chan)
     return _pwm_values[chan];
 }
 
+void AP_RCProtocol_Backend::read(uint16_t *pwm, uint8_t n)
+{
+    if (n >= MAX_RCIN_CHANNELS) {
+        n = MAX_RCIN_CHANNELS;
+    }
+    memcpy(pwm, _pwm_values, n*sizeof(pwm[0]));
+}
+
 /*
   provide input from a backend
  */
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h
index 41154bdf14..f32137755e 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h
+++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h
@@ -28,6 +28,7 @@ public:
     virtual void process_pulse(uint32_t width_s0, uint32_t width_s1) {}
     virtual void process_byte(uint8_t byte, uint32_t baudrate) {}
     uint16_t read(uint8_t chan);
+    void read(uint16_t *pwm, uint8_t n);
     bool new_input();
     uint8_t num_channels();