motion_queuing: Move flushing logic from toolhead to motion_queuing module

Move low-level step generation timing code to the motion_queing
module.  This helps simplify the toolhead module.  It also helps
centralize the step generation code into the motion_queing module.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

klippy/extras/force_move.py

@@ -76,7 +76,7 @@ class ForceMove:
         self.trapq_append(self.trapq, print_time, accel_t, cruise_t, accel_t,
                           0., 0., 0., axis_r, 0., 0., 0., cruise_v, accel)
         print_time = print_time + accel_t + cruise_t + accel_t
-        toolhead.note_mcu_movequeue_activity(print_time)
+        self.motion_queuing.note_mcu_movequeue_activity(print_time)
         toolhead.dwell(accel_t + cruise_t + accel_t)
         toolhead.flush_step_generation()
         stepper.set_trapq(prev_trapq)

klippy/extras/manual_stepper.py

@@ -72,8 +72,7 @@ class ManualStepper:
         self.sync_print_time()
         self.next_cmd_time = self._submit_move(self.next_cmd_time, movepos,
                                                speed, accel)
-        toolhead = self.printer.lookup_object('toolhead')
+        self.motion_queuing.note_mcu_movequeue_activity(self.next_cmd_time)
-        toolhead.note_mcu_movequeue_activity(self.next_cmd_time)
         if sync:
             self.sync_print_time()
     def do_homing_move(self, movepos, speed, accel, triggered, check_trigger):
@@ -201,8 +200,8 @@ class ManualStepper:
         end_time = self._submit_move(start_time, newpos[0],
                                      speed, self.homing_accel)
         # Drip updates to motors
-        toolhead = self.printer.lookup_object('toolhead')
+        self.motion_queuing.drip_update_time(start_time, end_time,
-        toolhead.drip_update_time(start_time, end_time, drip_completion)
+                                             drip_completion)
         # Clear trapq of any remaining parts of movement
         reactor = self.printer.get_reactor()
         self.motion_queuing.wipe_trapq(self.trapq)

klippy/extras/motion_queuing.py

@@ -6,12 +6,22 @@
 import logging
 import chelper
 
+BGFLUSH_LOW_TIME = 0.200
+BGFLUSH_BATCH_TIME = 0.200
+BGFLUSH_EXTRA_TIME = 0.250
 MOVE_HISTORY_EXPIRE = 30.
+MIN_KIN_TIME = 0.100
+MOVE_BATCH_TIME = 0.500
+STEPCOMPRESS_FLUSH_TIME = 0.050
 SDS_CHECK_TIME = 0.001 # step+dir+step filter in stepcompress.c
 
+DRIP_SEGMENT_TIME = 0.050
+DRIP_TIME = 0.100
+
 class PrinterMotionQueuing:
     def __init__(self, config):
-        self.printer = config.get_printer()
+        self.printer = printer = config.get_printer()
+        self.reactor = printer.get_reactor()
         # Low level C allocations
         self.trapqs = []
         self.stepcompress = []
@@ -26,10 +36,22 @@ class PrinterMotionQueuing:
         self.flush_callbacks = []
         # History expiration
         self.clear_history_time = 0.
-        is_debug = self.printer.get_start_args().get('debugoutput') is not None
+        # Flush tracking
-        self.is_debugoutput = is_debug
+        self.flush_timer = self.reactor.register_timer(self._flush_handler)
+        self.do_kick_flush_timer = True
+        self.last_flush_time = self.last_step_gen_time = 0.
+        self.need_flush_time = self.need_step_gen_time = 0.
+        self.check_flush_lookahead_cb = (lambda e: None)
+        # MCU tracking
+        self.all_mcus = [m for n, m in printer.lookup_objects(module='mcu')]
+        self.mcu = self.all_mcus[0]
+        self.can_pause = True
+        if self.mcu.is_fileoutput():
+            self.can_pause = False
         # Kinematic step generation scan window time tracking
         self.kin_flush_delay = SDS_CHECK_TIME
