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;