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<