From 7013a7b15f642e9233ba78ac9188ff01d2ebd4fb Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Mon, 19 Jul 2021 14:51:05 -0400 Subject: [PATCH] stepcompress: Support extracting stepcompress history Signed-off-by: Kevin O'Connor --- klippy/chelper/__init__.py | 9 ++++++++ klippy/chelper/stepcompress.c | 39 +++++++++++++++++++++++++++++++---- klippy/chelper/stepcompress.h | 9 ++++++++ klippy/stepper.py | 6 ++++++ 4 files changed, 59 insertions(+), 4 deletions(-) diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index c2ad802a..6c579ab7 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -30,6 +30,12 @@ OTHER_FILES = [ ] defs_stepcompress = """ + struct pull_history_steps { + uint64_t first_clock, last_clock; + int64_t start_position; + int step_count, interval, add; + }; + struct stepcompress *stepcompress_alloc(uint32_t oid); void stepcompress_fill(struct stepcompress *sc, uint32_t max_error , uint32_t invert_sdir, int32_t queue_step_msgtag @@ -42,6 +48,9 @@ defs_stepcompress = """ , uint64_t clock); int stepcompress_queue_msg(struct stepcompress *sc , uint32_t *data, int len); + int stepcompress_extract_old(struct stepcompress *sc + , struct pull_history_steps *p, int max + , uint64_t start_clock, uint64_t end_clock); struct steppersync *steppersync_alloc(struct serialqueue *sq , struct stepcompress **sc_list, int sc_num, int move_num); diff --git a/klippy/chelper/stepcompress.c b/klippy/chelper/stepcompress.c index 50a4d7c4..fab15a2c 100644 --- a/klippy/chelper/stepcompress.c +++ b/klippy/chelper/stepcompress.c @@ -60,8 +60,7 @@ struct history_steps { struct list_node node; uint64_t first_clock, last_clock; int64_t start_position; - int step_count; - struct step_move sm; + int step_count, interval, add; }; @@ -355,9 +354,10 @@ add_move(struct stepcompress *sc, uint64_t first_clock, struct step_move *move) hs->first_clock = first_clock; hs->last_clock = last_clock; hs->start_position = sc->last_position; + hs->interval = move->interval; + hs->add = move->add; hs->step_count = sc->sdir ? move->count : -move->count; sc->last_position += hs->step_count; - memcpy(&hs->sm, move, sizeof(hs->sm)); list_add_head(&hs->node, &sc->history_list); } @@ -559,6 +559,13 @@ stepcompress_set_last_position(struct stepcompress *sc, int64_t last_position) if (ret) return ret; sc->last_position = last_position; + + // Add a marker to the history list + struct history_steps *hs = malloc(sizeof(*hs)); + memset(hs, 0, sizeof(*hs)); + hs->first_clock = hs->last_clock = sc->last_step_clock; + hs->start_position = last_position; + list_add_head(&hs->node, &sc->history_list); return 0; } @@ -575,7 +582,7 @@ stepcompress_find_past_position(struct stepcompress *sc, uint64_t clock) } if (clock >= hs->last_clock) return hs->start_position + hs->step_count; - int32_t interval = hs->sm.interval, add = hs->sm.add; + int32_t interval = hs->interval, add = hs->add; int32_t ticks = (int32_t)(clock - hs->first_clock) + interval, offset; if (!add) { offset = ticks / interval; @@ -605,6 +612,30 @@ stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len) return 0; } +// Return history of queue_step commands +int __visible +stepcompress_extract_old(struct stepcompress *sc, struct pull_history_steps *p + , int max, uint64_t start_clock, uint64_t end_clock) +{ + int res = 0; + struct history_steps *hs; + list_for_each_entry(hs, &sc->history_list, node) { + if (start_clock >= hs->last_clock || res >= max) + break; + if (end_clock <= hs->first_clock) + continue; + p->first_clock = hs->first_clock; + p->last_clock = hs->last_clock; + p->start_position = hs->start_position; + p->step_count = hs->step_count; + p->interval = hs->interval; + p->add = hs->add; + p++; + res++; + } + return res; +} + /**************************************************************** * Step compress synchronization diff --git a/klippy/chelper/stepcompress.h b/klippy/chelper/stepcompress.h index 4c708541..d10cccf9 100644 --- a/klippy/chelper/stepcompress.h +++ b/klippy/chelper/stepcompress.h @@ -5,6 +5,12 @@ #define ERROR_RET -989898989 +struct pull_history_steps { + uint64_t first_clock, last_clock; + int64_t start_position; + int step_count, interval, add; +}; + struct stepcompress *stepcompress_alloc(uint32_t oid); void stepcompress_fill(struct stepcompress *sc, uint32_t max_error , uint32_t invert_sdir, int32_t queue_step_msgtag @@ -21,6 +27,9 @@ int stepcompress_set_last_position(struct stepcompress *sc int64_t stepcompress_find_past_position(struct stepcompress *sc , uint64_t clock); int stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len); +int stepcompress_extract_old(struct stepcompress *sc + , struct pull_history_steps *p, int max + , uint64_t start_clock, uint64_t end_clock); struct serialqueue; struct steppersync *steppersync_alloc( diff --git a/klippy/stepper.py b/klippy/stepper.py index 5c717ca9..6609fca2 100644 --- a/klippy/stepper.py +++ b/klippy/stepper.py @@ -119,6 +119,12 @@ class MCU_stepper: def get_past_commanded_position(self, print_time): mcu_pos = self.get_past_mcu_position(print_time) return mcu_pos * self._step_dist - self._mcu_position_offset + def dump_steps(self, count, start_clock, end_clock): + ffi_main, ffi_lib = chelper.get_ffi() + data = ffi_main.new('struct pull_history_steps[]', count) + count = ffi_lib.stepcompress_extract_old(self._stepqueue, data, count, + start_clock, end_clock) + return (data, count) def set_stepper_kinematics(self, sk): old_sk = self._stepper_kinematics mcu_pos = 0