From 19ed67331d7c957cc26f750b7d3cce23688753e4 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Mon, 6 Feb 2017 11:37:03 -0500 Subject: [PATCH] stepcompress: Propagate errors back to python code Propagate error codes back to the python code and raise an exception on an error. Signed-off-by: Kevin O'Connor --- klippy/chelper.py | 12 ++-- klippy/mcu.py | 51 ++++++++----- klippy/stepcompress.c | 163 +++++++++++++++++++++++++++--------------- 3 files changed, 146 insertions(+), 80 deletions(-) diff --git a/klippy/chelper.py b/klippy/chelper.py index f979e2c7..b7baf061 100644 --- a/klippy/chelper.py +++ b/klippy/chelper.py @@ -16,7 +16,7 @@ defs_stepcompress = """ , uint32_t queue_step_msgid, uint32_t set_next_step_dir_msgid , uint32_t invert_sdir, uint32_t oid); void stepcompress_free(struct stepcompress *sc); - void stepcompress_push(struct stepcompress *sc, double step_clock + int stepcompress_push(struct stepcompress *sc, double step_clock , int32_t sdir); int32_t stepcompress_push_factor(struct stepcompress *sc , double steps, double step_offset @@ -32,16 +32,14 @@ defs_stepcompress = """ , double clock_offset, double dist, double start_pos , double accel_multiplier, double step_dist, double height , double closestxy_d, double closest_height2, double movez_r); - void stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock); - void stepcompress_set_homing(struct stepcompress *sc, uint64_t homing_clock); - void stepcompress_queue_msg(struct stepcompress *sc - , uint32_t *data, int len); - uint32_t stepcompress_get_errors(struct stepcompress *sc); + int stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock); + int stepcompress_set_homing(struct stepcompress *sc, uint64_t homing_clock); + int stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len); struct steppersync *steppersync_alloc(struct serialqueue *sq , struct stepcompress **sc_list, int sc_num, int move_num); void steppersync_free(struct steppersync *ss); - void steppersync_flush(struct steppersync *ss, uint64_t move_clock); + int steppersync_flush(struct steppersync *ss, uint64_t move_clock); """ defs_serialqueue = """ diff --git a/klippy/mcu.py b/klippy/mcu.py index eddc56a5..4fadc13d 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -19,6 +19,8 @@ def parse_pin_extras(pin, can_pullup=False): pin = pin[1:].strip() return pin, pullup, invert +STEPCOMPRESS_ERROR_RET = -989898989 + class MCU_stepper: def __init__(self, mcu, step_pin, dir_pin, min_stop_interval, max_error): self._mcu = mcu @@ -59,18 +61,31 @@ class MCU_stepper: def get_mcu_position(self): return self.commanded_position + self._mcu_position_offset def note_homing_start(self, homing_clock): - self.ffi_lib.stepcompress_set_homing(self._stepqueue, homing_clock) + ret = self.ffi_lib.stepcompress_set_homing(self._stepqueue, homing_clock) + if ret: + raise error("Internal error in stepcompress") def note_homing_finalized(self): - self.ffi_lib.stepcompress_set_homing(self._stepqueue, 0) - self.ffi_lib.stepcompress_reset(self._stepqueue, 0) + ret = self.ffi_lib.stepcompress_set_homing(self._stepqueue, 0) + if ret: + raise error("Internal error in stepcompress") + ret = self.ffi_lib.stepcompress_reset(self._stepqueue, 0) + if ret: + raise error("Internal error in stepcompress") def reset_step_clock(self, mcu_time): clock = int(mcu_time * self._mcu_freq) - self.ffi_lib.stepcompress_reset(self._stepqueue, clock) + ret = self.ffi_lib.stepcompress_reset(self._stepqueue, clock) + if ret: + raise error("Internal error in stepcompress") data = (self._reset_cmd.msgid, self._oid, clock & 0xffffffff) - self.ffi_lib.stepcompress_queue_msg(self._stepqueue, data, len(data)) + ret = self.ffi_lib.stepcompress_queue_msg( + self._stepqueue, data, len(data)) + if ret: + raise error("Internal error in stepcompress") def step(self, mcu_time, sdir): clock = mcu_time * self._mcu_freq - self.ffi_lib.stepcompress_push(self._stepqueue, clock, sdir) + ret = self.ffi_lib.stepcompress_push(self._stepqueue, clock, sdir) + if ret: + raise error("Internal error in stepcompress") if sdir: self.commanded_position += 1 else: @@ -81,12 +96,16 @@ class MCU_stepper: count = self.ffi_lib.stepcompress_push_sqrt( self._stepqueue, steps, step_offset, clock , sqrt_offset * mcu_freq2, factor * mcu_freq2) + if count == STEPCOMPRESS_ERROR_RET: + raise error("Internal error in stepcompress") self.commanded_position += count return count def step_factor(self, mcu_time, steps, step_offset, factor): clock = mcu_time * self._mcu_freq count = self.ffi_lib.stepcompress_push_factor( self._stepqueue, steps, step_offset, clock, factor * self._mcu_freq) + if count == STEPCOMPRESS_ERROR_RET: + raise error("Internal error in stepcompress") self.commanded_position += count return count def step_delta_const(self, mcu_time, dist, start_pos @@ -97,6 +116,8 @@ class MCU_stepper: self._stepqueue, clock, dist, start_pos , inv_velocity * self._mcu_freq, step_dist , height, closestxy_d, closest_height2, movez_r) + if count == STEPCOMPRESS_ERROR_RET: + raise error("Internal error in stepcompress") self.commanded_position += count return count def step_delta_accel(self, mcu_time, dist, start_pos @@ -108,10 +129,10 @@ class MCU_stepper: self._stepqueue, clock, dist, start_pos , accel_multiplier * mcu_freq2, step_dist , height, closestxy_d, closest_height2, movez_r) + if count == STEPCOMPRESS_ERROR_RET: + raise error("Internal error in stepcompress") self.commanded_position += count return count - def get_errors(self): - return self.ffi_lib.stepcompress_get_errors(self._stepqueue) class MCU_endstop: error = error @@ -390,15 +411,9 @@ class MCU: self.ffi_lib.steppersync_free(self._steppersync) self._steppersync = None def stats(self, eventtime): - stats = self.serial.stats(eventtime) - stats += " mcu_task_avg=%.06f mcu_task_stddev=%.06f" % ( + return "%s mcu_task_avg=%.06f mcu_task_stddev=%.06f" % ( + self.serial.stats(eventtime), self._mcu_tick_avg, self._mcu_tick_stddev) - err = 0 - for s in self._steppers: - err += s.get_errors() - if err: - stats += " step_errors=%d" % (err,) - return stats def force_shutdown(self): self.send(self._emergency_stop_cmd.encode()) def clear_shutdown(self): @@ -527,7 +542,9 @@ class MCU: return mcu_time = print_time + self._print_start_time clock = int(mcu_time * self._mcu_freq) - self.ffi_lib.steppersync_flush(self._steppersync, clock) + ret = self.ffi_lib.steppersync_flush(self._steppersync, clock) + if ret: + raise error("Internal error in stepcompress") def pause(self, waketime): return self._printer.reactor.pause(waketime) def __del__(self): diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c index e7d09572..5b401cbe 100644 --- a/klippy/stepcompress.c +++ b/klippy/stepcompress.c @@ -31,8 +31,6 @@ struct stepcompress { uint64_t *queue, *queue_end, *queue_pos, *queue_next; // Internal tracking uint32_t max_error; - // Error checking - uint32_t errors; // Message generation uint64_t last_step_clock, homing_clock; struct list_head msg_queue; @@ -224,27 +222,28 @@ compress_bisect_add(struct stepcompress *sc) * Step compress checking ****************************************************************/ +#define ERROR_RET -989898989 + // Verify that a given 'step_move' matches the actual step times -static void +static int check_line(struct stepcompress *sc, struct step_move move) { if (!CHECK_LINES) - return; + return 0; if (move.count == 1) { if (move.interval != (uint32_t)(*sc->queue_pos - sc->last_step_clock) || *sc->queue_pos < sc->last_step_clock) { errorf("Count 1 point out of range: %d %d %d" , move.interval, move.count, move.add); - sc->errors++; + return ERROR_RET; } - return; + return 0; } - int err = 0; if (!move.count || (!move.interval && !move.add) || move.interval >= 0x80000000) { errorf("Point out of range: %d %d %d" , move.interval, move.count, move.add); - err++; + return ERROR_RET; } uint32_t interval = move.interval, p = 0; uint16_t i; @@ -254,16 +253,16 @@ check_line(struct stepcompress *sc, struct step_move move) if (p < point.minp || p > point.maxp) { errorf("Point %d of %d: %d not in %d:%d" , i+1, move.count, p, point.minp, point.maxp); - err++; + return ERROR_RET; } if (interval >= 0x80000000) { errorf("Point %d of %d: interval overflow %d" , i+1, move.count, interval); - err++; + return ERROR_RET; } interval += move.add; } - sc->errors += err; + return 0; } @@ -317,14 +316,16 @@ stepcompress_free(struct stepcompress *sc) } // Convert previously scheduled steps into commands for the mcu -static void +static int stepcompress_flush(struct stepcompress *sc, uint64_t move_clock) { if (sc->queue_pos >= sc->queue_next) - return; + return 0; while (move_clock > sc->last_step_clock) { struct step_move move = compress_bisect_add(sc); - check_line(sc, move); + int ret = check_line(sc, move); + if (ret) + return ret; uint32_t msg[5] = { sc->queue_step_msgid, sc->oid, move.interval, move.count, move.add @@ -350,54 +351,70 @@ stepcompress_flush(struct stepcompress *sc, uint64_t move_clock) } sc->queue_pos += move.count; } + return 0; } // Send the set_next_step_dir command -static void +static int set_next_step_dir(struct stepcompress *sc, int sdir) { if (sc->sdir == sdir) - return; + return 0; sc->sdir = sdir; - stepcompress_flush(sc, UINT64_MAX); + int ret = stepcompress_flush(sc, UINT64_MAX); + if (ret) + return ret; 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->homing_clock ?: sc->last_step_clock; list_add_tail(&qm->node, &sc->msg_queue); + return 0; } // Check if the internal queue needs to be expanded, and expand if so -static void +static int _check_expand(struct stepcompress *sc, uint64_t *qn) { sc->queue_next = qn; - if (qn - sc->queue_pos > 65535 + 2000) + if (qn - sc->queue_pos > 65535 + 2000) { // No point in keeping more than 64K steps in memory - stepcompress_flush(sc, *(qn - 65535)); + int ret = stepcompress_flush(sc, *(qn - 65535)); + if (ret) + return ret; + } expand_queue(sc, 1); + return 0; } -static inline void +static inline int check_expand(struct stepcompress *sc, uint64_t **pqn, uint64_t **pqend) { if (likely(*pqn < *pqend)) - return; - _check_expand(sc, *pqn); + return 0; + int ret = _check_expand(sc, *pqn); + if (ret) + return ret; *pqn = sc->queue_next; *pqend = sc->queue_end; + return 0; } // Schedule a step event at the specified step_clock time -void +int stepcompress_push(struct stepcompress *sc, double step_clock, int32_t sdir) { - set_next_step_dir(sc, !!sdir); + int ret = set_next_step_dir(sc, !!sdir); + if (ret) + return ret; step_clock += 0.5; uint64_t *qn = sc->queue_next, *qend = sc->queue_end; - check_expand(sc, &qn, &qend); + ret = check_expand(sc, &qn, &qend); + if (ret) + return ret; *qn++ = step_clock; sc->queue_next = qn; + return 0; } // Schedule 'steps' number of steps with a constant time between steps @@ -416,12 +433,16 @@ stepcompress_push_factor(struct stepcompress *sc } int count = steps + .5 - step_offset; if (count <= 0 || count > 10000000) { - if (count && steps) + if (count && steps) { errorf("push_factor invalid count %d %f %f %f %f" , sc->oid, steps, step_offset, clock_offset, factor); + return ERROR_RET; + } return 0; } - set_next_step_dir(sc, sdir); + int ret = set_next_step_dir(sc, sdir); + if (ret) + return ret; int res = sdir ? count : -count; // Calculate each step time @@ -429,7 +450,9 @@ stepcompress_push_factor(struct stepcompress *sc double pos = step_offset + .5; uint64_t *qn = sc->queue_next, *qend = sc->queue_end; while (count--) { - check_expand(sc, &qn, &qend); + int ret = check_expand(sc, &qn, &qend); + if (ret) + return ret; *qn++ = clock_offset + pos*factor; pos += 1.0; } @@ -452,13 +475,17 @@ stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset } int count = steps + .5 - step_offset; if (count <= 0 || count > 10000000) { - if (count && steps) + if (count && steps) { errorf("push_sqrt invalid count %d %f %f %f %f %f" , sc->oid, steps, step_offset, clock_offset, sqrt_offset , factor); + return ERROR_RET; + } return 0; } - set_next_step_dir(sc, sdir); + int ret = set_next_step_dir(sc, sdir); + if (ret) + return ret; int res = sdir ? count : -count; // Calculate each step time @@ -466,7 +493,9 @@ stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset double pos = step_offset + .5 + sqrt_offset/factor; uint64_t *qn = sc->queue_next, *qend = sc->queue_end; while (count--) { - check_expand(sc, &qn, &qend); + int ret = check_expand(sc, &qn, &qend); + if (ret) + return ret; double v = safe_sqrt(pos*factor); *qn++ = clock_offset + (factor >= 0. ? v : -v); pos += 1.0; @@ -488,13 +517,17 @@ stepcompress_push_delta_const( double end_height = safe_sqrt(closest_height2 - reldist*reldist); int count = (end_height - height + movez_r*dist) / step_dist + .5; if (count <= 0 || count > 10000000) { - if (count) + if (count) { errorf("push_delta_const invalid count %d %d %f %f %f %f %f %f %f %f" , sc->oid, count, clock_offset, dist, step_dist, start_pos , closest_height2, height, movez_r, inv_velocity); + return ERROR_RET; + } return 0; } - set_next_step_dir(sc, step_dist > 0.); + int ret = set_next_step_dir(sc, step_dist > 0.); + if (ret) + return ret; int res = step_dist > 0. ? count : -count; // Calculate each step time @@ -505,7 +538,9 @@ stepcompress_push_delta_const( if (!movez_r) { // Optmized case for common XY only moves (no Z movement) while (count--) { - check_expand(sc, &qn, &qend); + int ret = check_expand(sc, &qn, &qend); + if (ret) + return ret; double v = safe_sqrt(closest_height2 - height*height); double pos = start_pos + (step_dist > 0. ? -v : v); *qn++ = clock_offset + pos * inv_velocity; @@ -515,7 +550,9 @@ stepcompress_push_delta_const( // Optmized case for Z only moves double v = (step_dist > 0. ? -end_height : end_height); while (count--) { - check_expand(sc, &qn, &qend); + int ret = check_expand(sc, &qn, &qend); + if (ret) + return ret; double pos = start_pos + movez_r*height + v; *qn++ = clock_offset + pos * inv_velocity; height += step_dist; @@ -523,7 +560,9 @@ stepcompress_push_delta_const( } else { // General case (handles XY+Z moves) while (count--) { - check_expand(sc, &qn, &qend); + int ret = check_expand(sc, &qn, &qend); + if (ret) + return ret; double relheight = movexy_r*height - movez_r*closestxy_d; double v = safe_sqrt(closest_height2 - relheight*relheight); double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v); @@ -548,13 +587,17 @@ stepcompress_push_delta_accel( double end_height = safe_sqrt(closest_height2 - reldist*reldist); int count = (end_height - height + movez_r*dist) / step_dist + .5; if (count <= 0 || count > 10000000) { - if (count) + if (count) { errorf("push_delta_accel invalid count %d %d %f %f %f %f %f %f %f %f" , sc->oid, count, clock_offset, dist, step_dist, start_pos , closest_height2, height, movez_r, accel_multiplier); + return ERROR_RET; + } return 0; } - set_next_step_dir(sc, step_dist > 0.); + int ret = set_next_step_dir(sc, step_dist > 0.); + if (ret) + return ret; int res = step_dist > 0. ? count : -count; // Calculate each step time @@ -563,7 +606,9 @@ stepcompress_push_delta_accel( height += .5 * step_dist; uint64_t *qn = sc->queue_next, *qend = sc->queue_end; while (count--) { - check_expand(sc, &qn, &qend); + int ret = check_expand(sc, &qn, &qend); + if (ret) + return ret; double relheight = movexy_r*height - movez_r*closestxy_d; double v = safe_sqrt(closest_height2 - relheight*relheight); double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v); @@ -576,38 +621,40 @@ stepcompress_push_delta_accel( } // Reset the internal state of the stepcompress object -void +int stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock) { - stepcompress_flush(sc, UINT64_MAX); + int ret = stepcompress_flush(sc, UINT64_MAX); + if (ret) + return ret; sc->last_step_clock = last_step_clock; sc->sdir = -1; + return 0; } // Indicate the stepper is in homing mode (or done homing if zero) -void +int stepcompress_set_homing(struct stepcompress *sc, uint64_t homing_clock) { - stepcompress_flush(sc, UINT64_MAX); + int ret = stepcompress_flush(sc, UINT64_MAX); + if (ret) + return ret; sc->homing_clock = homing_clock; + return 0; } // Queue an mcu command to go out in order with stepper commands -void +int stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len) { - stepcompress_flush(sc, UINT64_MAX); + int ret = stepcompress_flush(sc, UINT64_MAX); + if (ret) + return ret; struct queue_message *qm = message_alloc_and_encode(data, len); qm->req_clock = sc->homing_clock ?: sc->last_step_clock; list_add_tail(&qm->node, &sc->msg_queue); -} - -// Return the count of internal errors found -uint32_t -stepcompress_get_errors(struct stepcompress *sc) -{ - return sc->errors; + return 0; } @@ -693,13 +740,16 @@ heap_replace(struct steppersync *ss, uint64_t req_clock) } // Find and transmit any scheduled steps prior to the given 'move_clock' -void +int steppersync_flush(struct steppersync *ss, uint64_t move_clock) { // Flush each stepcompress to the specified move_clock int i; - for (i=0; isc_num; i++) - stepcompress_flush(ss->sc_list[i], move_clock); + for (i=0; isc_num; i++) { + int ret = stepcompress_flush(ss->sc_list[i], move_clock); + if (ret) + return ret; + } // Order commands by the reqclock of each pending command struct list_head msgs; @@ -739,4 +789,5 @@ steppersync_flush(struct steppersync *ss, uint64_t move_clock) // Transmit commands if (!list_empty(&msgs)) serialqueue_send_batch(ss->sq, ss->cq, &msgs); + return 0; }