+        # Register handlers
+        printer.register_event_handler("klippy:shutdown", self._handle_shutdown)
     def allocate_trapq(self):
         ffi_main, ffi_lib = chelper.get_ffi()
         trapq = ffi_main.gc(ffi_lib.trapq_alloc(), ffi_lib.trapq_free)
@@ -60,7 +82,7 @@ class PrinterMotionQueuing:
             fcbs = list(self.flush_callbacks)
             fcbs.remove(callback)
             self.flush_callbacks = fcbs
-    def flush_motion_queues(self, must_flush_time, max_step_gen_time):
+    def _flush_motion_queues(self, must_flush_time, max_step_gen_time):
         # Invoke flush callbacks (if any)
         for cb in self.flush_callbacks:
             cb(must_flush_time, max_step_gen_time)
@@ -80,7 +102,7 @@ class PrinterMotionQueuing:
         # Determine maximum history to keep
         trapq_free_time = max_step_gen_time - self.kin_flush_delay
         clear_history_time = self.clear_history_time
-        if self.is_debugoutput:
+        if not self.can_pause:
             clear_history_time = trapq_free_time - MOVE_HISTORY_EXPIRE
         # Move processed trapq moves to history list, and expire old history
         for trapq in self.trapqs:
@@ -92,18 +114,109 @@ class PrinterMotionQueuing:
             self.steppersync_history_expire(ss, clock)
     def wipe_trapq(self, trapq):
         # Expire any remaining movement in the trapq (force to history list)
-        NEVER = 9999999999999999.
+        self.trapq_finalize_moves(trapq, self.reactor.NEVER, 0.)
-        self.trapq_finalize_moves(trapq, NEVER, 0.)
     def lookup_trapq_append(self):
         ffi_main, ffi_lib = chelper.get_ffi()
         return ffi_lib.trapq_append
     def set_step_generate_scan_time(self, delay):
         self.kin_flush_delay = delay
     def stats(self, eventtime):
-        mcu = self.printer.lookup_object('mcu')
+        # Hack to globally invoke mcu check_active()
-        est_print_time = mcu.estimated_print_time(eventtime)
+        for m in self.all_mcus:
+            m.check_active(self.last_step_gen_time, eventtime)
+        # Calculate history expiration
+        est_print_time = self.mcu.estimated_print_time(eventtime)
         self.clear_history_time = est_print_time - MOVE_HISTORY_EXPIRE
         return False, ""
