diff options
| -rw-r--r-- | klippy/chelper/itersolve.c | 13 | ||||
| -rw-r--r-- | klippy/chelper/stepcompress.c | 96 | ||||
| -rw-r--r-- | klippy/chelper/stepcompress.h | 14 |
3 files changed, 35 insertions, 88 deletions
diff --git a/klippy/chelper/itersolve.c b/klippy/chelper/itersolve.c index 0b7c4b04..9dde391a 100644 --- a/klippy/chelper/itersolve.c +++ b/klippy/chelper/itersolve.c @@ -1,6 +1,6 @@ // Iterative solver for kinematic moves // -// Copyright (C) 2018-2019 Kevin O'Connor <kevin@koconnor.net> +// Copyright (C) 2018-2020 Kevin O'Connor <kevin@koconnor.net> // // This file may be distributed under the terms of the GNU GPLv3 license. @@ -59,15 +59,12 @@ static int32_t itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m , double move_start, double move_end) { - struct stepcompress *sc = sk->sc; sk_calc_callback calc_position_cb = sk->calc_position_cb; double half_step = .5 * sk->step_dist; - double mcu_freq = stepcompress_get_mcu_freq(sc); 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 = 0.000100; - int sdir = stepcompress_get_step_dir(sc); - struct queue_append qa = queue_append_start(sc, m->print_time, .5); + int sdir = stepcompress_get_step_dir(sk->sc); for (;;) { // Determine if next step is in forward or reverse direction double dist = high.position - last.position; @@ -101,16 +98,13 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m high.position = calc_position_cb(sk, m, high.time); continue; } - int ret = queue_append_set_next_step_dir(&qa, next_sdir); - if (ret) - return ret; sdir = next_sdir; } // Find step double target = last.position + (sdir ? half_step : -half_step); struct timepos next = itersolve_find_step(sk, m, low, high, target); // Add step at given time - int ret = queue_append(&qa, next.time * mcu_freq); + int ret = stepcompress_append(sk->sc, sdir, m->print_time, next.time); if (ret) return ret; seek_time_delta = next.time - last.time; @@ -123,7 +117,6 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m // The high range is no longer valid - recalculate it goto seek_new_high_range; } - queue_append_finish(qa); sk->commanded_pos = last.position; if (sk->post_cb) sk->post_cb(sk); diff --git a/klippy/chelper/stepcompress.c b/klippy/chelper/stepcompress.c index 7b92e3db..4847bf16 100644 --- a/klippy/chelper/stepcompress.c +++ b/klippy/chelper/stepcompress.c @@ -1,9 +1,9 @@ // Stepper pulse schedule compression // -// Copyright (C) 2016-2018 Kevin O'Connor <kevin@koconnor.net> +// Copyright (C) 2016-2020 Kevin O'Connor <kevin@koconnor.net> // // This file may be distributed under the terms of the GNU GPLv3 license. -// + // The goal of this code is to take a series of scheduled stepper // pulse times and compress them into a handful of commands that can // be efficiently transmitted and executed on a microcontroller (mcu). @@ -32,7 +32,7 @@ struct stepcompress { uint32_t *queue, *queue_end, *queue_pos, *queue_next; // Internal tracking uint32_t max_error; - double mcu_time_offset, mcu_freq; + double mcu_time_offset, mcu_freq, last_step_print_time; // Message generation uint64_t last_step_clock; struct list_head msg_queue; @@ -261,6 +261,14 @@ stepcompress_free(struct stepcompress *sc) free(sc); } +// Determine the "print time" of the last_step_clock +static void +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; +} + // Convert previously scheduled steps into commands for the mcu static int stepcompress_flush(struct stepcompress *sc, uint64_t move_clock) @@ -289,6 +297,7 @@ stepcompress_flush(struct stepcompress *sc, uint64_t move_clock) } sc->queue_pos += move.count; } + calc_last_step_print_time(sc); return 0; } @@ -304,6 +313,7 @@ stepcompress_flush_far(struct stepcompress *sc, uint64_t abs_step_clock) 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); + calc_last_step_print_time(sc); return 0; } @@ -335,6 +345,7 @@ stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock) return ret; sc->last_step_clock = last_step_clock; sc->sdir = -1; + calc_last_step_print_time(sc); return 0; } @@ -359,12 +370,7 @@ stepcompress_set_time(struct stepcompress *sc { sc->mcu_time_offset = time_offset; sc->mcu_freq = mcu_freq; -} - -double -stepcompress_get_mcu_freq(struct stepcompress *sc) -{ - return sc->mcu_freq; + calc_last_step_print_time(sc); } uint32_t @@ -379,33 +385,10 @@ stepcompress_get_step_dir(struct stepcompress *sc) return sc->sdir; } - -/**************************************************************** - * Queue management - ****************************************************************/ - // Maximium clock delta between messages in the queue #define CLOCK_DIFF_MAX (3<<28) -// Create a cursor for inserting clock times into the queue -inline struct queue_append -queue_append_start(struct stepcompress *sc, double print_time, double adjust) -{ - double print_clock = (print_time - sc->mcu_time_offset) * sc->mcu_freq; - return (struct queue_append) { - .sc = sc, .qnext = sc->queue_next, .qend = sc->queue_end, - .last_step_clock_32 = sc->last_step_clock, - .clock_offset = (print_clock - (double)sc->last_step_clock) + adjust }; -} - -// Finalize a cursor created with queue_append_start() -inline void -queue_append_finish(struct queue_append qa) -{ - qa.sc->queue_next = qa.qnext; -} - -// Slow path for queue_append() +// Slow path for stepcompress_append() static int queue_append_slow(struct stepcompress *sc, double rel_sc) { @@ -453,42 +436,23 @@ queue_append_slow(struct stepcompress *sc, double rel_sc) return 0; } -// Add a clock time to the queue (flushing the queue if needed) +// Add a step time to the queue (flushing the queue if needed) inline int -queue_append(struct queue_append *qa, double step_clock) +stepcompress_append(struct stepcompress *sc, int sdir + , double print_time, double step_time) { - double rel_sc = step_clock + qa->clock_offset; - if (likely(!(qa->qnext >= qa->qend || rel_sc >= (double)CLOCK_DIFF_MAX))) { - *qa->qnext++ = qa->last_step_clock_32 + (uint32_t)rel_sc; - return 0; + if (unlikely(sdir != sc->sdir)) { + int ret = set_next_step_dir(sc, sdir); + if (ret) + return ret; } - // Call queue_append_slow() to handle queue expansion and integer overflow - struct stepcompress *sc = qa->sc; - uint64_t old_last_step_clock = sc->last_step_clock; - sc->queue_next = qa->qnext; - int ret = queue_append_slow(sc, rel_sc); - if (ret) - return ret; - qa->qnext = sc->queue_next; - qa->qend = sc->queue_end; - qa->last_step_clock_32 = sc->last_step_clock; - qa->clock_offset -= sc->last_step_clock - old_last_step_clock; - return 0; -} - -inline int -queue_append_set_next_step_dir(struct queue_append *qa, int sdir) -{ - struct stepcompress *sc = qa->sc; - uint64_t old_last_step_clock = sc->last_step_clock; - sc->queue_next = qa->qnext; - int ret = set_next_step_dir(sc, sdir); - if (ret) - return ret; - qa->qnext = sc->queue_next; - qa->qend = sc->queue_end; - qa->last_step_clock_32 = sc->last_step_clock; - qa->clock_offset -= sc->last_step_clock - old_last_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; return 0; } diff --git a/klippy/chelper/stepcompress.h b/klippy/chelper/stepcompress.h index b56de304..540acabe 100644 --- a/klippy/chelper/stepcompress.h +++ b/klippy/chelper/stepcompress.h @@ -12,20 +12,10 @@ void stepcompress_fill(struct stepcompress *sc, uint32_t max_error void stepcompress_free(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); -double stepcompress_get_mcu_freq(struct stepcompress *sc); uint32_t stepcompress_get_oid(struct stepcompress *sc); int stepcompress_get_step_dir(struct stepcompress *sc); - -struct queue_append { - struct stepcompress *sc; - uint32_t *qnext, *qend, last_step_clock_32; - double clock_offset; -}; -struct queue_append queue_append_start( - struct stepcompress *sc, double print_time, double adjust); -void queue_append_finish(struct queue_append qa); -int queue_append(struct queue_append *qa, double step_clock); -int queue_append_set_next_step_dir(struct queue_append *qa, int sdir); +int stepcompress_append(struct stepcompress *sc, int sdir + , double print_time, double step_time); struct serialqueue; struct steppersync *steppersync_alloc( |