3diy
/
klipper
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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 <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2017-02-06 11:37:03 -05:00
parent 667b72870f
commit 19ed67331d
3 changed files with 146 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
klippy/chelper.py
View File

@ -16,7 +16,7 @@ defs_stepcompress = """
, uint32_t queue_step_msgid, uint32_t set_next_step_dir_msgid , uint32_t queue_step_msgid, uint32_t set_next_step_dir_msgid
, uint32_t invert_sdir, uint32_t oid); , uint32_t invert_sdir, uint32_t oid);
void stepcompress_free(struct stepcompress *sc); 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 sdir);
int32_t stepcompress_push_factor(struct stepcompress *sc int32_t stepcompress_push_factor(struct stepcompress *sc
, double steps, double step_offset , double steps, double step_offset
@ -32,16 +32,14 @@ defs_stepcompress = """
, double clock_offset, double dist, double start_pos , double clock_offset, double dist, double start_pos
, double accel_multiplier, double step_dist, double height , double accel_multiplier, double step_dist, double height
, double closestxy_d, double closest_height2, double movez_r); , double closestxy_d, double closest_height2, double movez_r);
void stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock); int stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock);
void stepcompress_set_homing(struct stepcompress *sc, uint64_t homing_clock); int stepcompress_set_homing(struct stepcompress *sc, uint64_t homing_clock);
void stepcompress_queue_msg(struct stepcompress *sc int stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len);
, uint32_t *data, int len);
uint32_t stepcompress_get_errors(struct stepcompress *sc);
struct steppersync *steppersync_alloc(struct serialqueue *sq struct steppersync *steppersync_alloc(struct serialqueue *sq
, struct stepcompress **sc_list, int sc_num, int move_num); , struct stepcompress **sc_list, int sc_num, int move_num);
void steppersync_free(struct steppersync *ss); 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 = """ defs_serialqueue = """

51
klippy/mcu.py
View File

