diff --git a/klippy/extras/output_pin.py b/klippy/extras/output_pin.py
index 3d7a952c1..2c5cae9ce 100644
--- a/klippy/extras/output_pin.py
+++ b/klippy/extras/output_pin.py
@@ -32,12 +32,18 @@ class GCodeRequestQueue:
                 pos += 1
             req_pt, req_val = rqueue[pos]
             # Invoke callback for the request
-            want_dequeue, min_wait_time = self.callback(next_time, req_val)
-            self.next_min_flush_time = next_time + max(min_wait_time,
-                                                       PIN_MIN_TIME)
-            if want_dequeue:
-                pos += 1
-            del rqueue[:pos]
+            min_wait = 0.
+            ret = self.callback(next_time, req_val)
+            if ret is not None:
+                # Handle special cases
+                action, min_wait = ret
+                if action == "discard":
+                    del rqueue[:pos+1]
+                    continue
+                if action == "delay":
+                    pos -= 1
+            del rqueue[:pos+1]
+            self.next_min_flush_time = next_time + max(min_wait, PIN_MIN_TIME)
             # Ensure following queue items are flushed
             self.toolhead.note_mcu_movequeue_activity(self.next_min_flush_time)
     def queue_request(self, print_time, value):
@@ -82,13 +88,13 @@ class PrinterOutputPin:
     def get_status(self, eventtime):
         return {'value': self.last_value}
     def _set_pin(self, print_time, value):
-        if value != self.last_value:
-            self.last_value = value
-            if self.is_pwm:
-                self.mcu_pin.set_pwm(print_time, value)
-            else:
-                self.mcu_pin.set_digital(print_time, value)
-        return (True, 0.)
+        if value == self.last_value:
+            return "discard", 0.
+        self.last_value = value
+        if self.is_pwm:
+            self.mcu_pin.set_pwm(print_time, value)
+        else:
+            self.mcu_pin.set_digital(print_time, value)
     cmd_SET_PIN_help = "Set the value of an output pin"
     def cmd_SET_PIN(self, gcmd):
         # Read requested value
diff --git a/klippy/extras/servo.py b/klippy/extras/servo.py
index 10537e673..f1ce99763 100644
--- a/klippy/extras/servo.py
+++ b/klippy/extras/servo.py
@@ -45,10 +45,10 @@ class PrinterServo:
     def get_status(self, eventtime):
         return {'value': self.last_value}
     def _set_pwm(self, print_time, value):
-        if value != self.last_value:
-            self.last_value = value
-            self.mcu_servo.set_pwm(print_time, value)
-        return (True, 0.)
+        if value == self.last_value:
+            return "discard", 0.
+        self.last_value = value
+        self.mcu_servo.set_pwm(print_time, 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