From f5cc355044931439cc27744e4a2446a24fcb87cd Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Wed, 30 Aug 2017 10:43:57 -0400 Subject: [PATCH] stepcompress: Refactor queue insertion to use a cursor Create an insertion "cursor" for adding items to the step compression queue. This makes the calling code simpler and it makes it easier to update the queue memory management in the future. Signed-off-by: Kevin O'Connor --- klippy/stepcompress.c | 202 ++++++++++++++++++++++-------------------- 1 file changed, 105 insertions(+), 97 deletions(-) diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c index 5ea60f58..2a9e1ffb 100644 --- a/klippy/stepcompress.c +++ b/klippy/stepcompress.c @@ -39,42 +39,6 @@ struct stepcompress { }; -/**************************************************************** - * Queue management - ****************************************************************/ - -// Shuffle the internal queue to avoid having to allocate more ram -static void -clean_queue(struct stepcompress *sc) -{ - int in_use = sc->queue_next - sc->queue_pos; - memmove(sc->queue, sc->queue_pos, in_use * sizeof(*sc->queue)); - sc->queue_pos = sc->queue; - sc->queue_next = sc->queue + in_use; -} - -// Expand the internal queue of step times -static void -expand_queue(struct stepcompress *sc, int count) -{ - int alloc = sc->queue_end - sc->queue; - if (count + sc->queue_next - sc->queue_pos <= alloc) { - clean_queue(sc); - return; - } - int pos = sc->queue_pos - sc->queue; - int next = sc->queue_next - sc->queue; - if (!alloc) - alloc = QUEUE_START_SIZE; - while (next + count > alloc) - alloc *= 2; - sc->queue = realloc(sc->queue, alloc * sizeof(*sc->queue)); - sc->queue_end = sc->queue + alloc; - sc->queue_pos = sc->queue + pos; - sc->queue_next = sc->queue + next; -} - - /**************************************************************** * Step compression ****************************************************************/ @@ -362,35 +326,6 @@ set_next_step_dir(struct stepcompress *sc, int sdir) return 0; } -// Check if the internal queue needs to be expanded, and expand if so -static int -_check_push(struct stepcompress *sc) -{ - if (sc->queue_next - sc->queue_pos > 65535 + 2000) { - // No point in keeping more than 64K steps in memory - int ret = stepcompress_flush(sc, *(sc->queue_next - 65535)); - if (ret) - return ret; - } - expand_queue(sc, 1); - return 0; -} -static inline int -check_push(struct stepcompress *sc, uint64_t **pqnext, uint64_t **pqend - , uint64_t c) -{ - if (unlikely(*pqnext >= *pqend)) { - sc->queue_next = *pqnext; - int ret = _check_push(sc); - if (ret) - return ret; - *pqnext = sc->queue_next; - *pqend = sc->queue_end; - } - *(*pqnext)++ = c; - return 0; -} - // Reset the internal state of the stepcompress object int stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock) @@ -429,10 +364,96 @@ stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len) } +/**************************************************************** + * Queue management + ****************************************************************/ + +struct queue_append { + struct stepcompress *sc; + uint64_t *qnext, *qend; + double clock_offset; +}; + +// Create a cursor for inserting clock times into the queue +static inline struct queue_append +queue_append_start(struct stepcompress *sc, double clock_offset, double adjust) +{ + return (struct queue_append) { + .sc = sc, .qnext = sc->queue_next, .qend = sc->queue_end, + .clock_offset = clock_offset + adjust }; +} + +// Finalize a cursor created with queue_append_start() +static inline void +queue_append_finish(struct queue_append qa) +{ + qa.sc->queue_next = qa.qnext; +} + +// Slow path for queue_append() +static int +queue_append_slow(struct stepcompress *sc, double abs_step_clock) +{ + if (sc->queue_next - sc->queue_pos > 65535 + 2000) { + // No point in keeping more than 64K steps in memory + int ret = stepcompress_flush(sc, *(sc->queue_next - 65535)); + if (ret) + return ret; + } + + int in_use = sc->queue_next - sc->queue_pos; + if (sc->queue_pos > sc->queue) { + // Shuffle the internal queue to avoid having to allocate more ram + memmove(sc->queue, sc->queue_pos, in_use * sizeof(*sc->queue)); + } else { + // Expand the internal queue of step times + int alloc = sc->queue_end - sc->queue; + if (!alloc) + alloc = QUEUE_START_SIZE; + while (in_use >= alloc) + alloc *= 2; + sc->queue = realloc(sc->queue, alloc * sizeof(*sc->queue)); + sc->queue_end = sc->queue + alloc; + } + sc->queue_pos = sc->queue; + sc->queue_next = sc->queue + in_use; + *sc->queue_next++ = abs_step_clock; + return 0; +} + +// Add a clock time to the queue (flushing the queue if needed) +static inline int +queue_append(struct queue_append *qa, double step_clock) +{ + double abs_step_clock = step_clock + qa->clock_offset; + if (likely(qa->qnext < qa->qend)) { + *qa->qnext++ = abs_step_clock; + return 0; + } + // Call queue_append_slow() to handle queue expansion + struct stepcompress *sc = qa->sc; + sc->queue_next = qa->qnext; + int ret = queue_append_slow(sc, abs_step_clock); + if (ret) + return ret; + qa->qnext = sc->queue_next; + qa->qend = sc->queue_end; + return 0; +} + + /**************************************************************** * Motion to step conversions ****************************************************************/ +// Common suffixes: _sd is step distance (a unit length the same +// distance the stepper moves on each step), _sv is step velocity (in +// units of step distance per clock tick), _sd2 is step distance +// squared, _r is ratio (scalar usually between 0.0 and 1.0). Times +// are represented as clock ticks (a unit of time determined by a +// micro-controller tick) and acceleration is in units of step +// distance per clock ticks squared. + // Wrapper around sqrt() to handle small negative numbers static double _safe_sqrt(double v) @@ -454,23 +475,14 @@ stepcompress_push(struct stepcompress *sc, double step_clock, int32_t sdir) int ret = set_next_step_dir(sc, !!sdir); if (ret) return ret; - step_clock += 0.5; - uint64_t *qnext = sc->queue_next, *qend = sc->queue_end; - ret = check_push(sc, &qnext, &qend, step_clock); + struct queue_append qa = queue_append_start(sc, step_clock, 0.5); + ret = queue_append(&qa, 0.); if (ret) return ret; - sc->queue_next = qnext; + queue_append_finish(qa); return 0; } -// Common suffixes: _sd is step distance (a unit length the same -// distance the stepper moves on each step), _sv is step velocity (in -// units of step distance per clock tick), _sd2 is step distance -// squared, _r is ratio (scalar usually between 0.0 and 1.0). Times -// are represented as clock ticks (a unit of time determined by a -// micro-controller tick) and acceleration is in units of step -// distance per clock ticks squared. - // Schedule 'steps' number of steps at constant acceleration. If // acceleration is zero (ie, constant velocity) it uses the formula: // step_clock = clock_offset + step_num/start_sv @@ -505,35 +517,34 @@ stepcompress_push_const( int res = sdir ? count : -count; // Calculate each step time - clock_offset += 0.5; double pos = step_offset + .5; - uint64_t *qnext = sc->queue_next, *qend = sc->queue_end; if (!accel) { // Move at constant velocity (zero acceleration) + struct queue_append qa = queue_append_start(sc, clock_offset, .5); double inv_cruise_sv = 1. / start_sv; while (count--) { - uint64_t c = clock_offset + pos*inv_cruise_sv; - int ret = check_push(sc, &qnext, &qend, c); + ret = queue_append(&qa, pos * inv_cruise_sv); if (ret) return ret; pos += 1.0; } + queue_append_finish(qa); } else { // Move with constant acceleration double inv_accel = 1. / accel; - clock_offset -= start_sv * inv_accel; pos += .5 * start_sv*start_sv * inv_accel; + struct queue_append qa = queue_append_start( + sc, clock_offset, 0.5 - start_sv * inv_accel); double accel_multiplier = 2. * inv_accel; while (count--) { double v = safe_sqrt(pos * accel_multiplier); - uint64_t c = clock_offset + (accel_multiplier >= 0. ? v : -v); - int ret = check_push(sc, &qnext, &qend, c); + int ret = queue_append(&qa, accel_multiplier >= 0. ? v : -v); if (ret) return ret; pos += 1.0; } + queue_append_finish(qa); } - sc->queue_next = qnext; return res; } @@ -565,19 +576,17 @@ _stepcompress_push_delta( int res = sdir ? count : -count; // Calculate each step time - clock_offset += 0.5; height += (sdir ? .5 : -.5); - uint64_t *qnext = sc->queue_next, *qend = sc->queue_end; if (!accel) { // Move at constant velocity (zero acceleration) + struct queue_append qa = queue_append_start(sc, clock_offset, .5); double inv_cruise_sv = 1. / start_sv; if (!movez_r) { // Optimized case for common XY only moves (no Z movement) while (count--) { double v = safe_sqrt(arm_sd2 - height*height); double pos = startxy_sd + (sdir ? -v : v); - uint64_t c = clock_offset + pos * inv_cruise_sv; - int ret = check_push(sc, &qnext, &qend, c); + int ret = queue_append(&qa, pos * inv_cruise_sv); if (ret) return ret; height += (sdir ? 1. : -1.); @@ -586,8 +595,7 @@ _stepcompress_push_delta( // Optimized case for Z only moves double pos = (sdir ? height-end_height : end_height-height); while (count--) { - uint64_t c = clock_offset + pos * inv_cruise_sv; - int ret = check_push(sc, &qnext, &qend, c); + int ret = queue_append(&qa, pos * inv_cruise_sv); if (ret) return ret; pos += 1.; @@ -599,33 +607,33 @@ _stepcompress_push_delta( double relheight = movexy_r*height - zoffset; double v = safe_sqrt(arm_sd2 - relheight*relheight); double pos = start_pos + movez_r*height + (sdir ? -v : v); - uint64_t c = clock_offset + pos * inv_cruise_sv; - int ret = check_push(sc, &qnext, &qend, c); + int ret = queue_append(&qa, pos * inv_cruise_sv); if (ret) return ret; height += (sdir ? 1. : -1.); } } + queue_append_finish(qa); } else { // Move with constant acceleration double start_pos = movexy_r*startxy_sd, zoffset = movez_r*startxy_sd; double inv_accel = 1. / accel; - clock_offset -= start_sv * inv_accel; start_pos += 0.5 * start_sv*start_sv * inv_accel; + struct queue_append qa = queue_append_start( + sc, clock_offset, 0.5 - start_sv * inv_accel); double accel_multiplier = 2. * inv_accel; while (count--) { double relheight = movexy_r*height - zoffset; double v = safe_sqrt(arm_sd2 - relheight*relheight); double pos = start_pos + movez_r*height + (sdir ? -v : v); v = safe_sqrt(pos * accel_multiplier); - uint64_t c = clock_offset + (accel_multiplier >= 0. ? v : -v); - int ret = check_push(sc, &qnext, &qend, c); + int ret = queue_append(&qa, accel_multiplier >= 0. ? v : -v); if (ret) return ret; height += (sdir ? 1. : -1.); } + queue_append_finish(qa); } - sc->queue_next = qnext; return res; }