itersolve: Add support for generating steps from a trapq

Support associating a stepper_kinematics with a trapq.  Support
generating steps from a time range on the given trapq.

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

1 files changed, 35 insertions, 3 deletions

diff --git a/klippy/mcu.py b/klippy/mcu.py
index 9864474d..ec490e65 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -21,11 +21,14 @@ class MCU_stepper:
         self._step_dist = 0.
         self._min_stop_interval = 0.
         self._reset_cmd_id = self._get_position_cmd = None
+        self._active_callbacks = []
         ffi_main, self._ffi_lib = chelper.get_ffi()
         self._stepqueue = ffi_main.gc(self._ffi_lib.stepcompress_alloc(oid),
                                       self._ffi_lib.stepcompress_free)
         self._mcu.register_stepqueue(self._stepqueue)
         self._stepper_kinematics = self._itersolve_gen_steps = None
+        self._itersolve_generate_steps = self._itersolve_check_active = None
+        self._trapq = ffi_main.NULL
         self.set_ignore_move(False)
     def get_mcu(self):
         return self._mcu
@@ -95,12 +98,16 @@ class MCU_stepper:
                 sk, self._stepqueue, self._step_dist)
         return old_sk
     def set_ignore_move(self, ignore_move):
-        was_ignore = (self._itersolve_gen_steps
-                      is not self._ffi_lib.itersolve_gen_steps)
+        fl = self._ffi_lib
+        was_ignore = self._itersolve_gen_steps is not fl.itersolve_gen_steps
         if ignore_move:
             self._itersolve_gen_steps = (lambda *args: 0)
+            self._itersolve_generate_steps = (lambda *args: 0)
+            self._itersolve_check_active = (lambda *args: 0.)
         else:
-            self._itersolve_gen_steps = self._ffi_lib.itersolve_gen_steps
+            self._itersolve_gen_steps = fl.itersolve_gen_steps
+            self._itersolve_generate_steps = fl.itersolve_generate_steps
+            self._itersolve_check_active = fl.itersolve_check_active
         return was_ignore
     def note_homing_end(self, did_trigger=False):
         ret = self._ffi_lib.stepcompress_reset(self._stepqueue, 0)
@@ -124,6 +131,31 @@ class MCU_stepper:
         ret = self._itersolve_gen_steps(self._stepper_kinematics, cmove)
         if ret:
             raise error("Internal error in stepcompress")
+    def set_trapq(self, tq):
+        if tq is None:
+            ffi_main, self._ffi_lib = chelper.get_ffi()
+            tq = ffi_main.NULL
+        self._ffi_lib.itersolve_set_trapq(self._stepper_kinematics, tq)
+        old_tq = self._trapq
+        self._trapq = tq
+        return old_tq
+    def add_active_callback(self, cb):
+        self._active_callbacks.append(cb)
+    def generate_steps(self, flush_time):
+        # Check for activity if necessary
+        if self._active_callbacks:
+            ret = self._itersolve_check_active(self._stepper_kinematics,
+                                               flush_time)
+            if ret:
+                cbs = self._active_callbacks
+                self._active_callbacks = []
+                for cb in cbs:
+                    cb(ret)
+        # Generate steps
+        ret = self._itersolve_generate_steps(self._stepper_kinematics,
+                                             flush_time)
+        if ret:
+            raise error("Internal error in stepcompress")
 
 class MCU_endstop:
     class TimeoutError(Exception):