Fix for frequency changes enabling servo PWM

drivers/servo/servo.cpp

@@ -96,7 +96,7 @@ namespace servo {
         pwm_set_clkdiv_int_frac(pin_num, div, mod);
 
         // If the the period is larger, update the pwm before setting the new wraps
-        if(pre_update_pwm) {
+        if(state.is_enabled() && pre_update_pwm) {
           apply_pulse(state.get_pulse());
         }
 
@@ -104,7 +104,7 @@ namespace servo {
         pwm_set_wrap(pin_num, pwm_period - 1);
 
         // If the the period is smaller, update the pwm after setting the new wraps
-        if(!pre_update_pwm) {
+        if(state.is_enabled() && !pre_update_pwm) {
           apply_pulse(state.get_pulse());
         }
 

drivers/servo/servo_cluster.cpp

@@ -136,8 +136,9 @@ namespace servo {
         // Update the pwm before setting the new wrap
         uint8_t servo_count = pwms.get_chan_count();
         for(uint servo = 0; servo < servo_count; servo++) {
-          float current_pulse = servos[servo].get_pulse();
-          apply_pulse(servo, current_pulse, false);
+          if(servos[servo].is_enabled()) {
+            apply_pulse(servo, servos[servo].get_pulse(), false);
+          }
           pwms.set_chan_offset(servo, (uint32_t)(servo_phases[servo] * (float)pwm_period), false);
         }
 
@@ -145,8 +146,7 @@ namespace servo {
         pwms.set_wrap(pwm_period, true);
 
         // Apply the new divider
-        // This is done after loading new PWM values to avoid a lockup condition
-        uint8_t div = div16 >> 4;
+        uint16_t div = div16 >> 4;
         uint8_t mod = div16 % 16;
         pwms.set_clkdiv_int_frac(div, mod);
 

micropython/modules/servo/README.md

@@ -172,8 +172,6 @@ frequency(
   freq   # float: the frequency between 10 and 350Hz
 )
 ```
-If the servo is disabled, this will enable it with the last pulse it was provided with.
-TODO change this behaviour!
 
 #### Useful Values
 
@@ -397,9 +395,6 @@ frequency(
   freq   # float: the frequency between 10 and 350Hz
 )
 ```
-If the servo is disabled, this will enable it with the last pulse it was provided with.
-TODO change this behaviour!
-
 Note, currently the frequency changes immediately, even if part-way through a pulse. It is recommended to disable all servos first before changing the frequency.
 
 #### Useful Values