motion_queuing: Track all trapqs and globally flush all trapqs

Add an allocate_trapq() helper function to facilitate the creation of
a low-level C trapq object.  Track all trapq objects and clear history
on them globally when the main motion queues are flushed.

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

klippy/extras/force_move.py

@@ -33,10 +33,10 @@ class ForceMove:
         self.printer = config.get_printer()
         self.steppers = {}
         # Setup iterative solver
+        self.motion_queuing = self.printer.load_object(config, 'motion_queuing')
+        self.trapq = self.motion_queuing.allocate_trapq()
+        self.trapq_append = self.motion_queuing.lookup_trapq_append()
         ffi_main, ffi_lib = chelper.get_ffi()
-        self.trapq = ffi_main.gc(ffi_lib.trapq_alloc(), ffi_lib.trapq_free)
-        self.trapq_append = ffi_lib.trapq_append
-        self.trapq_finalize_moves = ffi_lib.trapq_finalize_moves
         self.stepper_kinematics = ffi_main.gc(
             ffi_lib.cartesian_stepper_alloc(b'x'), ffi_lib.free)
         # Register commands
@@ -90,8 +90,7 @@ class ForceMove:
         toolhead.flush_step_generation()
         stepper.set_trapq(prev_trapq)
         stepper.set_stepper_kinematics(prev_sk)
-        self.trapq_finalize_moves(self.trapq, print_time + 99999.9,
+        self.motion_queuing.wipe_trapq(self.trapq)
-                                  print_time + 99999.9)
     def _lookup_stepper(self, gcmd):
         name = gcmd.get('STEPPER')
         if name not in self.steppers:

klippy/extras/manual_stepper.py

@@ -25,10 +25,9 @@ class ManualStepper:
         self.pos_min = config.getfloat('position_min', None)
         self.pos_max = config.getfloat('position_max', None)
         # Setup iterative solver
-        ffi_main, ffi_lib = chelper.get_ffi()
+        self.motion_queuing = self.printer.load_object(config, 'motion_queuing')
-        self.trapq = ffi_main.gc(ffi_lib.trapq_alloc(), ffi_lib.trapq_free)
+        self.trapq = self.motion_queuing.allocate_trapq()
-        self.trapq_append = ffi_lib.trapq_append
+        self.trapq_append = self.motion_queuing.lookup_trapq_append()
-        self.trapq_finalize_moves = ffi_lib.trapq_finalize_moves
         self.rail.setup_itersolve('cartesian_stepper_alloc', b'x')
         self.rail.set_trapq(self.trapq)
         # Registered with toolhead as an axtra axis
@@ -76,8 +75,6 @@ class ManualStepper:
         self.sync_print_time()
         self.next_cmd_time = self._submit_move(self.next_cmd_time, movepos,
                                                speed, accel)
-        self.trapq_finalize_moves(self.trapq, self.next_cmd_time + 99999.9,
-                                  self.next_cmd_time + 99999.9)
         toolhead = self.printer.lookup_object('toolhead')
         toolhead.note_mcu_movequeue_activity(self.next_cmd_time)
         if sync:
@@ -208,7 +205,7 @@ class ManualStepper:
         toolhead.drip_update_time(maxtime, drip_completion)
         # Clear trapq of any remaining parts of movement
         reactor = self.printer.get_reactor()
-        self.trapq_finalize_moves(self.trapq, reactor.NEVER, 0)
+        self.motion_queuing.wipe_trapq(self.trapq)
         self.rail.set_position([newpos[0], 0., 0.])
         self.sync_print_time()
     def get_kinematics(self):

klippy/extras/motion_queuing.py

@@ -4,16 +4,36 @@
 #
 # This file may be distributed under the terms of the GNU GPLv3 license.
 import logging
+import chelper
 
 class PrinterMotionQueuing:
     def __init__(self, config):
         self.printer = config.get_printer()
         self.steppers = []
+        self.trapqs = []
+        ffi_main, ffi_lib = chelper.get_ffi()
+        self.trapq_finalize_moves = ffi_lib.trapq_finalize_moves
+    def allocate_trapq(self):
+        ffi_main, ffi_lib = chelper.get_ffi()
+        trapq = ffi_main.gc(ffi_lib.trapq_alloc(), ffi_lib.trapq_free)
+        self.trapqs.append(trapq)
+        return trapq
     def register_stepper(self, config, stepper):
         self.steppers.append(stepper)
     def flush_motion_queues(self, must_flush_time, max_step_gen_time):
         for stepper in self.steppers:
             stepper.generate_steps(max_step_gen_time)
+    def clean_motion_queues(self, trapq_free_time, clear_history_time):
+        for trapq in self.trapqs:
+            self.trapq_finalize_moves(trapq, trapq_free_time,
+                                      clear_history_time)
+    def wipe_trapq(self, trapq):
+        # Expire any remaining movement in the trapq (force to history list)
+        NEVER = 9999999999999999.
+        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 load_config(config):
     return PrinterMotionQueuing(config)

