From d17e9f321a85a7c2e9e5d4aad702e5a2583814eb Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 13 Jan 2018 08:13:00 +1100
Subject: [PATCH] HAL_ChibOS: fixed mixture of brushed and normal PWM

allow non-brushed PWM servos with brushed main motors
---
 libraries/AP_HAL_ChibiOS/RCOutput.cpp | 15 +++++++++++++--
 libraries/AP_HAL_ChibiOS/RCOutput.h   |  2 ++
 2 files changed, 15 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
index 57d69412ae..a9e8eb79ff 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
@@ -81,12 +81,20 @@ void ChibiRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
                 grp_ch_mask |= (1U<<pwm_group_list[i].chan[j]);
             }
         }
-        if ((grp_ch_mask & chmask) == grp_ch_mask) {
+        if ((grp_ch_mask & chmask) != 0) {
+            /*
+              we enable the new frequency on all groups that have one
+              of the requested channels. This means we may enable high
+              speed on some channels that aren't requested, but that
+              is needed in order to fly a vehicle such a a hex
+              multicopter properly
+             */
             update_mask |= grp_ch_mask;
             pwmChangePeriod(pwm_group_list[i].pwm_drv, 
                             pwm_group_list[i].pwm_cfg.frequency/freq_hz);
         }
     }
+    fast_channel_mask |= update_mask;
     if (chmask != update_mask) {
         hal.console->printf("RCOutput: Failed to set PWM frequency req %x set %x\n", (unsigned)chmask, (unsigned)update_mask);
     }
@@ -195,7 +203,10 @@ void ChibiRCOutput::push_local(void)
             uint8_t chan = pwm_group_list[i].chan[j];
             if (outmask & (1UL<<chan)) {
                 uint32_t period_us = period[chan];
-                if(_output_mode == MODE_PWM_BRUSHED) {
+                if(_output_mode == MODE_PWM_BRUSHED && (fast_channel_mask & (1UL<<chan))) {
+                    // note that we only use brushed signals on fast
+                    // channels. This allows for ordinary PWM on
+                    // servos attached to a brushed vehicle
                     if (period_us <= _esc_pwm_min) {
                         period_us = 0;
                     } else if (period_us >= _esc_pwm_max) {
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h
index 06a1900ab6..ef19dcf4b2 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.h
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.h
@@ -79,6 +79,8 @@ private:
     uint16_t period[16];
     uint8_t num_channels;
     bool corked;
+    // mask of channels that are running in high speed
+    uint16_t fast_channel_mask;
 
     // push out values to local PWM
     void push_local(void);