+    # Flush tracking
+    def _handle_shutdown(self):
+        self.can_pause = False
+    def setup_lookahead_flush_callback(self, check_flush_lookahead_cb):
+        self.check_flush_lookahead_cb = check_flush_lookahead_cb
+    def advance_flush_time(self, target_time=None, lazy_target=False):
+        if target_time is None:
+            # This is a full flush
+            target_time = self.need_step_gen_time
+        want_flush_time = want_step_gen_time = target_time
+        if lazy_target:
+            # Account for step gen scan windows and optimize step compression
+            want_step_gen_time -= self.kin_flush_delay
+            want_flush_time = want_step_gen_time - STEPCOMPRESS_FLUSH_TIME
+        want_flush_time = max(want_flush_time, self.last_flush_time)
+        flush_time = self.last_flush_time
+        if want_flush_time > flush_time + 10. * MOVE_BATCH_TIME:
+            # Use closer startup time when coming out of idle state
+            curtime = self.reactor.monotonic()
+            est_print_time = self.mcu.estimated_print_time(curtime)
+            flush_time = max(flush_time, est_print_time)
+        while 1:
+            flush_time = min(flush_time + MOVE_BATCH_TIME, want_flush_time)
+            # Generate steps via itersolve
+            want_sg_wave = min(flush_time + STEPCOMPRESS_FLUSH_TIME,
+                               want_step_gen_time)
+            step_gen_time = max(self.last_step_gen_time, want_sg_wave,
+                                flush_time)
+            self._flush_motion_queues(flush_time, step_gen_time)
+            self.last_flush_time = flush_time
+            self.last_step_gen_time = step_gen_time
+            if flush_time >= want_flush_time:
+                break
+    def calc_step_gen_restart(self, est_print_time):
+        kin_time = max(est_print_time + MIN_KIN_TIME, self.last_step_gen_time)
+        return kin_time + self.kin_flush_delay
+    def _flush_handler(self, eventtime):
+        try:
+            # Check if flushing is done via lookahead queue
+            ret = self.check_flush_lookahead_cb(eventtime)
+            if ret is not None:
+                return ret
+            # Flush motion queues
+            est_print_time = self.mcu.estimated_print_time(eventtime)
+            while 1:
+                end_flush = self.need_flush_time + BGFLUSH_EXTRA_TIME
+                if self.last_flush_time >= end_flush:
+                    self.do_kick_flush_timer = True
+                    return self.reactor.NEVER
+                buffer_time = self.last_flush_time - est_print_time
+                if buffer_time > BGFLUSH_LOW_TIME:
+                    return eventtime + buffer_time - BGFLUSH_LOW_TIME
+                ftime = est_print_time + BGFLUSH_LOW_TIME + BGFLUSH_BATCH_TIME
+                self.advance_flush_time(min(end_flush, ftime))
+        except:
+            logging.exception("Exception in flush_handler")
+            self.printer.invoke_shutdown("Exception in flush_handler")
+        return self.reactor.NEVER
+    def note_mcu_movequeue_activity(self, mq_time, is_step_gen=True):
+        if is_step_gen:
+            mq_time += self.kin_flush_delay
+            self.need_step_gen_time = max(self.need_step_gen_time, mq_time)
+        self.need_flush_time = max(self.need_flush_time, mq_time)
+        if self.do_kick_flush_timer:
+            self.do_kick_flush_timer = False
+            self.reactor.update_timer(self.flush_timer, self.reactor.NOW)
+    def drip_update_time(self, start_time, end_time, drip_completion):
+        # Disable background flushing from timer
+        self.reactor.update_timer(self.flush_timer, self.reactor.NEVER)
+        self.do_kick_flush_timer = False
+        # Flush in segments until drip_completion signal
+        flush_delay = DRIP_TIME + STEPCOMPRESS_FLUSH_TIME + self.kin_flush_delay
+        flush_time = start_time
+        while flush_time < end_time:
+            if drip_completion.test():
+                break
+            curtime = self.reactor.monotonic()
+            est_print_time = self.mcu.estimated_print_time(curtime)
+            wait_time = flush_time - est_print_time - flush_delay
+            if wait_time > 0. and self.can_pause:
+                # Pause before sending more steps
+                drip_completion.wait(curtime + wait_time)
+                continue
+            flush_time = min(flush_time + DRIP_SEGMENT_TIME, end_time)
+            self.note_mcu_movequeue_activity(flush_time)
+            self.advance_flush_time(flush_time, lazy_target=True)
+        # Restore background flushing
+        self.reactor.update_timer(self.flush_timer, self.reactor.NOW)
+        self.advance_flush_time()
 
 def load_config(config):
     return PrinterMotionQueuing(config)

klippy/extras/output_pin.py

@@ -20,8 +20,8 @@ class GCodeRequestQueue:
         self.rqueue = []
         self.next_min_flush_time = 0.
         self.toolhead = None
-        motion_queuing = printer.load_object(config, 'motion_queuing')
+        self.motion_queuing = printer.load_object(config, 'motion_queuing')
-        motion_queuing.register_flush_callback(self._flush_notification)
+        self.motion_queuing.register_flush_callback(self._flush_notification)
         printer.register_event_handler("klippy:connect", self._handle_connect)
     def _handle_connect(self):
         self.toolhead = self.printer.lookup_object('toolhead')
@@ -51,11 +51,12 @@ class GCodeRequestQueue:
             del rqueue[:pos+1]
             self.next_min_flush_time = next_time + max(min_wait, min_sched_time)
             # Ensure following queue items are flushed
-            self.toolhead.note_mcu_movequeue_activity(self.next_min_flush_time,
+            self.motion_queuing.note_mcu_movequeue_activity(
-                                                      is_step_gen=False)
+                self.next_min_flush_time, is_step_gen=False)
     def _queue_request(self, print_time, value):
         self.rqueue.append((print_time, value))
