Fix for servo cluster frequency being inaccurate at the upper end

drivers/pwm/pwm_cluster.cpp

@@ -544,27 +544,35 @@ void PWMCluster::load_pwm() {
 }
 
 // Derived from the rp2 Micropython implementation: https://github.com/micropython/micropython/blob/master/ports/rp2/machine_pwm.c
-bool PWMCluster::calculate_pwm_factors(float freq, uint32_t& top_out, uint16_t& div16_out) {
+bool PWMCluster::calculate_pwm_factors(float freq, uint32_t& top_out, uint32_t& div256_out) {
   bool success = false;
   uint32_t source_hz = clock_get_hz(clk_sys) / PWM_CLUSTER_CYCLES;
 
   // Check the provided frequency is valid
   if((freq >= 0.01f) && (freq <= (float)(source_hz >> 1))) {
-    uint32_t div16_top = (uint32_t)((float)(source_hz << 4) / freq);
+    uint64_t div256_top = (uint64_t)((float)((uint64_t)source_hz << 8) / freq);
     uint64_t top = 1;
 
     while(true) {
         // Try a few small prime factors to get close to the desired frequency.
-        if((div16_top >= (5 << 4)) && (div16_top % 5 == 0) && (top * 5 <= MAX_PWM_CLUSTER_WRAP)) {
-            div16_top /= 5;
+        if((div256_top >= (11 << 8)) && (div256_top % 11 == 0) && (top * 11 <= MAX_PWM_CLUSTER_WRAP)) {
+            div256_top /= 11;
+            top *= 11;
+        }
+        else if((div256_top >= (7 << 8)) && (div256_top % 7 == 0) && (top * 7 <= MAX_PWM_CLUSTER_WRAP)) {
+            div256_top /= 7;
+            top *= 7;
+        }
+        else if((div256_top >= (5 << 8)) && (div256_top % 5 == 0) && (top * 5 <= MAX_PWM_CLUSTER_WRAP)) {
+            div256_top /= 5;
             top *= 5;
         }
-        else if((div16_top >= (3 << 4)) && (div16_top % 3 == 0) && (top * 3 <= MAX_PWM_CLUSTER_WRAP)) {
-            div16_top /= 3;
+        else if((div256_top >= (3 << 8)) && (div256_top % 3 == 0) && (top * 3 <= MAX_PWM_CLUSTER_WRAP)) {
+            div256_top /= 3;
             top *= 3;
         }
-        else if((div16_top >= (2 << 4)) && (top * 2 <= MAX_PWM_CLUSTER_WRAP)) {
-            div16_top /= 2;
+        else if((div256_top >= (2 << 8)) && (top * 2 <= MAX_PWM_CLUSTER_WRAP)) {
+            div256_top /= 2;
             top *= 2;
         }
         else {
@@ -573,9 +581,9 @@ bool PWMCluster::calculate_pwm_factors(float freq, uint32_t& top_out, uint16_t&
     }
 
     // Only return valid factors if the divisor is actually achievable
-    if(div16_top >= 16 && div16_top <= (UINT8_MAX << 4)) {
+    if(div256_top >= 256 && div256_top <= (UINT8_MAX << 8)) {
       top_out = top;
-      div16_out = div16_top;
+      div256_out = div256_top;
 
       success = true;
     }

drivers/pwm/pwm_cluster.hpp

@@ -117,7 +117,7 @@ namespace pimoroni {
 
     //--------------------------------------------------
   public:
-    static bool calculate_pwm_factors(float freq, uint32_t& top_out, uint16_t& div16_out);
+    static bool calculate_pwm_factors(float freq, uint32_t& top_out, uint32_t& div256_out);
   private:
     static bool bit_in_mask(uint bit, uint mask);
     static void sorted_insert(TransitionData array[], uint &size, const TransitionData &data);

drivers/servo/servo_cluster.cpp

@@ -33,8 +33,8 @@ namespace servo {
 
     if(pwms.init()) {
       // Calculate a suitable pwm wrap period for this frequency
-      uint32_t period; uint16_t div16;
-      if(pimoroni::PWMCluster::calculate_pwm_factors(pwm_frequency, period, div16)) {
+      uint32_t period; uint32_t div256;
+      if(pimoroni::PWMCluster::calculate_pwm_factors(pwm_frequency, period, div256)) {
         pwm_period = period;
 
         // Update the pwm before setting the new wrap
@@ -49,8 +49,8 @@ namespace servo {
 
         // Apply the new divider
         // This is done after loading new PWM values to avoid a lockup condition
-        uint8_t div = div16 >> 4;
-        uint8_t mod = div16 % 16;
+        uint8_t div = div256 >> 8;
+        uint8_t mod = div256 % 256;
         pwms.set_clkdiv_int_frac(div, mod);
 
         success = true;
@@ -127,8 +127,8 @@ namespace servo {
 
     if((freq >= ServoState::MIN_FREQUENCY) && (freq <= ServoState::MAX_FREQUENCY)) {
       // Calculate a suitable pwm wrap period for this frequency
-      uint32_t period; uint16_t div16;
-      if(pimoroni::PWMCluster::calculate_pwm_factors(freq, period, div16)) {
+      uint32_t period; uint32_t div256;
+      if(pimoroni::PWMCluster::calculate_pwm_factors(freq, period, div256)) {
 
         pwm_period = period;
         pwm_frequency = freq;
@@ -146,8 +146,8 @@ namespace servo {
         pwms.set_wrap(pwm_period, true);
 
         // Apply the new divider
-        uint16_t div = div16 >> 4;
-        uint8_t mod = div16 % 16;
+        uint16_t div = div256 >> 8;
+        uint8_t mod = div256 % 256;
         pwms.set_clkdiv_int_frac(div, mod);
 
         success = true;