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 <[email protected]>

klippy/chelper/__init__.py

@@ -60,7 +60,8 @@
     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 @@
         , 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

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;
 }

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

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)) {

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

klippy/extras/force_move.py

@@ -86,7 +86,8 @@ def manual_move(self, stepper, dist, speed, accel=0.):
                           0., 0., 0., axis_r, 0., 0., 0., cruise_v, accel)
         print_time = print_time + accel_t + cruise_t + accel_t
         stepper.generate_steps(print_time)
-        self.trapq_finalize_moves(self.trapq, print_time + 99999.9)
+        self.trapq_finalize_moves(self.trapq, print_time + 99999.9,
+                                  print_time + 99999.9)
         stepper.set_trapq(prev_trapq)
         stepper.set_stepper_kinematics(prev_sk)
         toolhead.note_kinematic_activity(print_time)

klippy/extras/manual_stepper.py

@@ -67,7 +67,8 @@ def do_move(self, movepos, speed, accel, sync=True):
                           0., cruise_v, accel)
         self.next_cmd_time = self.next_cmd_time + accel_t + cruise_t + accel_t
         self.rail.generate_steps(self.next_cmd_time)
-        self.trapq_finalize_moves(self.trapq, self.next_cmd_time + 99999.9)
+        self.trapq_finalize_moves(self.trapq, self.next_cmd_time + 99999.9,
+                                  self.next_cmd_time + 99999.9)
         toolhead = self.printer.lookup_object('toolhead')
         toolhead.note_kinematic_activity(self.next_cmd_time)
         if sync:

klippy/kinematics/extruder.py

@@ -211,8 +211,8 @@ def __init__(self, config, extruder_num):
         gcode.register_mux_command("ACTIVATE_EXTRUDER", "EXTRUDER",
                                    self.name, self.cmd_ACTIVATE_EXTRUDER,
                                    desc=self.cmd_ACTIVATE_EXTRUDER_help)
-    def update_move_time(self, flush_time):
-        self.trapq_finalize_moves(self.trapq, flush_time)
+    def update_move_time(self, flush_time, clear_history_time):
+        self.trapq_finalize_moves(self.trapq, flush_time, clear_history_time)
     def get_status(self, eventtime):
         sts = self.heater.get_status(eventtime)
         sts['can_extrude'] = self.heater.can_extrude
@@ -313,7 +313,7 @@ def cmd_ACTIVATE_EXTRUDER(self, gcmd):
 class DummyExtruder:
     def __init__(self, printer):
         self.printer = printer
-    def update_move_time(self, flush_time):
+    def update_move_time(self, flush_time, clear_history_time):
         pass
     def check_move(self, move):
         raise move.move_error("Extrude when no extruder present")

klippy/mcu.py

@@ -955,15 +955,18 @@ def request_move_queue_slot(self):
         self._reserved_move_slots += 1
     def register_flush_callback(self, callback):
         self._flush_callbacks.append(callback)
-    def flush_moves(self, print_time):
+    def flush_moves(self, print_time, clear_history_time):
         if self._steppersync is None:
             return
         clock = self.print_time_to_clock(print_time)
         if clock < 0:
             return
         for cb in self._flush_callbacks:
             cb(print_time, clock)
-        ret = self._ffi_lib.steppersync_flush(self._steppersync, clock)
+        clear_history_clock = \
+            max(0, self.print_time_to_clock(clear_history_time))
+        ret = self._ffi_lib.steppersync_flush(self._steppersync, clock,
+                                              clear_history_clock)
         if ret:
             raise error("Internal error in MCU '%s' stepcompress"
                         % (self._name,))

klippy/toolhead.py

@@ -195,6 +195,7 @@ def add_move(self, move):
 MOVE_BATCH_TIME = 0.500
 STEPCOMPRESS_FLUSH_TIME = 0.050
 SDS_CHECK_TIME = 0.001 # step+dir+step filter in stepcompress.c
+MOVE_HISTORY_EXPIRE = 30.
 
 DRIP_SEGMENT_TIME = 0.050
 DRIP_TIME = 0.100
@@ -289,13 +290,15 @@ def _advance_flush_time(self, flush_time):
         for sg in self.step_generators:
             sg(sg_flush_time)
         self.last_sg_flush_time = sg_flush_time
+        clear_history_time = self.mcu.estimated_print_time(
+            self.reactor.monotonic() - MOVE_HISTORY_EXPIRE)
         # Free trapq entries that are no longer needed
         free_time = sg_flush_time - self.kin_flush_delay
-        self.trapq_finalize_moves(self.trapq, free_time)
-        self.extruder.update_move_time(free_time)
+        self.trapq_finalize_moves(self.trapq, free_time, clear_history_time)
+        self.extruder.update_move_time(free_time, clear_history_time)
         # Flush stepcompress and mcu steppersync
         for m in self.all_mcus:
-            m.flush_moves(flush_time)
+            m.flush_moves(flush_time, clear_history_time)
         self.last_flush_time = flush_time
     def _advance_move_time(self, next_print_time):
         pt_delay = self.kin_flush_delay + STEPCOMPRESS_FLUSH_TIME
@@ -522,7 +525,7 @@ def drip_move(self, newpos, speed, drip_completion):
             self.move_queue.flush()
         except DripModeEndSignal as e:
             self.move_queue.reset()
-            self.trapq_finalize_moves(self.trapq, self.reactor.NEVER)
+            self.trapq_finalize_moves(self.trapq, self.reactor.NEVER, 0)
         # Exit "Drip" state
         self.reactor.update_timer(self.flush_timer, self.reactor.NOW)
         self.flush_step_generation()