Added frequency limits

drivers/servo/servo.cpp

@@ -71,7 +71,6 @@ namespace servo {
     pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
   }
 
-
   float Servo::get_frequency() const {
     return pwm_frequency;
   }
@@ -79,41 +78,43 @@ namespace servo {
   bool Servo::set_frequency(float freq) {
     bool success = false;
 
-    // Calculate a suitable pwm wrap period for this frequency
+    if((freq >= MIN_FREQUENCY) && (freq <= MAX_FREQUENCY)) {
-    uint16_t period; uint16_t div16;
+      // Calculate a suitable pwm wrap period for this frequency
-    if(Servo::calculate_pwm_factors(freq, period, div16)) {
+      uint16_t period; uint16_t div16;
+      if(Servo::calculate_pwm_factors(freq, period, div16)) {
 
-      // Record if the new period will be larger or smaller.
+        // Record if the new period will be larger or smaller.
-      // This is used to apply new pwm values either before or after the wrap is applied,
+        // This is used to apply new pwm values either before or after the wrap is applied,
-      // to avoid momentary blips in PWM output on SLOW_DECAY
+        // to avoid momentary blips in PWM output on SLOW_DECAY
-      bool pre_update_pwm = (period > pwm_period);
+        bool pre_update_pwm = (period > pwm_period);
 
-      pwm_period = period;
+        pwm_period = period;
-      pwm_frequency = freq;
+        pwm_frequency = freq;
 
-      uint pin_num = pwm_gpio_to_slice_num(pin);
+        uint pin_num = pwm_gpio_to_slice_num(pin);
 
-      // Apply the new divider
+        // Apply the new divider
-      uint8_t div = div16 >> 4;
+        uint8_t div = div16 >> 4;
-      uint8_t mod = div16 % 16;
+        uint8_t mod = div16 % 16;
-      pwm_set_clkdiv_int_frac(pin_num, div, mod);
+        pwm_set_clkdiv_int_frac(pin_num, div, mod);
 
-      // If the the period is larger, update the pwm before setting the new wraps
+        // If the the period is larger, update the pwm before setting the new wraps
-      if(pre_update_pwm) {
+        if(pre_update_pwm) {
-        float current_pulse = get_pulse();
+          float current_pulse = get_pulse();
-        pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(current_pulse, pwm_period, pwm_frequency));
+          pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(current_pulse, pwm_period, pwm_frequency));
+        }
+
+        // Set the new wrap (should be 1 less than the period to get full 0 to 100%)
+        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) {
+          float current_pulse = get_pulse();
+          pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(current_pulse, pwm_period, pwm_frequency));
+        }
+
+        success = true;
       }
-
-      // Set the new wrap (should be 1 less than the period to get full 0 to 100%)
-      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) {
-        float current_pulse = get_pulse();
-        pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(current_pulse, pwm_period, pwm_frequency));
-      }
-
-      success = true;
     }
     return success;
   }

drivers/servo/servo.hpp

@@ -11,9 +11,12 @@ namespace servo {
     // Constants
     //--------------------------------------------------
   public:
-    static const uint16_t DEFAULT_FREQUENCY = 50;       //The standard servo update rate
+    static constexpr float DEFAULT_FREQUENCY = 50.0f;       // The standard servo update rate
 
   private:
+    static constexpr float MIN_FREQUENCY = 10.0f;           // Lowest achievable with hardware PWM with good resolution
+    static constexpr float MAX_FREQUENCY = 350.0f;          // Highest nice value that still allows the full uS pulse range
+                                                            // Some servos are rated for 333Hz for instance
     static const uint32_t MAX_PWM_WRAP = UINT16_MAX;
 
 

micropython/modules/servo/servo.cpp

@@ -664,7 +664,7 @@ extern mp_obj_t Servo_frequency(size_t n_args, const mp_obj_t *pos_args, mp_map_
         float freq = mp_obj_get_float(args[ARG_freq].u_obj);
 
         if(!self->servo->set_frequency(freq))
-            mp_raise_ValueError("freq out of range");
+            mp_raise_ValueError("freq out of range. Expected 10Hz to 350Hz");
         else
             return mp_const_none;
     }