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>

5 files changed, 158 insertions, 10 deletions

diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py
index 91929747..1d980a77 100644
--- a/klippy/chelper/__init__.py
+++ b/klippy/chelper/__init__.py
@@ -45,6 +45,11 @@ defs_stepcompress = """
 
 defs_itersolve = """
     int32_t itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m);
+    int32_t itersolve_generate_steps(struct stepper_kinematics *sk
+        , double flush_time);
+    double itersolve_check_active(struct stepper_kinematics *sk
+        , double flush_time);
+    void itersolve_set_trapq(struct stepper_kinematics *sk, struct trapq *tq);
     void itersolve_set_stepcompress(struct stepper_kinematics *sk
         , struct stepcompress *sc, double step_dist);
     double itersolve_calc_position_from_coord(struct stepper_kinematics *sk
diff --git a/klippy/chelper/itersolve.c b/klippy/chelper/itersolve.c
index 3f35f1d4..c84d25cc 100644
--- a/klippy/chelper/itersolve.c
+++ b/klippy/chelper/itersolve.c
@@ -5,6 +5,7 @@
 // This file may be distributed under the terms of the GNU GPLv3 license.
 
 #include <math.h> // fabs
+#include <stddef.h> // offsetof
 #include <string.h> // memset
 #include "compiler.h" // __visible
 #include "itersolve.h" // itersolve_gen_steps
@@ -53,15 +54,17 @@ itersolve_find_step(struct stepper_kinematics *sk, struct move *m
     return best_guess;
 }
 
-// Generate step times for a stepper during a move
-int32_t __visible
-itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m)
+// Generate step times for a portion of a move
+static int32_t
+itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m
+                          , double move_start, double move_end)
 {
     struct stepcompress *sc = sk->sc;
     sk_calc_callback calc_position_cb = sk->calc_position_cb;
     double half_step = .5 * sk->step_dist;
     double mcu_freq = stepcompress_get_mcu_freq(sc);
-    struct timepos last = { 0., sk->commanded_pos }, low = last, high = last;
+    double start = move_start - m->print_time, end = move_end - m->print_time;
+    struct timepos last = { start, sk->commanded_pos }, low = last, high = last;
     double seek_time_delta = 0.000100;
     int sdir = stepcompress_get_step_dir(sc);
     struct queue_append qa = queue_append_start(sc, m->print_time, .5);
@@ -70,15 +73,15 @@ itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m)
         double dist = high.position - last.position;
         if (fabs(dist) < half_step) {
         seek_new_high_range:
-            if (high.time >= m->move_t)
+            if (high.time >= end)
                 // At end of move
                 break;
             // Need to increase next step search range
             low = high;
             high.time = last.time + seek_time_delta;
             seek_time_delta += seek_time_delta;
-            if (high.time > m->move_t)
-                high.time = m->move_t;
+            if (high.time > end)
+                high.time = end;
             high.position = calc_position_cb(sk, m, high.time);
             continue;
         }
@@ -125,6 +128,87 @@ itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m)
     return 0;
 }
 
+// Generate step times for a move
+int32_t __visible
+itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m)
+{
+    return itersolve_gen_steps_range(sk, m, m->print_time
+                                     , m->print_time + m->move_t);
+}
+
+// Check if a move is likely to cause movement on a stepper
+static inline int
+check_active(struct stepper_kinematics *sk, struct move *m)
+{
+    int af = sk->active_flags;
+    return ((af & AF_X && m->axes_r.x != 0.)
+            || (af & AF_Y && m->axes_r.y != 0.)
+            || (af & AF_Z && m->axes_r.z != 0.));
+}
+
+// Generate step times for a range of moves on the trapq
+int32_t __visible
+itersolve_generate_steps(struct stepper_kinematics *sk, double flush_time)
+{
+    double last_flush_time = sk->last_flush_time;
+    sk->last_flush_time = flush_time;
+    if (!sk->tq || list_empty(&sk->tq->moves))
+        return 0;
+    struct move *m = list_first_entry(&sk->tq->moves, struct move, node);
+    for (;;) {
+        double move_print_time = m->print_time;
+        double move_end_time = move_print_time + m->move_t;
+        if (last_flush_time >= move_end_time) {
+            if (list_is_last(&m->node, &sk->tq->moves))
+                break;
+            m = list_next_entry(m, node);
+            continue;
+        }
+        double start = move_print_time, end = move_end_time;
+        if (start < last_flush_time)
+            start = last_flush_time;
+        if (start >= flush_time)
+            break;
+        if (end > flush_time)
+            end = flush_time;
+        if (check_active(sk, m)) {
+            int32_t ret = itersolve_gen_steps_range(sk, m, start, end);
+            if (ret)
+                return ret;
+        }
+        last_flush_time = end;
+    }
+    return 0;
+}
+
+// Check if the given stepper is likely to be active in the given time range
+double __visible
+itersolve_check_active(struct stepper_kinematics *sk, double flush_time)
+{
+    if (!sk->tq || list_empty(&sk->tq->moves))
+        return 0.;
+    struct move *m = list_first_entry(&sk->tq->moves, struct move, node);
+    while (sk->last_flush_time >= m->print_time + m->move_t) {
+        if (list_is_last(&m->node, &sk->tq->moves))
+            return 0.;
+        m = list_next_entry(m, node);
+    }
+    while (m->print_time < flush_time) {
+        if (check_active(sk, m))
+            return m->print_time;
+        if (list_is_last(&m->node, &sk->tq->moves))
+            return 0.;
+        m = list_next_entry(m, node);
+    }
+    return 0.;
+}
+
+void __visible
+itersolve_set_trapq(struct stepper_kinematics *sk, struct trapq *tq)
+{
+    sk->tq = tq;
+}
+
 void __visible
 itersolve_set_stepcompress(struct stepper_kinematics *sk
                            , struct stepcompress *sc, double step_dist)
