HAL_ChibiOS: switch base freq when needed

timers can't do below 123 Hz with a clock of 8MHz, so we need to
change clock frequency based on the target period

libraries/AP_HAL_ChibiOS/RCOutput.cpp

@@ -27,7 +27,7 @@ extern const AP_HAL::HAL& hal;
 extern AP_IOMCU iomcu;
 #endif
 
-const struct ChibiRCOutput::pwm_group ChibiRCOutput::pwm_group_list[] = { HAL_PWM_GROUPS };
+struct ChibiRCOutput::pwm_group ChibiRCOutput::pwm_group_list[] = { HAL_PWM_GROUPS };
 
 #define NUM_GROUPS ARRAY_SIZE_SIMPLE(pwm_group_list)
 
@@ -91,16 +91,61 @@ void ChibiRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
               multicopter properly
              */
             update_mask |= grp_ch_mask;
+            uint16_t freq_set = freq_hz;
+            if (!pwm_group_list[i].advanced_timer && freq_set > 400) {
+                freq_set = 400;
+            }
+            bool changed_clock = false;
+            if (freq_set > 400 && pwm_group_list[i].pwm_cfg.frequency == 1000000) {
+                // need to change to an 8MHz clock
+                pwm_group_list[i].pwm_cfg.frequency = 8000000;
+                changed_clock = true;
+            } else if (freq_set <= 400 && pwm_group_list[i].pwm_cfg.frequency == 8000000) {
+                // need to change to an 1MHz clock
+                pwm_group_list[i].pwm_cfg.frequency = 1000000;
+            }
+            if (changed_clock) {
+                pwmStop(pwm_group_list[i].pwm_drv);
+                pwmStart(pwm_group_list[i].pwm_drv, &pwm_group_list[i].pwm_cfg);
+            }
             pwmChangePeriod(pwm_group_list[i].pwm_drv, 
-                            pwm_group_list[i].pwm_cfg.frequency/freq_hz);
+                            pwm_group_list[i].pwm_cfg.frequency/freq_set);
         }
     }
+    if (freq_hz > 50) {
         fast_channel_mask |= update_mask;
+    }
     if (chmask != update_mask) {
         hal.console->printf("RCOutput: Failed to set PWM frequency req %x set %x\n", (unsigned)chmask, (unsigned)update_mask);
     }
 }
 
+/*
+  set default output rate
+ */
+void ChibiRCOutput::set_default_rate(uint16_t freq_hz)
+{
+#if HAL_WITH_IO_MCU
+    if (AP_BoardConfig::io_enabled()) {
+        iomcu.set_default_rate(freq_hz);
+    }
+#endif
+    for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
+        uint16_t grp_ch_mask = 0;
+        for (uint8_t j=0; j<4; j++) {
+            if (pwm_group_list[i].chan[j] != CHAN_DISABLED) {
+                grp_ch_mask |= (1U<<pwm_group_list[i].chan[j]);
+            }
+        }
+        if (grp_ch_mask & fast_channel_mask) {
+            // don't change fast channels
+            continue;
+        }
+        pwmChangePeriod(pwm_group_list[i].pwm_drv, 
+                        pwm_group_list[i].pwm_cfg.frequency/freq_hz);
+    }
+}
+
 uint16_t ChibiRCOutput::get_freq(uint8_t chan)
 {
     if (chan >= total_channels) {

libraries/AP_HAL_ChibiOS/RCOutput.h

@@ -53,15 +53,22 @@ public:
 
     bool enable_px4io_sbus_out(uint16_t rate_hz) override;
 
+    /*
+      set default update rate
+     */
+    void set_default_rate(uint16_t rate_hz) override;
+    
 private:
     struct pwm_group {
+        // only advanced timers can do high clocks needed for more than 400Hz
+        bool advanced_timer;
         uint8_t chan[4]; // chan number, zero based, 255 for disabled
         PWMConfig pwm_cfg;
         PWMDriver* pwm_drv;
     };
     enum output_mode _output_mode = MODE_PWM_NORMAL;
 
-    static const pwm_group pwm_group_list[];
+    static pwm_group pwm_group_list[];
     uint16_t _esc_pwm_min;
     uint16_t _esc_pwm_max;
 

libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py

@@ -536,11 +536,12 @@ def write_PWM_config(f):
             groups.append('HAL_PWM_GROUP%u' % group)
             if n in [1, 8]:
                 # only the advanced timers do 8MHz clocks
-                pwm_clock = 8000000
+                advanced_timer = 'true'
             else:
+                advanced_timer = 'false'
             pwm_clock = 1000000
             period = 20000 * pwm_clock / 1000000
-            f.write('''#define HAL_PWM_GROUP%u { \\
+            f.write('''#define HAL_PWM_GROUP%u { %s, \\
         {%u, %u, %u, %u}, \\
         /* Group Initial Config */ \\
         { \\
@@ -553,7 +554,7 @@ def write_PWM_config(f):
            {%s, NULL}, \\
            {%s, NULL}, \\
            {%s, NULL}  \\
-          }, 0, 0}, &PWMD%u}\n''' % (group,
+          }, 0, 0}, &PWMD%u}\n''' % (group, advanced_timer,
                                      chan_list[0], chan_list[1], chan_list[2], chan_list[3],
                                      pwm_clock,
                                      period,