From 56cd39f038f20b0e6c59516f51853a71bec8501c Mon Sep 17 00:00:00 2001
From: Kevin O'Connor <kevin@koconnor.net>
Date: Fri, 1 Nov 2019 22:21:30 -0400
Subject: [PATCH] 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>
---
 klippy/chelper/itersolve.c | 31 ++++++++++++++++++++++++++-----
 klippy/chelper/itersolve.h |  3 ++-
 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;