diff --git a/klippy/extras/servo.py b/klippy/extras/servo.py
index 561f14b88..5bb585b23 100644
--- a/klippy/extras/servo.py
+++ b/klippy/extras/servo.py
@@ -17,6 +17,10 @@ class PrinterServo:
                                          below=SERVO_SIGNAL_PERIOD)
         self.max_angle = config.getfloat('maximum_servo_angle', 180.)
         self.signal_duration = config.getfloat('signal_duration', 0, minval=0.)
+        if self.signal_duration:
+            self.signal_duration = max(self.signal_duration, SERVO_SIGNAL_PERIOD)
+            if abs(self.signal_duration / SERVO_SIGNAL_PERIOD - round(self.signal_duration / SERVO_SIGNAL_PERIOD)) < 1e-9:
+                self.signal_duration -= 0.001
         self.steps_decomposed = config.getint('steps_decomposed', 0)
         self.angle_to_width = (self.max_width - self.min_width) / self.max_angle
         self.width_to_value = 1. / SERVO_SIGNAL_PERIOD
@@ -39,7 +43,7 @@ class PrinterServo:
         self.printer.register_event_handler("klippy:ready", self._handle_ready)
         # Create gcode request queue
         self.gcrq = output_pin.GCodeRequestQueue(
-            config, self.mcu_servo.get_mcu(), self._set_pwm)
+            config, self.mcu_servo.get_mcu(), self.__set_pwm)
         # Register commands
         servo_name = config.get_name().split()[1]
         gcode = self.printer.lookup_object('gcode')
@@ -48,39 +52,37 @@ class PrinterServo:
                                    desc=self.cmd_SET_SERVO_help)
     def get_status(self, eventtime):
         return {'value': self.last_value}
+    def __set_pwm(self, print_time, value):
+        if self.steps_decomposed:
+            return self._set_low_pwm(print_time, value)
+        else:
+            return self._set_pwm(print_time, value)
     def _set_pwm(self, print_time, value):
         if value == self.last_value:
             return "discard", 0.
         self.last_value = value
         self.mcu_servo.set_pwm(print_time, value)
-        self._handle_signal_duration(print_time)
+        if self.signal_duration:
+            self.mcu_servo.set_pwm(print_time + self.signal_duration, 0)
     def _get_s_curve_value(self, last_value, value, t):
         smooth_factor = t * t * (3 - 2 * t)
         return last_value + smooth_factor * (value - last_value)
     def _set_low_pwm(self, print_time, value):
         if value == self.last_value:
-            return
-        if self.last_value == 0:
-            self.last_value = self.initial_pwm
+            return "discard", 0.
+        enter_value = self.initial_pwm if self.last_value == 0 else self.last_value
+        self.last_value = value
         steps = self.steps_decomposed
         for step in range(steps):
             t = step / (steps - 1)
-            current_value = self._get_s_curve_value(self.last_value, value, t)
-            print_time = max(print_time, self.last_value_time + 0.02)
-            self.mcu_servo.set_pwm(print_time, current_value)
-            self.last_value_time = print_time
-        self.last_value = value
-        self._handle_signal_duration(print_time)
-    def _handle_signal_duration(self, print_time):
+            current_value = self._get_s_curve_value(enter_value, value, t)
+            next_time = print_time + SERVO_SIGNAL_PERIOD * step
+            self.mcu_servo.set_pwm(next_time, current_value)
         if self.signal_duration:
-            if abs(self.signal_duration / 0.02 - round(self.signal_duration / 0.02)) < 1e-9:
-                self.signal_duration -= 0.001
-            print_time = max(print_time + SERVO_SIGNAL_PERIOD, print_time + self.signal_duration)
-            self.mcu_servo.set_pwm(print_time, 0)
-            self.last_value_time = print_time
+            next_time = print_time + SERVO_SIGNAL_PERIOD * steps + self.signal_duration
+            self.mcu_servo.set_pwm(next_time, 0)
     def _handle_ready(self):
-        print_time = self.printer.lookup_object('toolhead').get_last_move_time()
-        self._set_pwm(print_time, self.initial_pwm)
+        self.gcrq.queue_gcode_request(self.initial_pwm)
     def _get_pwm_from_angle(self, angle):
         angle = max(0., min(self.max_angle, angle))
         width = self.min_width + angle * self.angle_to_width
@@ -91,19 +93,12 @@ class PrinterServo:
         return width * self.width_to_value
     cmd_SET_SERVO_help = "Set servo angle"
     def cmd_SET_SERVO(self, gcmd):
-        print_time = max(print_time, self.last_value_time)
         width = gcmd.get_float('WIDTH', None)
         if width is not None:
-            if self.steps_decomposed:
-                self._set_low_pwm(print_time, self._get_pwm_from_pulse_width(width))
-            else:
-                value = self._get_pwm_from_pulse_width(width)
+            value = self._get_pwm_from_pulse_width(width)
         else:
             angle = gcmd.get_float('ANGLE')
-            if self.steps_decomposed:
-                self._set_low_pwm(print_time, self._get_pwm_from_angle(angle))
-            else:
-                value = self._get_pwm_from_angle(angle)
+            value = self._get_pwm_from_angle(angle)
         self.gcrq.queue_gcode_request(value)
 
 def load_config_prefix(config):