diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp
index 10e1d2d2c2..f7c0797e9f 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.cpp
+++ b/libraries/AP_IOMCU/AP_IOMCU.cpp
@@ -550,8 +550,15 @@ void AP_IOMCU::push(void)
 // set output frequency
 void AP_IOMCU::set_freq(uint16_t chmask, uint16_t freq)
 {
+    const uint8_t masks[] = { 0x03,0x0C,0xF0 };
+    // ensure mask is legal for the timer layout
+    for (uint8_t i=0; i<ARRAY_SIZE_SIMPLE(masks); i++) {
+        if (chmask & masks[i]) {
+            chmask |= masks[i];
+        }
+    }
     rate.freq = freq;
-    rate.chmask = chmask;
+    rate.chmask |= chmask;
     trigger_event(IOEVENT_SET_RATES);
 }