-        self.toolhead.note_mcu_movequeue_activity(print_time, is_step_gen=False)
+        self.motion_queuing.note_mcu_movequeue_activity(
+            print_time, is_step_gen=False)
     def queue_gcode_request(self, value):
         self.toolhead.register_lookahead_callback(
             (lambda pt: self._queue_request(pt, value)))

klippy/extras/pwm_tool.py

@@ -16,8 +16,8 @@ class MCU_queued_pwm:
         self._max_duration = 2.
         self._oid = oid = mcu.create_oid()
         printer = mcu.get_printer()
-        motion_queuing = printer.load_object(config, 'motion_queuing')
+        self._motion_queuing = printer.load_object(config, 'motion_queuing')
-        self._stepqueue = motion_queuing.allocate_stepcompress(mcu, oid)
+        self._stepqueue = self._motion_queuing.allocate_stepcompress(mcu, oid)
         ffi_main, ffi_lib = chelper.get_ffi()
         self._stepcompress_queue_mq_msg = ffi_lib.stepcompress_queue_mq_msg
         mcu.register_config_callback(self._build_config)
@@ -62,8 +62,8 @@ class MCU_queued_pwm:
         if self._duration_ticks >= 1<<31:
             raise config_error("PWM pin max duration too large")
         if self._duration_ticks:
-            motion_queuing = printer.lookup_object('motion_queuing')
+            self._motion_queuing.register_flush_callback(
-            motion_queuing.register_flush_callback(self._flush_notification)
+                self._flush_notification)
         if self._hardware_pwm:
             self._pwm_max = self._mcu.get_constant_float("PWM_MAX")
             self._default_value = self._shutdown_value * self._pwm_max
@@ -116,8 +116,8 @@ class MCU_queued_pwm:
             # Continue flushing to resend time
             wakeclock += self._duration_ticks
         wake_print_time = self._mcu.clock_to_print_time(wakeclock)
-        self._toolhead.note_mcu_movequeue_activity(wake_print_time,
+        self._motion_queuing.note_mcu_movequeue_activity(wake_print_time,
-                                                   is_step_gen=False)
+                                                         is_step_gen=False)
     def set_pwm(self, print_time, value):
         clock = self._mcu.print_time_to_clock(print_time)
         if self._invert:

klippy/toolhead.py

@@ -193,25 +193,14 @@ class LookAheadQueue:
 BUFFER_TIME_LOW = 1.0
 BUFFER_TIME_HIGH = 2.0
 BUFFER_TIME_START = 0.250
-BGFLUSH_LOW_TIME = 0.200
-BGFLUSH_BATCH_TIME = 0.200
-BGFLUSH_EXTRA_TIME = 0.250
-MIN_KIN_TIME = 0.100
-MOVE_BATCH_TIME = 0.500
-STEPCOMPRESS_FLUSH_TIME = 0.050
 SDS_CHECK_TIME = 0.001 # step+dir+step filter in stepcompress.c
 
-DRIP_SEGMENT_TIME = 0.050
-DRIP_TIME = 0.100
-
 # Main code to track events (and their timing) on the printer toolhead
 class ToolHead:
     def __init__(self, config):
         self.printer = config.get_printer()
         self.reactor = self.printer.get_reactor()
-        self.all_mcus = [
+        self.mcu = self.printer.lookup_object('mcu')
-            m for n, m in self.printer.lookup_objects(module='mcu')]
-        self.mcu = self.all_mcus[0]
         self.lookahead = LookAheadQueue()
         self.lookahead.set_flush_time(BUFFER_TIME_HIGH)
         self.commanded_pos = [0., 0., 0., 0.]
@@ -236,16 +225,13 @@ class ToolHead:
         self.print_time = 0.
         self.special_queuing_state = "NeedPrime"
         self.priming_timer = None
-        # Flush tracking
-        self.flush_timer = self.reactor.register_timer(self._flush_handler)
-        self.do_kick_flush_timer = True
-        self.last_flush_time = self.last_step_gen_time = 0.
-        self.need_flush_time = self.need_step_gen_time = 0.
         # Kinematic step generation scan window time tracking
         self.kin_flush_delay = SDS_CHECK_TIME
         self.kin_flush_times = []
         # Setup for generating moves
         self.motion_queuing = self.printer.load_object(config, 'motion_queuing')
