HAL_ChibiOS: only set freq on valid IO channels

libraries/AP_HAL_ChibiOS/RCOutput.cpp

@@ -80,7 +80,7 @@ void RCOutput::init()
     chMtxObjectInit(&trigger_mutex);
 
     // setup default output rate of 50Hz
-    set_freq(0xFFFF, 50);
+    set_freq(0xFFFF ^ ((1U<<chan_offset)-1), 50);
 
 #ifdef HAL_GPIO_PIN_SAFETY_IN
     safety_state = AP_HAL::Util::SAFETY_DISARMED;
@@ -169,10 +169,15 @@ 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
+        uint16_t io_chmask = chmask & 0xFF;
         if (freq_hz > 50) {
-            io_fast_channel_mask = chmask;
+            io_fast_channel_mask |= io_chmask;
+        } else {
+            io_fast_channel_mask &= ~io_chmask;
+        }
+        if (io_chmask) {
+            iomcu.set_freq(io_fast_channel_mask, freq_hz);
         }
-        iomcu.set_freq(chmask, freq_hz);
     }
 #endif
 
@@ -206,9 +211,6 @@ void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
             fast_channel_mask |= group.ch_mask;
         }
     }
-    if (chmask != update_mask) {
-        hal.console->printf("RCOutput: Failed to set PWM frequency req %x set %x\n", (unsigned)chmask, (unsigned)update_mask);
-    }
 }
 
 /*