From 7554c7f69423bf3d22f340a8b4851c25de855983 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Thu, 10 Nov 2016 12:44:04 -0500 Subject: [PATCH] stepcompress: Do all step rounding in C code Commits f0cefebf and 8f331f08 changed the way the code determined what steps to take on fractional steps. Unfortunately, it was possible in some situations for the C code to round differently from the python code which could result in warnings and lost steps. Change the code so that all fractional step handling is done in the C code. Implementing the step rounding logic in one location avoids any conflicts. In order to efficiently handle the step rounding in the C code, the C code has also been extended to directly send the "set_next_step_dir" command. Signed-off-by: Kevin O'Connor --- klippy/cartesian.py | 70 +++++++++++++++++---------------- klippy/chelper.py | 10 +++-- klippy/extruder.py | 83 +++++++++++++++++---------------------- klippy/mcu.py | 28 ++++++-------- klippy/stepcompress.c | 90 +++++++++++++++++++++++++++++-------------- klippy/stepper.py | 4 +- 6 files changed, 154 insertions(+), 131 deletions(-) diff --git a/klippy/cartesian.py b/klippy/cartesian.py index 0a5f7961..85746860 100644 --- a/klippy/cartesian.py +++ b/klippy/cartesian.py @@ -97,45 +97,47 @@ class CartKinematics: inv_accel = 1. / move.accel inv_cruise_v = 1. / move.cruise_v for i in StepList: - inv_step_dist = self.steppers[i].inv_step_dist - new_step_pos = int(move.end_pos[i]*inv_step_dist + 0.5) - step_pos = self.stepper_pos[i] - if new_step_pos == step_pos: + if not move.axes_d[i]: continue - self.stepper_pos[i] = new_step_pos + mcu_time, so = self.steppers[i].prep_move(move_time) + inv_step_dist = self.steppers[i].inv_step_dist steps = move.axes_d[i] * inv_step_dist - step_offset = step_pos - move.start_pos[i] * inv_step_dist + 0.5 - sdir = 1 - if steps < 0: - sdir = 0 - steps = -steps - step_offset = 1. - step_offset - mcu_time, so = self.steppers[i].prep_move(move_time, sdir) + move_step_d = move.move_d / abs(steps) - move_step_d = move.move_d / steps + step_pos = self.stepper_pos[i] + step_offset = step_pos - move.start_pos[i] * inv_step_dist # Acceleration steps - #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel - accel_time_offset = move.start_v * inv_accel - accel_sqrt_offset = accel_time_offset**2 accel_multiplier = 2.0 * move_step_d * inv_accel - accel_steps = move.accel_r * steps - step_offset = so.step_sqrt( - mcu_time - accel_time_offset, accel_steps, step_offset - , accel_sqrt_offset, accel_multiplier) - mcu_time += move.accel_t + if move.accel_r: + #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel + accel_time_offset = move.start_v * inv_accel + accel_sqrt_offset = accel_time_offset**2 + accel_steps = move.accel_r * steps + count = so.step_sqrt( + mcu_time - accel_time_offset, accel_steps, step_offset + , accel_sqrt_offset, accel_multiplier) + step_pos += count + step_offset += count - accel_steps + mcu_time += move.accel_t # Cruising steps - #t = pos/cruise_v - cruise_multiplier = move_step_d * inv_cruise_v - cruise_steps = move.cruise_r * steps - step_offset = so.step_factor( - mcu_time, cruise_steps, step_offset, cruise_multiplier) - mcu_time += move.cruise_t + if move.cruise_r: + #t = pos/cruise_v + cruise_multiplier = move_step_d * inv_cruise_v + cruise_steps = move.cruise_r * steps + count = so.step_factor( + mcu_time, cruise_steps, step_offset, cruise_multiplier) + step_pos += count + step_offset += count - cruise_steps + mcu_time += move.cruise_t # Deceleration steps - #t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel) - decel_time_offset = move.cruise_v * inv_accel - decel_sqrt_offset = decel_time_offset**2 - decel_steps = move.decel_r * steps - so.step_sqrt( - mcu_time + decel_time_offset, decel_steps, step_offset - , decel_sqrt_offset, -accel_multiplier) + if move.decel_r: + #t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel) + decel_time_offset = move.cruise_v * inv_accel + decel_sqrt_offset = decel_time_offset**2 + decel_steps = move.decel_r * steps + count = so.step_sqrt( + mcu_time + decel_time_offset, decel_steps, step_offset + , decel_sqrt_offset, -accel_multiplier) + step_pos += count + self.stepper_pos[i] = step_pos diff --git a/klippy/chelper.py b/klippy/chelper.py index 5b7837ee..a3ce4a95 100644 --- a/klippy/chelper.py +++ b/klippy/chelper.py @@ -13,12 +13,14 @@ OTHER_FILES = ['list.h', 'serialqueue.h'] defs_stepcompress = """ struct stepcompress *stepcompress_alloc(uint32_t max_error - , uint32_t queue_step_msgid, uint32_t oid); - void stepcompress_push(struct stepcompress *sc, double step_clock); - double stepcompress_push_factor(struct stepcompress *sc + , uint32_t queue_step_msgid, uint32_t set_next_step_dir_msgid + , uint32_t invert_sdir, uint32_t oid); + void stepcompress_push(struct stepcompress *sc, double step_clock + , int32_t sdir); + int32_t stepcompress_push_factor(struct stepcompress *sc , double steps, double step_offset , double clock_offset, double factor); - double stepcompress_push_sqrt(struct stepcompress *sc + int32_t stepcompress_push_sqrt(struct stepcompress *sc , double steps, double step_offset , double clock_offset, double sqrt_offset, double factor); void stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock); diff --git a/klippy/extruder.py b/klippy/extruder.py index e7f1b40c..fb726fca 100644 --- a/klippy/extruder.py +++ b/klippy/extruder.py @@ -88,69 +88,58 @@ class PrinterExtruder: # There is still only a decel phase (no retraction) decel_d -= extra_decel_d - # Determine regular steps - forward_d = accel_d + cruise_d + decel_d - end_pos = start_pos + forward_d + # Prepare for steps + stepper_pos = self.stepper_pos inv_step_dist = self.stepper.inv_step_dist - new_step_pos = int(end_pos*inv_step_dist + 0.5) - if new_step_pos != self.stepper_pos: - steps = forward_d * inv_step_dist - step_offset = self.stepper_pos - start_pos * inv_step_dist + 0.5 - self.stepper_pos = new_step_pos - sdir = 1 - if steps < 0: - sdir = 0 - steps = -steps - step_offset = 1. - step_offset - mcu_time, so = self.stepper.prep_move(move_time, sdir) + step_dist = self.stepper.step_dist + mcu_time, so = self.stepper.prep_move(move_time) + step_offset = stepper_pos - start_pos * inv_step_dist - move_step_d = forward_d / steps - inv_move_step_d = 1. / move_step_d - - # Acceleration steps + # Acceleration steps + accel_multiplier = 2.0 * step_dist * inv_accel + if accel_d: #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel accel_time_offset = start_v * inv_accel accel_sqrt_offset = accel_time_offset**2 - accel_multiplier = 2.0 * move_step_d * inv_accel - accel_steps = accel_d * inv_move_step_d - step_offset = so.step_sqrt( + accel_steps = accel_d * inv_step_dist + count = so.step_sqrt( mcu_time - accel_time_offset, accel_steps, step_offset , accel_sqrt_offset, accel_multiplier) + stepper_pos += count + step_offset += count - accel_steps mcu_time += accel_t - # Cruising steps + # Cruising steps + if cruise_d: #t = pos/cruise_v - cruise_multiplier = move_step_d / cruise_v - cruise_steps = cruise_d * inv_move_step_d - step_offset = so.step_factor( + cruise_multiplier = step_dist / cruise_v + cruise_steps = cruise_d * inv_step_dist + count = so.step_factor( mcu_time, cruise_steps, step_offset, cruise_multiplier) + stepper_pos += count + step_offset += count - cruise_steps mcu_time += cruise_t - # Deceleration steps + # Deceleration steps + if decel_d: #t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel) decel_time_offset = decel_v * inv_accel decel_sqrt_offset = decel_time_offset**2 - decel_steps = decel_d * inv_move_step_d - so.step_sqrt( + decel_steps = decel_d * inv_step_dist + count = so.step_sqrt( mcu_time + decel_time_offset, decel_steps, step_offset , decel_sqrt_offset, -accel_multiplier) - - # Determine retract steps - start_pos = end_pos - end_pos -= retract_d - new_step_pos = int(end_pos*inv_step_dist + 0.5) - if new_step_pos != self.stepper_pos: - steps = retract_d * inv_step_dist - step_offset = start_pos * inv_step_dist - self.stepper_pos + 0.5 - self.stepper_pos = new_step_pos - mcu_time, so = self.stepper.prep_move( - move_time+accel_t+cruise_t+decel_t, 0) - - move_step_d = retract_d / steps - - # Acceleration steps + stepper_pos += count + step_offset += count - decel_steps + mcu_time += decel_t + # Retraction steps + if retract_d: #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel accel_time_offset = retract_v * inv_accel accel_sqrt_offset = accel_time_offset**2 - accel_multiplier = 2.0 * move_step_d * inv_accel - so.step_sqrt(mcu_time - accel_time_offset, steps, step_offset - , accel_sqrt_offset, accel_multiplier) - self.extrude_pos = end_pos + accel_steps = -retract_d * inv_step_dist + count = so.step_sqrt( + mcu_time - accel_time_offset, accel_steps, step_offset + , accel_sqrt_offset, accel_multiplier) + stepper_pos += count + + self.stepper_pos = stepper_pos + self.extrude_pos = start_pos + accel_d + cruise_d + decel_d - retract_d diff --git a/klippy/mcu.py b/klippy/mcu.py index 7e172f63..79537ed6 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -22,7 +22,7 @@ class MCU_stepper: self._oid = mcu.create_oid() step_pin, pullup, invert_step = parse_pin_extras(step_pin) dir_pin, pullup, self._invert_dir = parse_pin_extras(dir_pin) - self._sdir = -1 + self._need_reset = True self._mcu_freq = mcu.get_mcu_freq() min_stop_interval = int(min_stop_interval * self._mcu_freq) max_error = int(max_error * self._mcu_freq) @@ -39,30 +39,26 @@ class MCU_stepper: "reset_step_clock oid=%c clock=%u") ffi_main, self.ffi_lib = chelper.get_ffi() self._stepqueue = self.ffi_lib.stepcompress_alloc( - max_error, self._step_cmd.msgid, self._oid) + max_error, self._step_cmd.msgid + , self._dir_cmd.msgid, self._invert_dir, self._oid) self.print_to_mcu_time = mcu.print_to_mcu_time def get_oid(self): return self._oid def get_invert_dir(self): return self._invert_dir def note_stepper_stop(self): - self._sdir = -1 - def _reset_step_clock(self, clock): + self._need_reset = True + def check_reset(self, mcu_time): + if not self._need_reset: + return + self._need_reset = False + clock = int(mcu_time * self._mcu_freq) self.ffi_lib.stepcompress_reset(self._stepqueue, clock) data = (self._reset_cmd.msgid, self._oid, clock & 0xffffffff) self.ffi_lib.stepcompress_queue_msg(self._stepqueue, data, len(data)) - def set_next_step_dir(self, mcu_time, sdir): - if self._sdir == sdir: - return - if self._sdir == -1: - clock = int(mcu_time * self._mcu_freq) - self._reset_step_clock(clock) - self._sdir = sdir - data = (self._dir_cmd.msgid, self._oid, sdir ^ self._invert_dir) - self.ffi_lib.stepcompress_queue_msg(self._stepqueue, data, len(data)) - def step(self, mcu_time): + def step(self, mcu_time, sdir): clock = mcu_time * self._mcu_freq - self.ffi_lib.stepcompress_push(self._stepqueue, clock) + self.ffi_lib.stepcompress_push(self._stepqueue, clock, sdir) def step_sqrt(self, mcu_time, steps, step_offset, sqrt_offset, factor): clock = mcu_time * self._mcu_freq mcu_freq2 = self._mcu_freq**2 @@ -477,7 +473,7 @@ class Dummy_MCU_stepper: self._stepid, dirstr, countstr, addstr, interval)) def set_next_step_dir(self, dir): self._sdir = dir - def _reset_step_clock(self, clock): + def check_reset(self, clock): self._mcu.outfile.write("G6S%dT%d\n" % (self._stepid, clock)) def print_to_mcu_time(self, print_time): return self._mcu.print_to_mcu_time(print_time) diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c index ee8f32bc..c3f06761 100644 --- a/klippy/stepcompress.c +++ b/klippy/stepcompress.c @@ -35,7 +35,8 @@ struct stepcompress { // Message generation uint64_t last_step_clock; struct list_head msg_queue; - uint32_t queue_step_msgid, oid; + uint32_t queue_step_msgid, set_next_step_dir_msgid, oid; + int sdir, invert_sdir; }; @@ -268,14 +269,19 @@ safe_sqrt(double v) // Allocate a new 'stepcompress' object struct stepcompress * -stepcompress_alloc(uint32_t max_error, uint32_t queue_step_msgid, uint32_t oid) +stepcompress_alloc(uint32_t max_error, uint32_t queue_step_msgid + , uint32_t set_next_step_dir_msgid, uint32_t invert_sdir + , uint32_t oid) { struct stepcompress *sc = malloc(sizeof(*sc)); memset(sc, 0, sizeof(*sc)); sc->max_error = max_error; list_init(&sc->msg_queue); sc->queue_step_msgid = queue_step_msgid; + sc->set_next_step_dir_msgid = set_next_step_dir_msgid; sc->oid = oid; + sc->sdir = -1; + sc->invert_sdir = !!invert_sdir; return sc; } @@ -312,75 +318,102 @@ stepcompress_flush(struct stepcompress *sc, uint64_t move_clock) } } +// Send the set_next_step_dir command +static void +set_next_step_dir(struct stepcompress *sc, int sdir) +{ + sc->sdir = sdir; + stepcompress_flush(sc, UINT64_MAX); + uint32_t msg[3] = { + sc->set_next_step_dir_msgid, sc->oid, sdir ^ sc->invert_sdir + }; + struct queue_message *qm = message_alloc_and_encode(msg, 3); + qm->req_clock = sc->last_step_clock; + list_add_tail(&qm->node, &sc->msg_queue); +} + // Check if the internal queue needs to be expanded, and expand if so static inline void -check_expand(struct stepcompress *sc, int count) +check_expand(struct stepcompress *sc, int sdir, int count) { + if (sdir != sc->sdir) + set_next_step_dir(sc, sdir); if (sc->queue_next + count > sc->queue_end) expand_queue(sc, count); } // Schedule a step event at the specified step_clock time void -stepcompress_push(struct stepcompress *sc, double step_clock) +stepcompress_push(struct stepcompress *sc, double step_clock, int32_t sdir) { - check_expand(sc, 1); + sdir = !!sdir; + check_expand(sc, sdir, 1); step_clock += 0.5; *sc->queue_next++ = step_clock; } // Schedule 'steps' number of steps with a constant time between steps // using the formula: step_clock = clock_offset + step_num*factor -double +int32_t stepcompress_push_factor(struct stepcompress *sc , double steps, double step_offset , double clock_offset, double factor) { // Calculate number of steps to take - double ceil_steps = ceil(steps - step_offset); - double next_step_offset = ceil_steps - (steps - step_offset); - int count = ceil_steps; - if (count < 0 || count > 1000000) { - fprintf(stderr, "ERROR: push_factor invalid count %d %f %f %f %f\n" - , sc->oid, steps, step_offset, clock_offset, factor); - return next_step_offset; + int sdir = 1; + if (steps < 0) { + sdir = 0; + steps = -steps; + step_offset = -step_offset; } - check_expand(sc, count); + int count = steps + .5 - step_offset; + if (count <= 0 || count > 1000000) { + if (count && steps) + fprintf(stderr, "ERROR: push_factor invalid count %d %f %f %f %f\n" + , sc->oid, steps, step_offset, clock_offset, factor); + return 0; + } + check_expand(sc, sdir, count); // Calculate each step time uint64_t *qn = sc->queue_next, *end = &qn[count]; clock_offset += 0.5; - double pos = step_offset; + double pos = step_offset + .5; while (qn < end) { *qn++ = clock_offset + pos*factor; pos += 1.0; } sc->queue_next = qn; - return next_step_offset; + return sdir ? count : -count; } // Schedule 'steps' number of steps using the formula: // step_clock = clock_offset + sqrt(step_num*factor + sqrt_offset) -double +int32_t stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset , double clock_offset, double sqrt_offset, double factor) { // Calculate number of steps to take - double ceil_steps = ceil(steps - step_offset); - double next_step_offset = ceil_steps - (steps - step_offset); - int count = ceil_steps; - if (count < 0 || count > 1000000) { - fprintf(stderr, "ERROR: push_sqrt invalid count %d %f %f %f %f %f\n" - , sc->oid, steps, step_offset, clock_offset, sqrt_offset - , factor); - return next_step_offset; + int sdir = 1; + if (steps < 0) { + sdir = 0; + steps = -steps; + step_offset = -step_offset; } - check_expand(sc, count); + int count = steps + .5 - step_offset; + if (count <= 0 || count > 1000000) { + if (count && steps) + fprintf(stderr, "ERROR: push_sqrt invalid count %d %f %f %f %f %f\n" + , sc->oid, steps, step_offset, clock_offset, sqrt_offset + , factor); + return 0; + } + check_expand(sc, sdir, count); // Calculate each step time uint64_t *qn = sc->queue_next, *end = &qn[count]; clock_offset += 0.5; - double pos = step_offset + sqrt_offset/factor; + double pos = step_offset + .5 + sqrt_offset/factor; if (factor >= 0.0) while (qn < end) { *qn++ = clock_offset + safe_sqrt(pos*factor); @@ -392,7 +425,7 @@ stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset pos += 1.0; } sc->queue_next = qn; - return next_step_offset; + return sdir ? count : -count; } // Reset the internal state of the stepcompress object @@ -401,6 +434,7 @@ stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock) { stepcompress_flush(sc, UINT64_MAX); sc->last_step_clock = last_step_clock; + sc->sdir = -1; } // Queue an mcu command to go out in order with stepper commands diff --git a/klippy/stepper.py b/klippy/stepper.py index 8881c99f..57a5e9ae 100644 --- a/klippy/stepper.py +++ b/klippy/stepper.py @@ -69,9 +69,9 @@ class PrinterStepper: mcu_time = self.mcu_enable.print_to_mcu_time(move_time) self.mcu_enable.set_digital(mcu_time, enable) self.need_motor_enable = True - def prep_move(self, move_time, sdir): + def prep_move(self, move_time): mcu_time = self.mcu_stepper.print_to_mcu_time(move_time) - self.mcu_stepper.set_next_step_dir(mcu_time, sdir) + self.mcu_stepper.check_reset(mcu_time) if self.need_motor_enable: self.motor_enable(move_time, 1) self.need_motor_enable = False