klippy/kinematics/extruder.py

@@ -163,10 +163,9 @@ class PrinterExtruder:
         self.instant_corner_v = config.getfloat(
             'instantaneous_corner_velocity', 1., minval=0.)
         # Setup extruder trapq (trapezoidal motion queue)
-        ffi_main, ffi_lib = chelper.get_ffi()
+        self.motion_queuing = self.printer.load_object(config, 'motion_queuing')
-        self.trapq = ffi_main.gc(ffi_lib.trapq_alloc(), ffi_lib.trapq_free)
+        self.trapq = self.motion_queuing.allocate_trapq()
-        self.trapq_append = ffi_lib.trapq_append
+        self.trapq_append = self.motion_queuing.lookup_trapq_append()
-        self.trapq_finalize_moves = ffi_lib.trapq_finalize_moves
         # Setup extruder stepper
         self.extruder_stepper = None
         if (config.get('step_pin', None) is not None

klippy/toolhead.py

@@ -245,14 +245,10 @@ class ToolHead:
         # Kinematic step generation scan window time tracking
         self.kin_flush_delay = SDS_CHECK_TIME
         self.kin_flush_times = []
-        # Setup iterative solver
+        # Setup for generating moves
-        ffi_main, ffi_lib = chelper.get_ffi()
-        self.trapq = ffi_main.gc(ffi_lib.trapq_alloc(), ffi_lib.trapq_free)
-        self.trapq_append = ffi_lib.trapq_append
-        self.trapq_finalize_moves = ffi_lib.trapq_finalize_moves
-        # Motion flushing
         self.motion_queuing = self.printer.load_object(config, 'motion_queuing')
-        self.flush_trapqs = [self.trapq]
+        self.trapq = self.motion_queuing.allocate_trapq()
+        self.trapq_append = self.motion_queuing.lookup_trapq_append()
         # Create kinematics class
         gcode = self.printer.lookup_object('gcode')
         self.Coord = gcode.Coord
@@ -287,8 +283,7 @@ class ToolHead:
         if not self.can_pause:
             clear_history_time = flush_time - MOVE_HISTORY_EXPIRE
         free_time = sg_flush_time - self.kin_flush_delay
-        for trapq in self.flush_trapqs:
+        self.motion_queuing.clean_motion_queues(free_time, clear_history_time)
-            self.trapq_finalize_moves(trapq, free_time, clear_history_time)
         # Flush stepcompress and mcu steppersync
         for m in self.all_mcus:
             m.flush_moves(flush_time, clear_history_time)
@@ -484,32 +479,20 @@ class ToolHead:
             eventtime = self.reactor.pause(eventtime + 0.100)
     def set_extruder(self, extruder, extrude_pos):
         # XXX - should use add_extra_axis
-        prev_ea_trapq = self.extra_axes[0].get_trapq()
-        if prev_ea_trapq in self.flush_trapqs:
-            self.flush_trapqs.remove(prev_ea_trapq)
         self.extra_axes[0] = extruder
         self.commanded_pos[3] = extrude_pos
-        ea_trapq = extruder.get_trapq()
-        if ea_trapq is not None:
-            self.flush_trapqs.append(ea_trapq)
     def get_extruder(self):
         return self.extra_axes[0]
     def add_extra_axis(self, ea, axis_pos):
         self._flush_lookahead()
         self.extra_axes.append(ea)
         self.commanded_pos.append(axis_pos)
-        ea_trapq = ea.get_trapq()
-        if ea_trapq is not None:
-            self.flush_trapqs.append(ea_trapq)
         self.printer.send_event("toolhead:update_extra_axes")
     def remove_extra_axis(self, ea):
         self._flush_lookahead()
         if ea not in self.extra_axes:
             return
         ea_index = self.extra_axes.index(ea) + 3
-        ea_trapq = ea.get_trapq()
-        if ea_trapq in self.flush_trapqs:
-            self.flush_trapqs.remove(ea_trapq)
         self.commanded_pos.pop(ea_index)
         self.extra_axes.pop(ea_index - 3)
         self.printer.send_event("toolhead:update_extra_axes")
@@ -574,7 +557,7 @@ class ToolHead:
         next_move_time = self._drip_load_trapq(move)
         self.drip_update_time(next_move_time, drip_completion)
         # Move finished; cleanup any remnants on trapq
-        self.trapq_finalize_moves(self.trapq, self.reactor.NEVER, 0)
+        self.motion_queuing.wipe_trapq(self.trapq)
     # Misc commands
     def stats(self, eventtime):
         max_queue_time = max(self.print_time, self.last_flush_time)