servo: sync pwm clock times

Arriving of SW PWM out of sync
can cause pulse width distortion - make them longer
Synchronize the update clock to avoid that

Signed-off-by: Timofey Titovets <nefelim4ag@gmail.com>
Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

klippy/extras/multi_pin.py

@@ -46,6 +46,8 @@ class PrinterMultiPin:
     def set_digital(self, print_time, value):
         for mcu_pin in self.mcu_pins:
             mcu_pin.set_digital(print_time, value)
+    def next_aligned_print_time(self, print_time, allow_early=0.):
+        return print_time
     def set_pwm(self, print_time, value):
         for mcu_pin in self.mcu_pins:
             mcu_pin.set_pwm(print_time, value)

klippy/extras/output_pin.py

@@ -46,6 +46,11 @@ class GCodeRequestQueue:
                 if action == "discard":
                     del rqueue[:pos+1]
                     continue
+                if action == "reschedule":
+                    del rqueue[:pos]
+                    self.next_min_flush_time = max(self.next_min_flush_time,
+                                                   min_wait)
+                    continue
                 if action == "delay":
                     pos -= 1
             del rqueue[:pos+1]
@@ -75,6 +80,10 @@ class GCodeRequestQueue:
                 action, min_wait = ret
                 if action == "discard":
                     break
+                if action == "reschedule":
+                    self.next_min_flush_time = max(self.next_min_flush_time,
+                                                   min_wait)
+                    continue
             self.next_min_flush_time = next_time + max(min_wait, min_sched_time)
             if action != "delay":
                 break

klippy/extras/replicape.py

@@ -67,6 +67,8 @@ class pca9685_pwm:
         cmd_queue = self._mcu.alloc_command_queue()
         self._set_cmd = self._mcu.lookup_command(
             "queue_pca9685_out oid=%c clock=%u value=%hu", cq=cmd_queue)
+    def next_aligned_print_time(self, print_time, allow_early=0.):
+        return print_time
     def set_pwm(self, print_time, value):
         clock = self._mcu.print_time_to_clock(print_time)
         if self._invert:

klippy/extras/servo.py

@@ -6,6 +6,7 @@
 from . import output_pin
 
 SERVO_SIGNAL_PERIOD = 0.020
+RESCHEDULE_SLACK = 0.000500
 
 class PrinterServo:
     def __init__(self, config):
@@ -47,8 +48,12 @@ class PrinterServo:
     def _set_pwm(self, print_time, value):
         if value == self.last_value:
             return "discard", 0.
+        aligned_ptime = self.mcu_servo.next_aligned_print_time(print_time,
+                                                               RESCHEDULE_SLACK)
+        if aligned_ptime > print_time + RESCHEDULE_SLACK:
+            return "reschedule", aligned_ptime
         self.last_value = value
-        self.mcu_servo.set_pwm(print_time, value)
+        self.mcu_servo.set_pwm(aligned_ptime, value)
     def _get_pwm_from_angle(self, angle):
         angle = max(0., min(self.max_angle, angle))
         width = self.min_width + angle * self.angle_to_width

klippy/extras/sx1509.py

@@ -178,6 +178,8 @@ class SX1509_pwm(object):
         self._shutdown_value = max(0., min(1., shutdown_value))
         self._sx1509.set_register(self._i_on_reg,
                                   ~int(255 * self._start_value) & 0xFF)
+    def next_aligned_print_time(self, print_time, allow_early=0.):
+        return print_time
     def set_pwm(self, print_time, value):
         self._sx1509.set_register(self._i_on_reg, ~int(255 * value)
                                   if not self._invert

klippy/mcu.py

@@ -422,6 +422,7 @@ class MCU_pwm:
         self._invert = pin_params['invert']
         self._start_value = self._shutdown_value = float(self._invert)
         self._last_clock = 0
+        self._last_value = .0
         self._pwm_max = 0.
         self._set_cmd = None
     def get_mcu(self):
@@ -437,6 +438,7 @@ class MCU_pwm:
             shutdown_value = 1. - shutdown_value
         self._start_value = max(0., min(1., start_value))
         self._shutdown_value = max(0., min(1., shutdown_value))
+        self._last_value = self._start_value
     def _build_config(self):
         if self._max_duration and self._start_value != self._shutdown_value:
             raise pins.error("Pin with max duration must have start"
@@ -488,6 +490,20 @@ class MCU_pwm:
             % (self._oid, self._last_clock, svalue), is_init=True)
         self._set_cmd = self._mcu.lookup_command(
             "queue_digital_out oid=%c clock=%u on_ticks=%u", cq=cmd_queue)
+    def next_aligned_print_time(self, print_time, allow_early=0.):
+        # Filter cases where there is no need to sync anything
+        if self._hardware_pwm:
+            return print_time
+        if self._last_value == 1. or self._last_value == .0:
+            return print_time
+        # Simplify the calling and allow scheduling slightly earlier
+        req_ptime = print_time - min(allow_early, 0.5 * self._cycle_time)
+        cycle_ticks = self._mcu.seconds_to_clock(self._cycle_time)
+        req_clock = self._mcu.print_time_to_clock(req_ptime)
+        last_clock = self._last_clock
+        pulses = (req_clock - last_clock + cycle_ticks - 1) // cycle_ticks
+        next_clock = last_clock + pulses * cycle_ticks
+        return self._mcu.clock_to_print_time(next_clock)
     def set_pwm(self, print_time, value):
         if self._invert:
             value = 1. - value
@@ -496,6 +512,7 @@ class MCU_pwm:
         self._set_cmd.send([self._oid, clock, v],
                            minclock=self._last_clock, reqclock=clock)
         self._last_clock = clock
+        self._last_value = value
 
 class MCU_adc:
     def __init__(self, mcu, pin_params):