stepcompress: Add support for tracking history of queue_step commands

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

3 files changed, 130 insertions, 28 deletions

diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py
index 11808aef..4d199126 100644
--- a/klippy/chelper/__init__.py
+++ b/klippy/chelper/__init__.py
@@ -35,6 +35,10 @@ defs_stepcompress = """
         , int32_t set_next_step_dir_msgtag);
     void stepcompress_free(struct stepcompress *sc);
     int stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock);
+    int stepcompress_set_last_position(struct stepcompress *sc
+        , int64_t last_position);
+    int64_t stepcompress_find_past_position(struct stepcompress *sc
+        , uint64_t clock);
     int stepcompress_queue_msg(struct stepcompress *sc
         , uint32_t *data, int len);
 
diff --git a/klippy/chelper/stepcompress.c b/klippy/chelper/stepcompress.c
index cd803ae7..8622df21 100644
--- a/klippy/chelper/stepcompress.c
+++ b/klippy/chelper/stepcompress.c
@@ -14,6 +14,7 @@
 // This code is written in C (instead of python) for processing
 // efficiency - the repetitive integer math is vastly faster in C.
 
+#include <math.h> // sqrt
 #include <stddef.h> // offsetof
 #include <stdint.h> // uint32_t
 #include <stdio.h> // fprintf
@@ -42,6 +43,25 @@ struct stepcompress {
     // Step+dir+step filter
     uint64_t next_step_clock;
     int next_step_dir;
+    // History tracking
+    int64_t last_position;
+    struct list_head history_list;
+};
+
+struct step_move {
+    uint32_t interval;
+    uint16_t count;
+    int16_t add;
+};
+
+#define HISTORY_EXPIRE (30.0)
+
+struct history_move {
+    struct list_node node;
+    uint64_t first_clock, last_clock;
+    int64_t start_position;
+    int step_count;
+    struct step_move sm;
 };
 
 
@@ -84,12 +104,6 @@ minmax_point(struct stepcompress *sc, uint32_t *pos)
 // using 11 works well in practice.
 #define QUADRATIC_DEV 11
 
-struct step_move {
-    uint32_t interval;
-    uint16_t count;
-    int16_t add;
-};
-
 // Find a 'step_move' that covers a series of step times
 static struct step_move
 compress_bisect_add(struct stepcompress *sc)
@@ -237,6 +251,7 @@ stepcompress_alloc(uint32_t oid)
     struct stepcompress *sc = malloc(sizeof(*sc));
     memset(sc, 0, sizeof(*sc));
     list_init(&sc->msg_queue);
+    list_init(&sc->history_list);
     sc->oid = oid;
     sc->sdir = -1;
     return sc;
@@ -254,6 +269,20 @@ stepcompress_fill(struct stepcompress *sc, uint32_t max_error
     sc->set_next_step_dir_msgtag = set_next_step_dir_msgtag;
 }
 
+// Helper to free items from the history_list
+static void
+free_history(struct stepcompress *sc, uint64_t end_clock)
+{
+    while (!list_empty(&sc->history_list)) {
+        struct history_move *hm = list_last_entry(
+            &sc->history_list, struct history_move, node);
+        if (hm->last_clock > end_clock)
+            break;
+        list_del(&hm->node);
+        free(hm);
+    }
+}
+
 // Free memory associated with a 'stepcompress' object
 void __visible
 stepcompress_free(struct stepcompress *sc)
@@ -262,6 +291,7 @@ stepcompress_free(struct stepcompress *sc)
         return;
     free(sc->queue);
     message_queue_free(&sc->msg_queue);
+    free_history(sc, UINT64_MAX);
     free(sc);
 }
 
@@ -283,6 +313,9 @@ 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
@@ -295,6 +328,39 @@ stepcompress_set_time(struct stepcompress *sc
     calc_last_step_print_time(sc);
 }
 
+// Maximium clock delta between messages in the queue
+#define CLOCK_DIFF_MAX (3<<28)
+
+// Helper to create a queue_step command from a 'struct step_move'
+static void
+add_move(struct stepcompress *sc, uint64_t first_clock, struct step_move *move)
+{
+    int32_t addfactor = move->count*(move->count-1)/2;
+    uint32_t ticks = move->add*addfactor + move->interval*(move->count-1);
+    uint64_t last_clock = first_clock + ticks;
+
+    // Create and queue a queue_step command
+    uint32_t msg[5] = {
+        sc->queue_step_msgtag, sc->oid, move->interval, move->count, move->add
+    };
+    struct queue_message *qm = message_alloc_and_encode(msg, 5);
+    qm->min_clock = qm->req_clock = sc->last_step_clock;
+    if (move->count == 1 && first_clock >= sc->last_step_clock + CLOCK_DIFF_MAX)
+        qm->req_clock = first_clock;
+    list_add_tail(&qm->node, &sc->msg_queue);
+    sc->last_step_clock = last_clock;
+
+    // Create and store move in history tracking
+    struct history_move *hm = malloc(sizeof(*hm));
+    hm->first_clock = first_clock;
+    hm->last_clock = last_clock;
+    hm->start_position = sc->last_position;
+    hm->step_count = sc->sdir ? move->count : -move->count;
+    sc->last_position += hm->step_count;
+    memcpy(&hm->sm, move, sizeof(hm->sm));
+    list_add_head(&hm->node, &sc->history_list);
+}
+
 // Convert previously scheduled steps into commands for the mcu
 static int
 queue_flush(struct stepcompress *sc, uint64_t move_clock)
@@ -307,15 +373,7 @@ queue_flush(struct stepcompress *sc, uint64_t move_clock)
         if (ret)
             return ret;
 
-        uint32_t msg[5] = {
-            sc->queue_step_msgtag, sc->oid, move.interval, move.count, move.add
-        };
-        struct queue_message *qm = message_alloc_and_encode(msg, 5);
-        qm->min_clock = qm->req_clock = sc->last_step_clock;
-        int32_t addfactor = move.count*(move.count-1)/2;
-        uint32_t ticks = move.add*addfactor + move.interval*move.count;
-        sc->last_step_clock += ticks;
-        list_add_tail(&qm->node, &sc->msg_queue);
+        add_move(sc, sc->last_step_clock + move.interval, &move);
 
         if (sc->queue_pos + move.count >= sc->queue_next) {
             sc->queue_pos = sc->queue_next = sc->queue;
@@ -331,14 +389,8 @@ queue_flush(struct stepcompress *sc, uint64_t move_clock)
 static int
 stepcompress_flush_far(struct stepcompress *sc, uint64_t abs_step_clock)
 {
-    uint32_t msg[5] = {
-        sc->queue_step_msgtag, sc->oid, abs_step_clock - sc->last_step_clock,
-        1, 0
-    };
-    struct queue_message *qm = message_alloc_and_encode(msg, 5);
-    qm->min_clock = sc->last_step_clock;
-    sc->last_step_clock = qm->req_clock = abs_step_clock;
-    list_add_tail(&qm->node, &sc->msg_queue);
+    struct step_move move = { abs_step_clock - sc->last_step_clock, 1, 0 };
+    add_move(sc, abs_step_clock, &move);
     calc_last_step_print_time(sc);
     return 0;
 }
@@ -349,10 +401,10 @@ set_next_step_dir(struct stepcompress *sc, int sdir)
 {
     if (sc->sdir == sdir)
         return 0;
-    sc->sdir = sdir;
     int ret = queue_flush(sc, UINT64_MAX);
     if (ret)
         return ret;
+    sc->sdir = sdir;
     uint32_t msg[3] = {
         sc->set_next_step_dir_msgtag, sc->oid, sdir ^ sc->invert_sdir
     };
@@ -362,9 +414,6 @@ set_next_step_dir(struct stepcompress *sc, int sdir)
     return 0;
 }
 
-// Maximium clock delta between messages in the queue
-#define CLOCK_DIFF_MAX (3<<28)
-
 // Slow path for queue_append() - handle next step far in future
 static int
 queue_append_far(struct stepcompress *sc)
@@ -502,6 +551,46 @@ stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock)
     return 0;
 }
 
+// Set last_position in the stepcompress object
+int __visible
+stepcompress_set_last_position(struct stepcompress *sc, int64_t last_position)
+{
+    int ret = stepcompress_flush(sc, UINT64_MAX);
+    if (ret)
+        return ret;
+    sc->last_position = last_position;
+    return 0;
+}
+
+// Search history of moves to find a past position at a given clock
+int64_t __visible
+stepcompress_find_past_position(struct stepcompress *sc, uint64_t clock)
+{
+    int64_t last_position = sc->last_position;
+    struct history_move *hm;
+    list_for_each_entry(hm, &sc->history_list, node) {
+        if (clock < hm->first_clock) {
+            last_position = hm->start_position;
+            continue;
+        }
+        if (clock >= hm->last_clock)
+            return hm->start_position + hm->step_count;
+        int32_t interval = hm->sm.interval, add = hm->sm.add;
+        int32_t ticks = (int32_t)(clock - hm->first_clock) + interval, offset;
+        if (!add) {
+            offset = ticks / interval;
+        } else {
+            // Solve for "count" using quadratic formula
+            double a = .5 * add, b = interval - .5 * add, c = -ticks;
+            offset = (sqrt(b*b - 4*a*c) - b) / (2. * a);
+        }
+        if (hm->step_count < 0)
+            return hm->start_position - offset;
+        return hm->start_position + offset;
+    }
+    return last_position;
+}
+
 // Queue an mcu command to go out in order with stepper commands
 int __visible
 stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len)
diff --git a/klippy/stepper.py b/klippy/stepper.py
index 5af26d66..071e6267 100644
--- a/klippy/stepper.py
+++ b/klippy/stepper.py
@@ -124,6 +124,11 @@ class MCU_stepper:
         return self._tag_position
     def set_tag_position(self, position):
         self._tag_position = position
+    def get_past_commanded_position(self, clock):
+        ffi_main, ffi_lib = chelper.get_ffi()
+        sq = self._stepqueue
+        mcu_pos = ffi_lib.stepcompress_find_past_position(sq, clock)
+        return mcu_pos * self._step_dist - self._mcu_position_offset
     def set_stepper_kinematics(self, sk):
         old_sk = self._stepper_kinematics
         self._stepper_kinematics = sk
@@ -145,7 +150,11 @@ class MCU_stepper:
         if not did_trigger or self._mcu.is_fileoutput():
             return
         params = self._get_position_cmd.send([self._oid])
-        mcu_pos_dist = params['pos'] * self._step_dist
+        last_pos = params['pos']
+        ret = ffi_lib.stepcompress_set_last_position(self._stepqueue, last_pos)
+        if ret:
+            raise error("Internal error in stepcompress")
+        mcu_pos_dist = last_pos * self._step_dist
         if self._invert_dir:
             mcu_pos_dist = -mcu_pos_dist
         self._mcu_position_offset = mcu_pos_dist - self.get_commanded_position()