diff --git a/klippy/chelper/itersolve.h b/klippy/chelper/itersolve.h
index da3a9fa7..e7548849 100644
--- a/klippy/chelper/itersolve.h
+++ b/klippy/chelper/itersolve.h
@@ -3,6 +3,10 @@
 
 #include <stdint.h> // int32_t
 
+enum {
+    AF_X = 1 << 0, AF_Y = 1 << 1, AF_Z = 1 <<2,
+};
+
 struct stepper_kinematics;
 struct move;
 typedef double (*sk_calc_callback)(struct stepper_kinematics *sk, struct move *m
@@ -11,11 +15,20 @@ typedef void (*sk_post_callback)(struct stepper_kinematics *sk);
 struct stepper_kinematics {
     double step_dist, commanded_pos;
     struct stepcompress *sc;
+
+    double last_flush_time;
+    struct trapq *tq;
+    int active_flags;
+
     sk_calc_callback calc_position_cb;
     sk_post_callback post_cb;
 };
 
 int32_t itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m);
+int32_t itersolve_generate_steps(struct stepper_kinematics *sk
+                                 , double flush_time);
+double itersolve_check_active(struct stepper_kinematics *sk, double flush_time);
+void itersolve_set_trapq(struct stepper_kinematics *sk, struct trapq *tq);
 void itersolve_set_stepcompress(struct stepper_kinematics *sk
                                 , struct stepcompress *sc, double step_dist);
 double itersolve_calc_position_from_coord(struct stepper_kinematics *sk
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):
diff --git a/klippy/stepper.py b/klippy/stepper.py
index 4617f97a..f3721b45 100644
--- a/klippy/stepper.py
+++ b/klippy/stepper.py
@@ -70,6 +70,7 @@ class PrinterStepper:
         mcu_stepper.setup_dir_pin(dir_pin_params)
         step_dist = config.getfloat('step_distance', above=0.)
         mcu_stepper.setup_step_distance(step_dist)
+        mcu_stepper.add_active_callback(self._stepper_active)
         self.enable = lookup_enable_pin(ppins, config.get('enable_pin', None))
         # Register STEPPER_BUZZ command
         force_move = printer.try_load_module(config, 'force_move')
@@ -77,6 +78,8 @@ class PrinterStepper:
         # Wrappers
         self.step_itersolve = mcu_stepper.step_itersolve
         self.setup_itersolve = mcu_stepper.setup_itersolve
+        self.generate_steps = mcu_stepper.generate_steps
+        self.set_trapq = mcu_stepper.set_trapq
         self.set_stepper_kinematics = mcu_stepper.set_stepper_kinematics
         self.set_ignore_move = mcu_stepper.set_ignore_move
         self.calc_position_from_coord = mcu_stepper.calc_position_from_coord
@@ -105,10 +108,15 @@ class PrinterStepper:
             2. * step_dist, max_halt_velocity, max_accel)
         min_stop_interval = second_last_step_time - last_step_time
         self.mcu_stepper.setup_min_stop_interval(min_stop_interval)
+    def _stepper_active(self, active_time):
+        self.motor_enable(active_time, 1)
     def motor_enable(self, print_time, enable=0):
         if self.need_motor_enable != (not enable):
             self.enable.set_enable(print_time, enable)
         self.need_motor_enable = not enable
+        if not enable:
+            # Enable stepper on future stepper movement
+            self.mcu_stepper.add_active_callback(self._stepper_active)
     def is_motor_enabled(self):
         return not self.need_motor_enable
 
@@ -213,6 +221,12 @@ class PrinterRail:
     def setup_itersolve(self, alloc_func, *params):
         for stepper in self.steppers:
             stepper.setup_itersolve(alloc_func, *params)
+    def generate_steps(self, flush_time):
+        for stepper in self.steppers:
+            stepper.generate_steps(flush_time)
+    def set_trapq(self, trapq):
+        for stepper in self.steppers:
+            stepper.set_trapq(trapq)
     def set_max_jerk(self, max_halt_velocity, max_accel):
         for stepper in self.steppers:
             stepper.set_max_jerk(max_halt_velocity, max_accel)