toolhead: Keep stepcompress move history relative to current time (#6439)

Expire history relative to current time rather than last move in history queue

Signed-off-by: Francois Chagnon <fc@francoischagnon.net>

5 files changed, 35 insertions, 16 deletions

diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py
index 290234c5..e4199561 100644
--- a/klippy/chelper/__init__.py
+++ b/klippy/chelper/__init__.py
@@ -60,7 +60,8 @@ defs_stepcompress = """
     void steppersync_free(struct steppersync *ss);
     void steppersync_set_time(struct steppersync *ss
         , double time_offset, double mcu_freq);
-    int steppersync_flush(struct steppersync *ss, uint64_t move_clock);
+    int steppersync_flush(struct steppersync *ss, uint64_t move_clock
+        , uint64_t clear_history_clock);
 """
 
 defs_itersolve = """
@@ -94,7 +95,8 @@ defs_trapq = """
         , double start_pos_x, double start_pos_y, double start_pos_z
         , double axes_r_x, double axes_r_y, double axes_r_z
         , double start_v, double cruise_v, double accel);
-    void trapq_finalize_moves(struct trapq *tq, double print_time);
+    void trapq_finalize_moves(struct trapq *tq, double print_time
+        , double clear_history_time);
     void trapq_set_position(struct trapq *tq, double print_time
         , double pos_x, double pos_y, double pos_z);
     int trapq_extract_old(struct trapq *tq, struct pull_move *p, int max
diff --git a/klippy/chelper/stepcompress.c b/klippy/chelper/stepcompress.c
index e5514b95..310f2bf3 100644
--- a/klippy/chelper/stepcompress.c
+++ b/klippy/chelper/stepcompress.c
@@ -54,8 +54,6 @@ struct step_move {
     int16_t add;
 };
 
-#define HISTORY_EXPIRE (30.0)
-
 struct history_steps {
     struct list_node node;
     uint64_t first_clock, last_clock;
@@ -292,6 +290,13 @@ free_history(struct stepcompress *sc, uint64_t end_clock)
     }
 }
 
+// Expire the stepcompress history older than the given clock
+static void
+stepcompress_history_expire(struct stepcompress *sc, uint64_t end_clock)
+{
+    free_history(sc, end_clock);
+}
+
 // Free memory associated with a 'stepcompress' object
 void __visible
 stepcompress_free(struct stepcompress *sc)
@@ -322,9 +327,6 @@ calc_last_step_print_time(struct stepcompress *sc)
 {
     double lsc = sc->last_step_clock;
     sc->last_step_print_time = sc->mcu_time_offset + (lsc - .5) / sc->mcu_freq;
-
-    if (lsc > sc->mcu_freq * HISTORY_EXPIRE)
-        free_history(sc, lsc - sc->mcu_freq * HISTORY_EXPIRE);
 }
 
 // Set the conversion rate of 'print_time' to mcu clock
@@ -731,6 +733,18 @@ steppersync_set_time(struct steppersync *ss, double time_offset
     }
 }
 
+// Expire the stepcompress history before the given clock time
+static void
+steppersync_history_expire(struct steppersync *ss, uint64_t end_clock)
+{
+    int i;
+    for (i = 0; i < ss->sc_num; i++)
+    {
+        struct stepcompress *sc = ss->sc_list[i];
+        stepcompress_history_expire(sc, end_clock);
+    }
+}
+
 // Implement a binary heap algorithm to track when the next available
 // 'struct move' in the mcu will be available
 static void
@@ -758,7 +772,8 @@ heap_replace(struct steppersync *ss, uint64_t req_clock)
 
 // Find and transmit any scheduled steps prior to the given 'move_clock'
 int __visible
-steppersync_flush(struct steppersync *ss, uint64_t move_clock)
+steppersync_flush(struct steppersync *ss, uint64_t move_clock
+                  , uint64_t clear_history_clock)
 {
     // Flush each stepcompress to the specified move_clock
     int i;
@@ -806,5 +821,7 @@ steppersync_flush(struct steppersync *ss, uint64_t move_clock)
     // Transmit commands
     if (!list_empty(&msgs))
         serialqueue_send_batch(ss->sq, ss->cq, &msgs);
+
+    steppersync_history_expire(ss, clear_history_clock);
     return 0;
 }
diff --git a/klippy/chelper/stepcompress.h b/klippy/chelper/stepcompress.h
index bfc0dfcd..c5b40383 100644
--- a/klippy/chelper/stepcompress.h
+++ b/klippy/chelper/stepcompress.h
@@ -42,6 +42,7 @@ struct steppersync *steppersync_alloc(
 void steppersync_free(struct steppersync *ss);
 void steppersync_set_time(struct steppersync *ss, double time_offset
                           , double mcu_freq);
-int steppersync_flush(struct steppersync *ss, uint64_t move_clock);
+int steppersync_flush(struct steppersync *ss, uint64_t move_clock
+                      , uint64_t clear_history_clock);
 
 #endif // stepcompress.h
diff --git a/klippy/chelper/trapq.c b/klippy/chelper/trapq.c
index 9b1b501b..b9930e99 100644
--- a/klippy/chelper/trapq.c
+++ b/klippy/chelper/trapq.c
@@ -163,11 +163,10 @@ trapq_append(struct trapq *tq, double print_time
     }
 }
 
-#define HISTORY_EXPIRE (30.0)
-
 // Expire any moves older than `print_time` from the trapezoid velocity queue
 void __visible
-trapq_finalize_moves(struct trapq *tq, double print_time)
+trapq_finalize_moves(struct trapq *tq, double print_time
+                     , double clear_history_time)
 {
     struct move *head_sentinel = list_first_entry(&tq->moves, struct move,node);
     struct move *tail_sentinel = list_last_entry(&tq->moves, struct move, node);
@@ -190,10 +189,9 @@ trapq_finalize_moves(struct trapq *tq, double print_time)
     if (list_empty(&tq->history))
         return;
     struct move *latest = list_first_entry(&tq->history, struct move, node);
-    double expire_time = latest->print_time + latest->move_t - HISTORY_EXPIRE;
     for (;;) {
         struct move *m = list_last_entry(&tq->history, struct move, node);
-        if (m == latest || m->print_time + m->move_t > expire_time)
+        if (m == latest || m->print_time + m->move_t > clear_history_time)
             break;
         list_del(&m->node);
         free(m);
@@ -206,7 +204,7 @@ trapq_set_position(struct trapq *tq, double print_time
                    , double pos_x, double pos_y, double pos_z)
 {
     // Flush all moves from trapq
-    trapq_finalize_moves(tq, NEVER_TIME);
+    trapq_finalize_moves(tq, NEVER_TIME, 0);
 
     // Prune any moves in the trapq history that were interrupted
     while (!list_empty(&tq->history)) {
diff --git a/klippy/chelper/trapq.h b/klippy/chelper/trapq.h
index bd8f4e8c..c463f0c5 100644
--- a/klippy/chelper/trapq.h
+++ b/klippy/chelper/trapq.h
@@ -43,7 +43,8 @@ void trapq_append(struct trapq *tq, double print_time
                   , double start_pos_x, double start_pos_y, double start_pos_z
                   , double axes_r_x, double axes_r_y, double axes_r_z
                   , double start_v, double cruise_v, double accel);
-void trapq_finalize_moves(struct trapq *tq, double print_time);
+void trapq_finalize_moves(struct trapq *tq, double print_time
+                          , double clear_history_time);
 void trapq_set_position(struct trapq *tq, double print_time
                         , double pos_x, double pos_y, double pos_z);
 int trapq_extract_old(struct trapq *tq, struct pull_move *p, int max