From 794ebe32e3dab506169aa320e591d8ae100b35c7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell 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<