+        self.motion_queuing.setup_lookahead_flush_callback(
+            self._check_flush_lookahead)
         self.trapq = self.motion_queuing.allocate_trapq()
         self.trapq_append = self.motion_queuing.lookup_trapq_append()
         # Create kinematics class
@@ -268,46 +254,15 @@ class ToolHead:
         # Register handlers
         self.printer.register_event_handler("klippy:shutdown",
                                             self._handle_shutdown)
-    # Print time and flush tracking
+    # Print time tracking
-    def _advance_flush_time(self, target_time=None, lazy_target=False):
-        if target_time is None:
-            # This is a full flush
-            target_time = self.need_step_gen_time
-        want_flush_time = want_step_gen_time = target_time
-        if lazy_target:
-            # Account for step gen scan windows and optimize step compression
-            want_step_gen_time -= self.kin_flush_delay
-            want_flush_time = want_step_gen_time - STEPCOMPRESS_FLUSH_TIME
-        want_flush_time = max(want_flush_time, self.last_flush_time)
-        flush_time = self.last_flush_time
-        if want_flush_time > flush_time + 10. * MOVE_BATCH_TIME:
-            # Use closer startup time when coming out of idle state
-            curtime = self.reactor.monotonic()
-            est_print_time = self.mcu.estimated_print_time(curtime)
-            flush_time = max(flush_time, est_print_time)
-        flush_motion_queues = self.motion_queuing.flush_motion_queues
-        while 1:
-            flush_time = min(flush_time + MOVE_BATCH_TIME, want_flush_time)
-            # Generate steps via itersolve
-            want_sg_wave = min(flush_time + STEPCOMPRESS_FLUSH_TIME,
-                               want_step_gen_time)
-            step_gen_time = max(self.last_step_gen_time, want_sg_wave,
-                                flush_time)
-            flush_motion_queues(flush_time, step_gen_time)
-            self.last_flush_time = flush_time
-            self.last_step_gen_time = step_gen_time
-            if flush_time >= want_flush_time:
-                break
-    def _calc_step_gen_restart(self, est_print_time):
-        kin_time = max(est_print_time + MIN_KIN_TIME, self.last_step_gen_time)
-        return kin_time + self.kin_flush_delay
     def _advance_move_time(self, next_print_time):
         self.print_time = max(self.print_time, next_print_time)
-        self._advance_flush_time(self.print_time, lazy_target=True)
+        self.motion_queuing.advance_flush_time(self.print_time,
+                                               lazy_target=True)
     def _calc_print_time(self):
         curtime = self.reactor.monotonic()
         est_print_time = self.mcu.estimated_print_time(curtime)
-        kin_time = self._calc_step_gen_restart(est_print_time)
+        kin_time = self.motion_queuing.calc_step_gen_restart(est_print_time)
         min_print_time = max(est_print_time + BUFFER_TIME_START, kin_time)
         if min_print_time > self.print_time:
             self.print_time = min_print_time
@@ -341,7 +296,7 @@ class ToolHead:
             for cb in move.timing_callbacks:
                 cb(next_move_time)
         # Generate steps for moves
-        self.note_mcu_movequeue_activity(next_move_time)
+        self.motion_queuing.note_mcu_movequeue_activity(next_move_time)
         self._advance_move_time(next_move_time)
     def _flush_lookahead(self):
         # Transit from "NeedPrime"/"Priming"/main state to "NeedPrime"
@@ -352,7 +307,7 @@ class ToolHead:
         self.check_stall_time = 0.
     def flush_step_generation(self):
         self._flush_lookahead()
-        self._advance_flush_time()
+        self.motion_queuing.advance_flush_time()
     def get_last_move_time(self):
         if self.special_queuing_state:
             self._flush_lookahead()
@@ -418,28 +373,6 @@ class ToolHead:
         if print_time != self.print_time:
             self.check_stall_time = self.print_time
         return None
