diff --git a/klippy/chelper/itersolve.c b/klippy/chelper/itersolve.c index 1094e5f3..9cae55fd 100644 --- a/klippy/chelper/itersolve.c +++ b/klippy/chelper/itersolve.c @@ -13,6 +13,11 @@ #include "stepcompress.h" // queue_append_start #include "trapq.h" // struct move + +/**************************************************************** + * Main iterative solver + ****************************************************************/ + struct timepos { double time, position; }; @@ -66,7 +71,7 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m double start = move_start - m->print_time, end = move_end - m->print_time; struct timepos last = { start, sk->commanded_pos }, low = last, high = last; double seek_time_delta = SEEK_TIME_RESET; - int sdir = !!stepcompress_get_step_dir(sk->sc), is_dir_change = 0; + int sdir = stepcompress_get_step_dir(sk->sc), is_dir_change = 0; for (;;) { double diff = high.position - last.position, dist = sdir ? diff : -diff; if (dist >= half_step) { @@ -90,6 +95,9 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m if (low.time < high.time) // The existing search range is still valid continue; + } else if (dist > 0.) { + // Avoid rollback if stepper fully reaches target position + stepcompress_commit(sk->sc); } else if (unlikely(dist < -(half_step + .000000001))) { // Found direction change is_dir_change = 1; @@ -127,6 +135,11 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m return 0; } + +/**************************************************************** + * Interface functions + ****************************************************************/ + // Check if a move is likely to cause movement on a stepper static inline int check_active(struct stepper_kinematics *sk, struct move *m) diff --git a/klippy/chelper/stepcompress.c b/klippy/chelper/stepcompress.c index 7b0fdfd7..812a3b50 100644 --- a/klippy/chelper/stepcompress.c +++ b/klippy/chelper/stepcompress.c @@ -38,6 +38,9 @@ struct stepcompress { struct list_head msg_queue; uint32_t queue_step_msgid, set_next_step_dir_msgid, oid; int sdir, invert_sdir; + // Step+dir+step filter + uint64_t next_step_clock; + int next_step_dir; }; @@ -270,7 +273,7 @@ stepcompress_get_oid(struct stepcompress *sc) int stepcompress_get_step_dir(struct stepcompress *sc) { - return sc->sdir; + return sc->next_step_dir; } // Determine the "print time" of the last_step_clock @@ -293,7 +296,7 @@ stepcompress_set_time(struct stepcompress *sc // Convert previously scheduled steps into commands for the mcu static int -stepcompress_flush(struct stepcompress *sc, uint64_t move_clock) +queue_flush(struct stepcompress *sc, uint64_t move_clock) { if (sc->queue_pos >= sc->queue_next) return 0; @@ -346,7 +349,7 @@ set_next_step_dir(struct stepcompress *sc, int sdir) if (sc->sdir == sdir) return 0; sc->sdir = sdir; - int ret = stepcompress_flush(sc, UINT64_MAX); + int ret = queue_flush(sc, UINT64_MAX); if (ret) return ret; uint32_t msg[3] = { @@ -361,26 +364,30 @@ set_next_step_dir(struct stepcompress *sc, int sdir) // Maximium clock delta between messages in the queue #define CLOCK_DIFF_MAX (3<<28) -// Slow path for stepcompress_append() +// Slow path for queue_append() - handle next step far in future static int -queue_append_slow(struct stepcompress *sc, double rel_sc) +queue_append_far(struct stepcompress *sc) { - uint64_t abs_step_clock = (uint64_t)rel_sc + sc->last_step_clock; - if (abs_step_clock >= sc->last_step_clock + CLOCK_DIFF_MAX) { - // Avoid integer overflow on steps far in the future - int ret = stepcompress_flush(sc, abs_step_clock - CLOCK_DIFF_MAX + 1); - if (ret) - return ret; - - if (abs_step_clock >= sc->last_step_clock + CLOCK_DIFF_MAX) - return stepcompress_flush_far(sc, abs_step_clock); - } + uint64_t step_clock = sc->next_step_clock; + sc->next_step_clock = 0; + int ret = queue_flush(sc, step_clock - CLOCK_DIFF_MAX + 1); + if (ret) + return ret; + if (step_clock >= sc->last_step_clock + CLOCK_DIFF_MAX) + return stepcompress_flush_far(sc, step_clock); + *sc->queue_next++ = step_clock; + return 0; +} +// Slow path for queue_append() - expand the internal queue storage +static int +queue_append_extend(struct stepcompress *sc) +{ if (sc->queue_next - sc->queue_pos > 65535 + 2000) { // No point in keeping more than 64K steps in memory uint32_t flush = (*(sc->queue_next-65535) - (uint32_t)sc->last_step_clock); - int ret = stepcompress_flush(sc, sc->last_step_clock + flush); + int ret = queue_flush(sc, sc->last_step_clock + flush); if (ret) return ret; } @@ -405,30 +412,82 @@ queue_append_slow(struct stepcompress *sc, double rel_sc) sc->queue_next = sc->queue + in_use; } - *sc->queue_next++ = abs_step_clock; + *sc->queue_next++ = sc->next_step_clock; + sc->next_step_clock = 0; return 0; } // Add a step time to the queue (flushing the queue if needed) -inline int -stepcompress_append(struct stepcompress *sc, int sdir - , double print_time, double step_time) +static int +queue_append(struct stepcompress *sc) { - if (unlikely(sdir != sc->sdir)) { - int ret = set_next_step_dir(sc, sdir); + if (unlikely(sc->next_step_dir != sc->sdir)) { + int ret = set_next_step_dir(sc, sc->next_step_dir); if (ret) return ret; } + if (unlikely(sc->next_step_clock >= sc->last_step_clock + CLOCK_DIFF_MAX)) + return queue_append_far(sc); + if (unlikely(sc->queue_next >= sc->queue_end)) + return queue_append_extend(sc); + *sc->queue_next++ = sc->next_step_clock; + sc->next_step_clock = 0; + return 0; +} + +#define SDS_FILTER_TIME .000750 + +// Add next step time +int +stepcompress_append(struct stepcompress *sc, int sdir + , double print_time, double step_time) +{ + // Calculate step clock double offset = print_time - sc->last_step_print_time; double rel_sc = (step_time + offset) * sc->mcu_freq; - if (unlikely(sc->queue_next >= sc->queue_end - || rel_sc >= (double)CLOCK_DIFF_MAX)) - // Slow path to handle queue expansion and integer overflow - return queue_append_slow(sc, rel_sc); - *sc->queue_next++ = (uint32_t)sc->last_step_clock + (uint32_t)rel_sc; + uint64_t step_clock = sc->last_step_clock + (uint64_t)rel_sc; + // Flush previous pending step (if any) + if (sc->next_step_clock) { + if (unlikely(sdir != sc->next_step_dir)) { + double diff = step_clock - sc->next_step_clock; + if (diff < SDS_FILTER_TIME * sc->mcu_freq) { + // Rollback last step to avoid rapid step+dir+step + sc->next_step_clock = 0; + sc->next_step_dir = sdir; + return 0; + } + } + int ret = queue_append(sc); + if (ret) + return ret; + } + // Store this step as the next pending step + sc->next_step_clock = step_clock; + sc->next_step_dir = sdir; return 0; } +// Commit next pending step (ie, do not allow a rollback) +int +stepcompress_commit(struct stepcompress *sc) +{ + if (sc->next_step_clock) + return queue_append(sc); + return 0; +} + +// Flush pending steps +static int +stepcompress_flush(struct stepcompress *sc, uint64_t move_clock) +{ + if (sc->next_step_clock && move_clock >= sc->next_step_clock) { + int ret = queue_append(sc); + if (ret) + return ret; + } + return queue_flush(sc, move_clock); +} + // Reset the internal state of the stepcompress object int __visible stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock) diff --git a/klippy/chelper/stepcompress.h b/klippy/chelper/stepcompress.h index 59f12699..3462b3f8 100644 --- a/klippy/chelper/stepcompress.h +++ b/klippy/chelper/stepcompress.h @@ -14,6 +14,7 @@ uint32_t stepcompress_get_oid(struct stepcompress *sc); int stepcompress_get_step_dir(struct stepcompress *sc); int stepcompress_append(struct stepcompress *sc, int sdir , double print_time, double step_time); +int stepcompress_commit(struct stepcompress *sc); int stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock); int stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len); diff --git a/klippy/toolhead.py b/klippy/toolhead.py index ada5f8c3..8ce0935c 100644 --- a/klippy/toolhead.py +++ b/klippy/toolhead.py @@ -1,6 +1,6 @@ # Code for coordinating events on the printer toolhead # -# Copyright (C) 2016-2019 Kevin O'Connor +# Copyright (C) 2016-2020 Kevin O'Connor # # This file may be distributed under the terms of the GNU GPLv3 license. import math, logging, importlib @@ -183,6 +183,7 @@ class MoveQueue: MIN_KIN_TIME = 0.100 MOVE_BATCH_TIME = 0.500 +SDS_CHECK_TIME = 0.001 # step+dir+step filter in stepcompress.c DRIP_SEGMENT_TIME = 0.050 DRIP_TIME = 0.100 @@ -236,7 +237,7 @@ class ToolHead: self.print_stall = 0 self.drip_completion = None # Kinematic step generation scan window time tracking - self.kin_flush_delay = 0. + self.kin_flush_delay = SDS_CHECK_TIME self.kin_flush_times = [] self.last_kin_flush_time = self.last_kin_move_time = 0. # Setup iterative solver @@ -512,7 +513,7 @@ class ToolHead: self.kin_flush_times.pop(self.kin_flush_times.index(old_delay)) if delay: self.kin_flush_times.append(delay) - new_delay = max(self.kin_flush_times + [0.]) + new_delay = max(self.kin_flush_times + [SDS_CHECK_TIME]) self.kin_flush_delay = new_delay def register_lookahead_callback(self, callback): last_move = self.move_queue.get_last()