From 81428265bdcd5db02bd981975e0a8d43bdc2415f Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Mon, 22 Feb 2021 17:57:36 -0500 Subject: [PATCH] stepcompress: Add support for tracking history of queue_step commands Signed-off-by: Kevin O'Connor --- klippy/chelper/__init__.py | 4 + klippy/chelper/stepcompress.c | 143 +++++++++++++++++++++++++++------- klippy/stepper.py | 11 ++- 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 // sqrt #include // offsetof #include // uint32_t #include // 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()