From 7e897c16c02ba09f986162c02e8870a75003036a Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 5 May 2018 18:54:12 +1000
Subject: [PATCH] HAL_ChibiOS: maintain rcout state beyond BRD_PWM_COUNT

allow channels beyond BRD_PWM_COUNT to be sent over SBUS or DShot
distribution
---
 libraries/AP_HAL_ChibiOS/RCOutput.cpp | 36 ++++++++++++++-------------
 libraries/AP_HAL_ChibiOS/RCOutput.h   | 16 +++++++-----
 2 files changed, 29 insertions(+), 23 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
index 5625a58a12..db49169284 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
@@ -59,7 +59,7 @@ void RCOutput::init()
                 group.chan[j] = CHAN_DISABLED;
             }
             if (group.chan[j] != CHAN_DISABLED) {
-                total_channels = MAX(total_channels, group.chan[j]+1);
+                num_fmu_channels = MAX(num_fmu_channels, group.chan[j]+1);
                 group.ch_mask |= (1U<<group.chan[j]);
             }
         }
@@ -74,7 +74,6 @@ void RCOutput::init()
         iomcu.init();
         // with IOMCU the local (FMU) channels start at 8
         chan_offset = 8;
-        total_channels += chan_offset;
     }
 #endif
     chMtxObjectInit(&trigger_mutex);
@@ -236,7 +235,7 @@ void RCOutput::set_default_rate(uint16_t freq_hz)
 
 uint16_t RCOutput::get_freq(uint8_t chan)
 {
-    if (chan >= total_channels) {
+    if (chan >= max_channels) {
         return 0;
     }
 #if HAL_WITH_IO_MCU
@@ -260,7 +259,7 @@ uint16_t RCOutput::get_freq(uint8_t chan)
 
 void RCOutput::enable_ch(uint8_t chan)
 {
-    if (chan >= total_channels) {
+    if (chan >= max_channels) {
         return;
     }
     if (chan < chan_offset) {
@@ -280,7 +279,7 @@ void RCOutput::enable_ch(uint8_t chan)
 
 void RCOutput::disable_ch(uint8_t chan)
 {
-    if (chan >= total_channels) {
+    if (chan >= max_channels) {
         return;
     }
     if (chan < chan_offset) {
@@ -301,7 +300,7 @@ void RCOutput::disable_ch(uint8_t chan)
 
 void RCOutput::write(uint8_t chan, uint16_t period_us)
 {
-    if (chan >= total_channels) {
+    if (chan >= max_channels) {
         return;
     }
     last_sent[chan] = period_us;
@@ -330,9 +329,12 @@ void RCOutput::write(uint8_t chan, uint16_t period_us)
     chan -= chan_offset;
 
     period[chan] = period_us;
-    num_channels = MAX(chan+1, num_channels);
-    if (!corked) {
-        push_local();
+
+    if (chan < num_fmu_channels) {
+        active_fmu_channels = MAX(chan+1, active_fmu_channels);
+        if (!corked) {
+            push_local();
+        }
     }
 }
 
@@ -341,10 +343,10 @@ void RCOutput::write(uint8_t chan, uint16_t period_us)
  */
 void RCOutput::push_local(void)
 {
-    if (num_channels == 0) {
+    if (active_fmu_channels == 0) {
         return;
     }
-    uint16_t outmask = (1U<<num_channels)-1;
+    uint16_t outmask = (1U<<active_fmu_channels)-1;
     outmask &= en_mask;
 
     uint16_t widest_pulse = 0;
@@ -420,7 +422,7 @@ void RCOutput::push_local(void)
 
 uint16_t RCOutput::read(uint8_t chan)
 {
-    if (chan >= total_channels) {
+    if (chan >= max_channels) {
         return 0;
     }
 #if HAL_WITH_IO_MCU
@@ -434,8 +436,8 @@ uint16_t RCOutput::read(uint8_t chan)
 
 void RCOutput::read(uint16_t* period_us, uint8_t len)
 {
-    if (len > total_channels) {
-        len = total_channels;
+    if (len > max_channels) {
+        len = max_channels;
     }
 #if HAL_WITH_IO_MCU
     for (uint8_t i=0; i<MIN(len, chan_offset); i++) {
@@ -453,7 +455,7 @@ void RCOutput::read(uint16_t* period_us, uint8_t len)
 
 uint16_t RCOutput::read_last_sent(uint8_t chan)
 {
-    if (chan >= total_channels) {
+    if (chan >= max_channels) {
         return 0;
     }
     return last_sent[chan];
@@ -461,8 +463,8 @@ uint16_t RCOutput::read_last_sent(uint8_t chan)
 
 void RCOutput::read_last_sent(uint16_t* period_us, uint8_t len)
 {
-    if (len > total_channels) {
-        len = total_channels;
+    if (len > max_channels) {
+        len = max_channels;
     }
     for (uint8_t i=0; i<len; i++) {
         period_us[i] = read_last_sent(i);
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h
index b8544dc10f..2e1be3a2c6 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.h
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.h
@@ -214,17 +214,21 @@ private:
     // offset of first local channel
     uint8_t chan_offset;
 
-    // total number of channels
-    uint8_t total_channels;
+    // total number of channels on FMU
+    uint8_t num_fmu_channels;
+
+    // number of active fmu channels
+    uint8_t active_fmu_channels;
+    
+    static const uint8_t max_channels = 16;
     
     // last sent values are for all channels
-    uint16_t last_sent[16];
+    uint16_t last_sent[max_channels];
     
     // these values are for the local channels. Non-local channels are handled by IOMCU
     uint32_t en_mask;
-    uint16_t period[16];
-    uint16_t safe_pwm[16]; // pwm to use when safety is on
-    uint8_t num_channels;
+    uint16_t period[max_channels];
+    uint16_t safe_pwm[max_channels]; // pwm to use when safety is on
     bool corked;
     // mask of channels that are running in high speed
     uint16_t fast_channel_mask;