@ -19,6 +19,8 @@ def parse_pin_extras(pin, can_pullup=False):
pin = pin[1:].strip() pin = pin[1:].strip()
return pin, pullup, invert return pin, pullup, invert
STEPCOMPRESS_ERROR_RET = -989898989
class MCU_stepper: class MCU_stepper:
def __init__(self, mcu, step_pin, dir_pin, min_stop_interval, max_error): def __init__(self, mcu, step_pin, dir_pin, min_stop_interval, max_error):
self._mcu = mcu self._mcu = mcu
@ -59,18 +61,31 @@ class MCU_stepper:
def get_mcu_position(self): def get_mcu_position(self):
return self.commanded_position + self._mcu_position_offset return self.commanded_position + self._mcu_position_offset
def note_homing_start(self, homing_clock): 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): def note_homing_finalized(self):
self.ffi_lib.stepcompress_set_homing(self._stepqueue, 0) ret = self.ffi_lib.stepcompress_set_homing(self._stepqueue, 0)
self.ffi_lib.stepcompress_reset(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): def reset_step_clock(self, mcu_time):
clock = int(mcu_time * self._mcu_freq) 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) 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): def step(self, mcu_time, sdir):
clock = mcu_time * self._mcu_freq 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: if sdir:
self.commanded_position += 1 self.commanded_position += 1
else: else:
@ -81,12 +96,16 @@ class MCU_stepper:
count = self.ffi_lib.stepcompress_push_sqrt( count = self.ffi_lib.stepcompress_push_sqrt(
self._stepqueue, steps, step_offset, clock self._stepqueue, steps, step_offset, clock
, sqrt_offset * mcu_freq2, factor * mcu_freq2) , sqrt_offset * mcu_freq2, factor * mcu_freq2)
if count == STEPCOMPRESS_ERROR_RET:
raise error("Internal error in stepcompress")
self.commanded_position += count self.commanded_position += count
return count return count
def step_factor(self, mcu_time, steps, step_offset, factor): def step_factor(self, mcu_time, steps, step_offset, factor):
clock = mcu_time * self._mcu_freq clock = mcu_time * self._mcu_freq
count = self.ffi_lib.stepcompress_push_factor( count = self.ffi_lib.stepcompress_push_factor(
self._stepqueue, steps, step_offset, clock, factor * self._mcu_freq) 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 self.commanded_position += count
return count return count
def step_delta_const(self, mcu_time, dist, start_pos def step_delta_const(self, mcu_time, dist, start_pos
@ -97,6 +116,8 @@ class MCU_stepper:
self._stepqueue, clock, dist, start_pos self._stepqueue, clock, dist, start_pos
, inv_velocity * self._mcu_freq, step_dist , inv_velocity * self._mcu_freq, step_dist
, height, closestxy_d, closest_height2, movez_r) , height, closestxy_d, closest_height2, movez_r)
if count == STEPCOMPRESS_ERROR_RET:
raise error("Internal error in stepcompress")
self.commanded_position += count self.commanded_position += count
return count return count
def step_delta_accel(self, mcu_time, dist, start_pos def step_delta_accel(self, mcu_time, dist, start_pos
@ -108,10 +129,10 @@ class MCU_stepper:
self._stepqueue, clock, dist, start_pos self._stepqueue, clock, dist, start_pos
, accel_multiplier * mcu_freq2, step_dist , accel_multiplier * mcu_freq2, step_dist
, height, closestxy_d, closest_height2, movez_r) , height, closestxy_d, closest_height2, movez_r)
if count == STEPCOMPRESS_ERROR_RET:
raise error("Internal error in stepcompress")
self.commanded_position += count self.commanded_position += count
return count return count
def get_errors(self):
return self.ffi_lib.stepcompress_get_errors(self._stepqueue)
class MCU_endstop: class MCU_endstop:
error = error error = error
@ -390,15 +411,9 @@ class MCU:
self.ffi_lib.steppersync_free(self._steppersync) self.ffi_lib.steppersync_free(self._steppersync)
self._steppersync = None self._steppersync = None
def stats(self, eventtime): def stats(self, eventtime):
stats = self.serial.stats(eventtime) return "%s mcu_task_avg=%.06f mcu_task_stddev=%.06f" % (
stats += " mcu_task_avg=%.06f mcu_task_stddev=%.06f" % ( self.serial.stats(eventtime),
self._mcu_tick_avg, self._mcu_tick_stddev) 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): def force_shutdown(self):
self.send(self._emergency_stop_cmd.encode()) self.send(self._emergency_stop_cmd.encode())
def clear_shutdown(self): def clear_shutdown(self):
@ -527,7 +542,9 @@ class MCU:
return return
mcu_time = print_time + self._print_start_time mcu_time = print_time + self._print_start_time
clock = int(mcu_time * self._mcu_freq) 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): def pause(self, waketime):
return self._printer.reactor.pause(waketime) return self._printer.reactor.pause(waketime)
def __del__(self): def __del__(self):

165
klippy/stepcompress.c
View File

