From 794ebe32e3dab506169aa320e591d8ae100b35c7 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 11 May 2018 16:30:18 +1000
Subject: [PATCH] HAL_ChibiOS: fixed mixture of oneshot and normal PWM on IOMCU

---
 libraries/AP_HAL_ChibiOS/RCOutput.cpp | 7 +++++--
 libraries/AP_HAL_ChibiOS/RCOutput.h   | 1 +
 2 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
index 94d8ae761b..5b34fa9afc 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
@@ -169,6 +169,9 @@ void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
 #if HAL_WITH_IO_MCU
     if (AP_BoardConfig::io_enabled()) {
         // change frequency on IOMCU
+        if (freq_hz > 50) {
+            io_fast_channel_mask = chmask;
+        }
         iomcu.set_freq(chmask, freq_hz);
     }
 #endif
@@ -307,7 +310,7 @@ void RCOutput::write(uint8_t chan, uint16_t period_us)
     // handle IO MCU channels
     if (AP_BoardConfig::io_enabled()) {
         uint16_t io_period_us = period_us;
-        if (iomcu_oneshot125) {
+        if (iomcu_oneshot125 && ((1U<<chan) & io_fast_channel_mask)) {
             // the iomcu only has one oneshot setting, so we need to scale by a factor
             // of 8 here for oneshot125
             io_period_us /= 8;
@@ -657,7 +660,7 @@ void RCOutput::set_output_mode(uint16_t mask, enum output_mode mode)
         iomcu_oneshot125 = (mode == MODE_PWM_ONESHOT125);
         // also setup IO to use a 1Hz frequency, so we only get output
         // when we trigger
-        iomcu.set_freq(0xFF, 1);
+        iomcu.set_freq(io_fast_channel_mask, 1);
         return iomcu.set_oneshot_mode();
     }
 #endif
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h
index 3a4e26fc38..b8544dc10f 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.h
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.h
@@ -228,6 +228,7 @@ private:
     bool corked;
     // mask of channels that are running in high speed
     uint16_t fast_channel_mask;
+    uint16_t io_fast_channel_mask;
 
     // min time to trigger next pulse to prevent overlap
     uint64_t min_pulse_trigger_us;