-    def _flush_handler(self, eventtime):
-        try:
-            # Check if flushing is done via lookahead queue
-            ret = self._check_flush_lookahead(eventtime)
-            if ret is not None:
-                return ret
-            # Flush motion queues
-            est_print_time = self.mcu.estimated_print_time(eventtime)
-            while 1:
-                end_flush = self.need_flush_time + BGFLUSH_EXTRA_TIME
-                if self.last_flush_time >= end_flush:
-                    self.do_kick_flush_timer = True
-                    return self.reactor.NEVER
-                buffer_time = self.last_flush_time - est_print_time
-                if buffer_time > BGFLUSH_LOW_TIME:
-                    return eventtime + buffer_time - BGFLUSH_LOW_TIME
-                ftime = est_print_time + BGFLUSH_LOW_TIME + BGFLUSH_BATCH_TIME
-                self._advance_flush_time(min(end_flush, ftime))
-        except:
-            logging.exception("Exception in flush_handler")
-            self.printer.invoke_shutdown("Exception in flush_handler")
-        return self.reactor.NEVER
     # Movement commands
     def get_position(self):
         return list(self.commanded_pos)
@@ -511,29 +444,6 @@ class ToolHead:
     def get_extra_axes(self):
         return [None, None, None] + self.extra_axes
     # Homing "drip move" handling
-    def drip_update_time(self, start_time, end_time, drip_completion):
-        # Disable background flushing from timer
-        self.reactor.update_timer(self.flush_timer, self.reactor.NEVER)
-        self.do_kick_flush_timer = False
-        # Flush in segments until drip_completion signal
-        flush_delay = DRIP_TIME + STEPCOMPRESS_FLUSH_TIME + self.kin_flush_delay
-        flush_time = start_time
-        while flush_time < end_time:
-            if drip_completion.test():
-                break
-            curtime = self.reactor.monotonic()
-            est_print_time = self.mcu.estimated_print_time(curtime)
-            wait_time = flush_time - est_print_time - flush_delay
-            if wait_time > 0. and self.can_pause:
-                # Pause before sending more steps
-                drip_completion.wait(curtime + wait_time)
-                continue
-            flush_time = min(flush_time + DRIP_SEGMENT_TIME, end_time)
-            self.note_mcu_movequeue_activity(flush_time)
-            self._advance_flush_time(flush_time, lazy_target=True)
-        # Restore background flushing
-        self.reactor.update_timer(self.flush_timer, self.reactor.NOW)
-        self._advance_flush_time()
     def _drip_load_trapq(self, submit_move):
         # Queue move into trapezoid motion queue (trapq)
         if submit_move.move_d:
@@ -563,13 +473,12 @@ class ToolHead:
         # Transmit move in "drip" mode
         self._process_lookahead()
         start_time, end_time = self._drip_load_trapq(move)
-        self.drip_update_time(start_time, end_time, drip_completion)
+        self.motion_queuing.drip_update_time(start_time, end_time,
+                                             drip_completion)
         # Move finished; cleanup any remnants on trapq
         self.motion_queuing.wipe_trapq(self.trapq)
     # Misc commands
     def stats(self, eventtime):
-        for m in self.all_mcus:
-            m.check_active(self.last_step_gen_time, eventtime)
         est_print_time = self.mcu.estimated_print_time(eventtime)
         buffer_time = self.print_time - est_print_time
         is_active = buffer_time > -60. or not self.special_queuing_state
@@ -616,14 +525,6 @@ class ToolHead:
             callback(self.get_last_move_time())
             return
         last_move.timing_callbacks.append(callback)
-    def note_mcu_movequeue_activity(self, mq_time, is_step_gen=True):
-        if is_step_gen:
-            mq_time += self.kin_flush_delay
-            self.need_step_gen_time = max(self.need_step_gen_time, mq_time)
-        self.need_flush_time = max(self.need_flush_time, mq_time)
-        if self.do_kick_flush_timer:
-            self.do_kick_flush_timer = False
-            self.reactor.update_timer(self.flush_timer, self.reactor.NOW)
     def get_max_velocity(self):
         return self.max_velocity, self.max_accel
     def _calc_junction_deviation(self):