@ -31,8 +31,6 @@ struct stepcompress {
uint64_t *queue, *queue_end, *queue_pos, *queue_next; uint64_t *queue, *queue_end, *queue_pos, *queue_next;
// Internal tracking // Internal tracking
uint32_t max_error; uint32_t max_error;
// Error checking
uint32_t errors;
// Message generation // Message generation
uint64_t last_step_clock, homing_clock; uint64_t last_step_clock, homing_clock;
struct list_head msg_queue; struct list_head msg_queue;
@ -224,27 +222,28 @@ compress_bisect_add(struct stepcompress *sc)
* Step compress checking * Step compress checking
****************************************************************/ ****************************************************************/
#define ERROR_RET -989898989
// Verify that a given 'step_move' matches the actual step times // Verify that a given 'step_move' matches the actual step times
static void static int
check_line(struct stepcompress *sc, struct step_move move) check_line(struct stepcompress *sc, struct step_move move)
{ {
if (!CHECK_LINES) if (!CHECK_LINES)
return; return 0;
if (move.count == 1) { if (move.count == 1) {
if (move.interval != (uint32_t)(*sc->queue_pos - sc->last_step_clock) if (move.interval != (uint32_t)(*sc->queue_pos - sc->last_step_clock)
|| *sc->queue_pos < sc->last_step_clock) { || *sc->queue_pos < sc->last_step_clock) {
errorf("Count 1 point out of range: %d %d %d" errorf("Count 1 point out of range: %d %d %d"
, move.interval, move.count, move.add); , move.interval, move.count, move.add);
sc->errors++; return ERROR_RET;
} }
return; return 0;
} }
int err = 0;
if (!move.count || (!move.interval && !move.add) if (!move.count || (!move.interval && !move.add)
|| move.interval >= 0x80000000) { || move.interval >= 0x80000000) {
errorf("Point out of range: %d %d %d" errorf("Point out of range: %d %d %d"
, move.interval, move.count, move.add); , move.interval, move.count, move.add);
err++; return ERROR_RET;
} }
uint32_t interval = move.interval, p = 0; uint32_t interval = move.interval, p = 0;
uint16_t i; uint16_t i;
@ -254,16 +253,16 @@ check_line(struct stepcompress *sc, struct step_move move)
if (p < point.minp || p > point.maxp) { if (p < point.minp || p > point.maxp) {
errorf("Point %d of %d: %d not in %d:%d" errorf("Point %d of %d: %d not in %d:%d"
, i+1, move.count, p, point.minp, point.maxp); , i+1, move.count, p, point.minp, point.maxp);
err++; return ERROR_RET;
} }
if (interval >= 0x80000000) { if (interval >= 0x80000000) {
errorf("Point %d of %d: interval overflow %d" errorf("Point %d of %d: interval overflow %d"
, i+1, move.count, interval); , i+1, move.count, interval);
err++; return ERROR_RET;
} }
interval += move.add; 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 // Convert previously scheduled steps into commands for the mcu
static void static int
stepcompress_flush(struct stepcompress *sc, uint64_t move_clock) stepcompress_flush(struct stepcompress *sc, uint64_t move_clock)
{ {
if (sc->queue_pos >= sc->queue_next) if (sc->queue_pos >= sc->queue_next)
return; return 0;
while (move_clock > sc->last_step_clock) { while (move_clock > sc->last_step_clock) {
struct step_move move = compress_bisect_add(sc); 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] = { uint32_t msg[5] = {
sc->queue_step_msgid, sc->oid, move.interval, move.count, move.add 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; sc->queue_pos += move.count;
} }
return 0;
} }
// Send the set_next_step_dir command // Send the set_next_step_dir command
static void static int
set_next_step_dir(struct stepcompress *sc, int sdir) set_next_step_dir(struct stepcompress *sc, int sdir)
{ {
if (sc->sdir == sdir) if (sc->sdir == sdir)
return; return 0;
sc->sdir = sdir; sc->sdir = sdir;
stepcompress_flush(sc, UINT64_MAX); int ret = stepcompress_flush(sc, UINT64_MAX);
if (ret)
return ret;
uint32_t msg[3] = { uint32_t msg[3] = {
sc->set_next_step_dir_msgid, sc->oid, sdir ^ sc->invert_sdir sc->set_next_step_dir_msgid, sc->oid, sdir ^ sc->invert_sdir
}; };
struct queue_message *qm = message_alloc_and_encode(msg, 3); struct queue_message *qm = message_alloc_and_encode(msg, 3);
qm->req_clock = sc->homing_clock ?: sc->last_step_clock; qm->req_clock = sc->homing_clock ?: sc->last_step_clock;
list_add_tail(&qm->node, &sc->msg_queue); list_add_tail(&qm->node, &sc->msg_queue);
return 0;
} }
// Check if the internal queue needs to be expanded, and expand if so // 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) _check_expand(struct stepcompress *sc, uint64_t *qn)
{ {
sc->queue_next = 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 // No point in keeping more than 64K steps in memory
stepcompress_flush(sc, *(qn - 65535)); int ret = stepcompress_flush(sc, *(qn - 65535));
expand_queue(sc, 1); if (ret)
return ret;
} }
static inline void expand_queue(sc, 1);
return 0;
}
static inline int
check_expand(struct stepcompress *sc, uint64_t **pqn, uint64_t **pqend) check_expand(struct stepcompress *sc, uint64_t **pqn, uint64_t **pqend)
{ {
if (likely(*pqn < *pqend)) if (likely(*pqn < *pqend))
return; return 0;
_check_expand(sc, *pqn); int ret = _check_expand(sc, *pqn);
if (ret)
return ret;
*pqn = sc->queue_next; *pqn = sc->queue_next;
*pqend = sc->queue_end; *pqend = sc->queue_end;
return 0;
} }
// Schedule a step event at the specified step_clock time // Schedule a step event at the specified step_clock time
void int
stepcompress_push(struct stepcompress *sc, double step_clock, int32_t sdir) 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; step_clock += 0.5;
uint64_t *qn = sc->queue_next, *qend = sc->queue_end; 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; *qn++ = step_clock;
sc->queue_next = qn; sc->queue_next = qn;
return 0;
} }
// Schedule 'steps' number of steps with a constant time between steps // 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; int count = steps + .5 - step_offset;
if (count <= 0 || count > 10000000) { if (count <= 0 || count > 10000000) {
if (count && steps) if (count && steps) {
errorf("push_factor invalid count %d %f %f %f %f" errorf("push_factor invalid count %d %f %f %f %f"
, sc->oid, steps, step_offset, clock_offset, factor); , sc->oid, steps, step_offset, clock_offset, factor);
return ERROR_RET;
}
return 0; 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; int res = sdir ? count : -count;
// Calculate each step time // Calculate each step time
@ -429,7 +450,9 @@ stepcompress_push_factor(struct stepcompress *sc
double pos = step_offset + .5; double pos = step_offset + .5;
uint64_t *qn = sc->queue_next, *qend = sc->queue_end; uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
while (count--) { while (count--) {
check_expand(sc, &qn, &qend); int ret = check_expand(sc, &qn, &qend);
if (ret)
return ret;
*qn++ = clock_offset + pos*factor; *qn++ = clock_offset + pos*factor;
pos += 1.0; pos += 1.0;
} }
@ -452,13 +475,17 @@ stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset
} }
int count = steps + .5 - step_offset; int count = steps + .5 - step_offset;
if (count <= 0 || count > 10000000) { if (count <= 0 || count > 10000000) {
if (count && steps) if (count && steps) {
errorf("push_sqrt invalid count %d %f %f %f %f %f" errorf("push_sqrt invalid count %d %f %f %f %f %f"
, sc->oid, steps, step_offset, clock_offset, sqrt_offset , sc->oid, steps, step_offset, clock_offset, sqrt_offset
, factor); , factor);
return ERROR_RET;
}
return 0; 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; int res = sdir ? count : -count;
// Calculate each step time // 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; double pos = step_offset + .5 + sqrt_offset/factor;
uint64_t *qn = sc->queue_next, *qend = sc->queue_end; uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
while (count--) { while (count--) {
check_expand(sc, &qn, &qend); int ret = check_expand(sc, &qn, &qend);
if (ret)
return ret;
double v = safe_sqrt(pos*factor); double v = safe_sqrt(pos*factor);
*qn++ = clock_offset + (factor >= 0. ? v : -v); *qn++ = clock_offset + (factor >= 0. ? v : -v);
pos += 1.0; pos += 1.0;
@ -488,13 +517,17 @@ stepcompress_push_delta_const(
double end_height = safe_sqrt(closest_height2 - reldist*reldist); double end_height = safe_sqrt(closest_height2 - reldist*reldist);
int count = (end_height - height + movez_r*dist) / step_dist + .5; int count = (end_height - height + movez_r*dist) / step_dist + .5;
if (count <= 0 || count > 10000000) { 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" 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 , sc->oid, count, clock_offset, dist, step_dist, start_pos
, closest_height2, height, movez_r, inv_velocity); , closest_height2, height, movez_r, inv_velocity);
return ERROR_RET;
}
return 0; 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; int res = step_dist > 0. ? count : -count;
// Calculate each step time // Calculate each step time
@ -505,7 +538,9 @@ stepcompress_push_delta_const(
if (!movez_r) { if (!movez_r) {
// Optmized case for common XY only moves (no Z movement) // Optmized case for common XY only moves (no Z movement)
while (count--) { 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 v = safe_sqrt(closest_height2 - height*height);
double pos = start_pos + (step_dist > 0. ? -v : v); double pos = start_pos + (step_dist > 0. ? -v : v);
*qn++ = clock_offset + pos * inv_velocity; *qn++ = clock_offset + pos * inv_velocity;
@ -515,7 +550,9 @@ stepcompress_push_delta_const(
// Optmized case for Z only moves // Optmized case for Z only moves
double v = (step_dist > 0. ? -end_height : end_height); double v = (step_dist > 0. ? -end_height : end_height);
while (count--) { 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; double pos = start_pos + movez_r*height + v;
*qn++ = clock_offset + pos * inv_velocity; *qn++ = clock_offset + pos * inv_velocity;
height += step_dist; height += step_dist;
@ -523,7 +560,9 @@ stepcompress_push_delta_const(
} else { } else {
// General case (handles XY+Z moves) // General case (handles XY+Z moves)
while (count--) { 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 relheight = movexy_r*height - movez_r*closestxy_d;
double v = safe_sqrt(closest_height2 - relheight*relheight); double v = safe_sqrt(closest_height2 - relheight*relheight);
double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v); 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); double end_height = safe_sqrt(closest_height2 - reldist*reldist);
int count = (end_height - height + movez_r*dist) / step_dist + .5; int count = (end_height - height + movez_r*dist) / step_dist + .5;
if (count <= 0 || count > 10000000) { 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" 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 , sc->oid, count, clock_offset, dist, step_dist, start_pos
, closest_height2, height, movez_r, accel_multiplier); , closest_height2, height, movez_r, accel_multiplier);
return ERROR_RET;
}
return 0; 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; int res = step_dist > 0. ? count : -count;
// Calculate each step time // Calculate each step time
@ -563,7 +606,9 @@ stepcompress_push_delta_accel(
height += .5 * step_dist; height += .5 * step_dist;
uint64_t *qn = sc->queue_next, *qend = sc->queue_end; uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
while (count--) { 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 relheight = movexy_r*height - movez_r*closestxy_d;
double v = safe_sqrt(closest_height2 - relheight*relheight); double v = safe_sqrt(closest_height2 - relheight*relheight);
double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v); 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 // Reset the internal state of the stepcompress object
void int
stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock) 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->last_step_clock = last_step_clock;
sc->sdir = -1; sc->sdir = -1;
return 0;
} }
// Indicate the stepper is in homing mode (or done homing if zero) // 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_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; sc->homing_clock = homing_clock;
return 0;
} }
// Queue an mcu command to go out in order with stepper commands // 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_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); struct queue_message *qm = message_alloc_and_encode(data, len);
qm->req_clock = sc->homing_clock ?: sc->last_step_clock; qm->req_clock = sc->homing_clock ?: sc->last_step_clock;
list_add_tail(&qm->node, &sc->msg_queue); list_add_tail(&qm->node, &sc->msg_queue);
} return 0;
// Return the count of internal errors found
uint32_t
stepcompress_get_errors(struct stepcompress *sc)
{
return sc->errors;
} }
@ -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' // Find and transmit any scheduled steps prior to the given 'move_clock'
void int
steppersync_flush(struct steppersync *ss, uint64_t move_clock) steppersync_flush(struct steppersync *ss, uint64_t move_clock)
{ {
// Flush each stepcompress to the specified move_clock // Flush each stepcompress to the specified move_clock
int i; int i;
for (i=0; i<ss->sc_num; i++) for (i=0; i<ss->sc_num; i++) {
stepcompress_flush(ss->sc_list[i], move_clock); int ret = stepcompress_flush(ss->sc_list[i], move_clock);
if (ret)
return ret;
}
// Order commands by the reqclock of each pending command // Order commands by the reqclock of each pending command
struct list_head msgs; struct list_head msgs;
@ -739,4 +789,5 @@ steppersync_flush(struct steppersync *ss, uint64_t move_clock)
// Transmit commands // Transmit commands
if (!list_empty(&msgs)) if (!list_empty(&msgs))
serialqueue_send_batch(ss->sq, ss->cq, &msgs); serialqueue_send_batch(ss->sq, ss->cq, &msgs);
return 0;
} }