itersolve: Support step generation in lead up to and after stepper activity

Add support for generating steps from kinematic functions that
calculate step times based on a range of the motion queue.  It
requires scanning for step generation during the lead up to stepper
activity (when the stepper would nominally be idle).  And it requires
scanning for step generation just after a stepper has nominally become
idle.

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

2 files changed, 28 insertions, 6 deletions

diff --git a/klippy/chelper/itersolve.c b/klippy/chelper/itersolve.c
index 84fd5f71..570ad2df 100644
--- a/klippy/chelper/itersolve.c
+++ b/klippy/chelper/itersolve.c
@@ -150,21 +150,41 @@ itersolve_generate_steps(struct stepper_kinematics *sk, double flush_time)
     struct move *m = list_first_entry(&sk->tq->moves, struct move, node);
     while (last_flush_time >= m->print_time + m->move_t)
         m = list_next_entry(m, node);
+    double force_steps_time = sk->last_move_time + sk->scan_past;
     for (;;) {
+        if (last_flush_time >= flush_time)
+            return 0;
         double start = m->print_time, end = start + m->move_t;
         if (start < last_flush_time)
             start = last_flush_time;
-        if (start >= flush_time)
-            return 0;
         if (end > flush_time)
             end = flush_time;
         if (check_active(sk, m)) {
+            if (sk->scan_future && start > last_flush_time) {
+                // Must generate steps leading up to stepper activity
+                force_steps_time = start;
+                if (last_flush_time < start - sk->scan_future)
+                    last_flush_time = start - sk->scan_future;
+                while (m->print_time > last_flush_time)
+                    m = list_prev_entry(m, node);
+                continue;
+            }
+            // Generate steps for this move
             int32_t ret = itersolve_gen_steps_range(sk, m, start, end);
             if (ret)
                 return ret;
+            sk->last_move_time = last_flush_time = end;
+            force_steps_time = end + sk->scan_past;
+        } else if (start < force_steps_time) {
+            // Must generates steps just past stepper activity
+            if (end > force_steps_time)
+                end = force_steps_time;
+            int32_t ret = itersolve_gen_steps_range(sk, m, start, end);
+            if (ret)
+                return ret;
+            last_flush_time = end;
         }
-        last_flush_time = end;
-        if (flush_time <= m->print_time + m->move_t)
+        if (flush_time + sk->scan_future <= m->print_time + m->move_t)
             return 0;
         m = list_next_entry(m, node);
     }
@@ -212,7 +232,8 @@ itersolve_calc_position_from_coord(struct stepper_kinematics *sk
     m.start_pos.x = x;
     m.start_pos.y = y;
     m.start_pos.z = z;
-    return sk->calc_position_cb(sk, &m, 0.);
+    m.move_t = 1000.;
+    return sk->calc_position_cb(sk, &m, 500.);
 }
 
 void __visible
diff --git a/klippy/chelper/itersolve.h b/klippy/chelper/itersolve.h
index 1615fc32..b7ab771a 100644
--- a/klippy/chelper/itersolve.h
+++ b/klippy/chelper/itersolve.h
@@ -16,7 +16,8 @@ struct stepper_kinematics {
     double step_dist, commanded_pos;
     struct stepcompress *sc;
 
-    double last_flush_time;
+    double last_flush_time, last_move_time;
+    double scan_past, scan_future;
     struct trapq *tq;
     int active_flags;