From f66244576664730f6787d1700abd750c5b2f7b16 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Wed, 13 Sep 2017 08:59:26 -0400 Subject: [PATCH] stepcompress: Implement print time to clock conversion in C code Implement the conversion from print_time to the local mcu's clock within the C code. This simplifies the python code. Signed-off-by: Kevin O'Connor --- docs/Code_Overview.md | 17 +++++---- klippy/chelper.py | 2 ++ klippy/mcu.py | 21 +++++------ klippy/stepcompress.c | 81 +++++++++++++++++++++++++++---------------- 4 files changed, 69 insertions(+), 52 deletions(-) diff --git a/docs/Code_Overview.md b/docs/Code_Overview.md index d5292ac5..00a77928 100644 --- a/docs/Code_Overview.md +++ b/docs/Code_Overview.md @@ -148,7 +148,7 @@ provides further information on the mechanics of moves. start/crusing/end velocity, and distance traveled during acceleration/cruising/deceleration. All the information is stored in the Move() class and is in cartesian space in units of millimeters - and seconds. Times are stored relative to the start of the print. + and seconds. The move is then handed off to the kinematics classes: `Move.move() -> kin.move()` @@ -174,14 +174,13 @@ provides further information on the mechanics of moves. stepcompress_push_const()`, or for delta kinematics: `DeltaKinematics.move() -> MCU_Stepper.step_delta() -> stepcompress_push_delta()`. The MCU_Stepper code just performs unit - and axis transformation (seconds to clock ticks and millimeters to - step distances), and calls the C code. The C code calculates the - stepper step times for each movement and fills an array (struct - stepcompress.queue) with the corresponding micro-controller clock - counter times for every step. Here the "micro-controller clock - counter" value directly corresponds to the micro-controller's - hardware counter - it is relative to when the micro-controller was - last powered up. + and axis transformation (millimeters to step distances), and calls + the C code. The C code calculates the stepper step times for each + movement and fills an array (struct stepcompress.queue) with the + corresponding micro-controller clock counter times for every + step. Here the "micro-controller clock counter" value directly + corresponds to the micro-controller's hardware counter - it is + relative to when the micro-controller was last powered up. * The next major step is to compress the steps: `stepcompress_flush() -> compress_bisect_add()` (in stepcompress.c). This code generates diff --git a/klippy/chelper.py b/klippy/chelper.py index b7dce015..1f09e72d 100644 --- a/klippy/chelper.py +++ b/klippy/chelper.py @@ -36,6 +36,8 @@ defs_stepcompress = """ 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_set_time(struct steppersync *ss + , double time_offset, double mcu_freq); int steppersync_flush(struct steppersync *ss, uint64_t move_clock); """ diff --git a/klippy/mcu.py b/klippy/mcu.py index ba3381c1..b48a99f7 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -20,9 +20,8 @@ class MCU_stepper: self._dir_pin = self._invert_dir = None self._commanded_pos = 0 self._step_dist = self._inv_step_dist = 1. - self._velocity_factor = self._accel_factor = 0. self._mcu_position_offset = 0 - self._mcu_freq = self._min_stop_interval = 0. + self._min_stop_interval = 0. self._reset_cmd = self._get_position_cmd = None self._ffi_lib = self._stepqueue = None def get_mcu(self): @@ -38,9 +37,6 @@ class MCU_stepper: self._step_dist = step_dist self._inv_step_dist = 1. / step_dist def build_config(self): - self._mcu_freq = self._mcu.get_mcu_freq() - self._velocity_factor = 1. / (self._mcu_freq * self._step_dist) - self._accel_factor = 1. / (self._mcu_freq**2 * self._step_dist) max_error = self._mcu.get_max_stepper_error() min_stop_interval = max(0., self._min_stop_interval - max_error) self._mcu.add_config_cmd( @@ -107,29 +103,27 @@ class MCU_stepper: if ret: raise error("Internal error in stepcompress") def step(self, print_time, sdir): - clock = print_time * self._mcu_freq - count = self._ffi_lib.stepcompress_push(self._stepqueue, clock, sdir) + count = self._ffi_lib.stepcompress_push( + self._stepqueue, print_time, sdir) if count == STEPCOMPRESS_ERROR_RET: raise error("Internal error in stepcompress") self._commanded_pos += count def step_const(self, print_time, start_pos, dist, start_v, accel): - clock = print_time * self._mcu_freq inv_step_dist = self._inv_step_dist step_offset = self._commanded_pos - start_pos * inv_step_dist count = self._ffi_lib.stepcompress_push_const( - self._stepqueue, clock, step_offset, dist * inv_step_dist, - start_v * self._velocity_factor, accel * self._accel_factor) + self._stepqueue, print_time, step_offset, dist * inv_step_dist, + start_v * inv_step_dist, accel * inv_step_dist) if count == STEPCOMPRESS_ERROR_RET: raise error("Internal error in stepcompress") self._commanded_pos += count def step_delta(self, print_time, dist, start_v, accel , height_base, startxy_d, arm_d, movez_r): - clock = print_time * self._mcu_freq inv_step_dist = self._inv_step_dist height = self._commanded_pos - height_base * inv_step_dist count = self._ffi_lib.stepcompress_push_delta( - self._stepqueue, clock, dist * inv_step_dist, - start_v * self._velocity_factor, accel * self._accel_factor, + self._stepqueue, print_time, dist * inv_step_dist, + start_v * inv_step_dist, accel * inv_step_dist, height, startxy_d * inv_step_dist, arm_d * inv_step_dist, movez_r) if count == STEPCOMPRESS_ERROR_RET: raise error("Internal error in stepcompress") @@ -636,6 +630,7 @@ class MCU: self._steppersync = self._ffi_lib.steppersync_alloc( self.serial.serialqueue, self._stepqueues, len(self._stepqueues), move_count) + self._ffi_lib.steppersync_set_time(self._steppersync, 0., self._mcu_freq) for c in self._init_cmds: self.send(self.create_command(c)) # Config creation helpers diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c index 9c815b3a..eef07fdb 100644 --- a/klippy/stepcompress.c +++ b/klippy/stepcompress.c @@ -31,6 +31,7 @@ struct stepcompress { uint32_t *queue, *queue_end, *queue_pos, *queue_next; // Internal tracking uint32_t max_error; + double mcu_time_offset, mcu_freq; // Message generation uint64_t last_step_clock, homing_clock; struct list_head msg_queue; @@ -364,6 +365,15 @@ stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len) return 0; } +// Set the conversion rate of 'print_time' to mcu clock +static void +stepcompress_set_time(struct stepcompress *sc + , double time_offset, double mcu_freq) +{ + sc->mcu_time_offset = time_offset; + sc->mcu_freq = mcu_freq; +} + /**************************************************************** * Queue management @@ -380,12 +390,13 @@ struct queue_append { // 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) +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 = (clock_offset - (double)sc->last_step_clock) + adjust }; + .clock_offset = (print_clock - (double)sc->last_step_clock) + adjust }; } // Finalize a cursor created with queue_append_start() @@ -467,11 +478,10 @@ queue_append(struct queue_append *qa, double step_clock) // 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. +// units of step distance per time), _sd2 is step distance squared, _r +// is ratio (scalar usually between 0.0 and 1.0). Times are in +// seconds and acceleration is in units of step distance per second +// squared. // Wrapper around sqrt() to handle small negative numbers static double @@ -489,12 +499,12 @@ static inline double safe_sqrt(double v) { // Schedule a step event at the specified step_clock time int32_t -stepcompress_push(struct stepcompress *sc, double step_clock, int32_t sdir) +stepcompress_push(struct stepcompress *sc, double print_time, int32_t sdir) { int ret = set_next_step_dir(sc, !!sdir); if (ret) return ret; - struct queue_append qa = queue_append_start(sc, step_clock, 0.5); + struct queue_append qa = queue_append_start(sc, print_time, 0.5); ret = queue_append(&qa, 0.); if (ret) return ret; @@ -504,13 +514,13 @@ stepcompress_push(struct stepcompress *sc, double step_clock, int32_t sdir) // 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 +// step_time = print_time + step_num/start_sv // Otherwise it uses the formula: -// step_clock = (clock_offset + sqrt(2*step_num/accel + (start_sv/accel)**2) -// - start_sv/accel) +// step_time = (print_time + sqrt(2*step_num/accel + (start_sv/accel)**2) +// - start_sv/accel) int32_t stepcompress_push_const( - struct stepcompress *sc, double clock_offset + struct stepcompress *sc, double print_time , double step_offset, double steps, double start_sv, double accel) { // Calculate number of steps to take @@ -524,7 +534,7 @@ stepcompress_push_const( if (count <= 0 || count > 10000000) { if (count && steps) { errorf("push_const invalid count %d %f %f %f %f %f" - , sc->oid, clock_offset, step_offset, steps + , sc->oid, print_time, step_offset, steps , start_sv, accel); return ERROR_RET; } @@ -538,8 +548,8 @@ stepcompress_push_const( // Calculate each step time 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; + struct queue_append qa = queue_append_start(sc, print_time, .5); + double inv_cruise_sv = sc->mcu_freq / start_sv; double pos = (step_offset + .5) * inv_cruise_sv; while (count--) { ret = queue_append(&qa, pos); @@ -551,10 +561,10 @@ stepcompress_push_const( } else { // Move with constant acceleration double inv_accel = 1. / accel; - double accel_time = start_sv * inv_accel; + double accel_time = start_sv * inv_accel * sc->mcu_freq; struct queue_append qa = queue_append_start( - sc, clock_offset, 0.5 - accel_time); - double accel_multiplier = 2. * inv_accel; + sc, print_time, 0.5 - accel_time); + double accel_multiplier = 2. * inv_accel * sc->mcu_freq * sc->mcu_freq; double pos = (step_offset + .5)*accel_multiplier + accel_time*accel_time; while (count--) { double v = safe_sqrt(pos); @@ -572,7 +582,7 @@ stepcompress_push_const( static int32_t _stepcompress_push_delta( struct stepcompress *sc, int sdir - , double clock_offset, double move_sd, double start_sv, double accel + , double print_time, double move_sd, double start_sv, double accel , double height, double startxy_sd, double arm_sd, double movez_r) { // Calculate number of steps to take @@ -584,7 +594,7 @@ _stepcompress_push_delta( if (count <= 0 || count > 10000000) { if (count) { errorf("push_delta invalid count %d %d %f %f %f %f %f %f %f %f" - , sc->oid, count, clock_offset, move_sd, start_sv, accel + , sc->oid, count, print_time, move_sd, start_sv, accel , height, startxy_sd, arm_sd, movez_r); return ERROR_RET; } @@ -599,8 +609,8 @@ _stepcompress_push_delta( height += (sdir ? .5 : -.5); 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; + struct queue_append qa = queue_append_start(sc, print_time, .5); + double inv_cruise_sv = sc->mcu_freq / start_sv; if (!movez_r) { // Optimized case for common XY only moves (no Z movement) while (count--) { @@ -641,8 +651,8 @@ _stepcompress_push_delta( double inv_accel = 1. / 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; + sc, print_time, 0.5 - start_sv * inv_accel * sc->mcu_freq); + double accel_multiplier = 2. * inv_accel * sc->mcu_freq * sc->mcu_freq; while (count--) { double relheight = movexy_r*height - zoffset; double v = safe_sqrt(arm_sd2 - relheight*relheight); @@ -660,7 +670,7 @@ _stepcompress_push_delta( int32_t stepcompress_push_delta( - struct stepcompress *sc, double clock_offset, double move_sd + struct stepcompress *sc, double print_time, double move_sd , double start_sv, double accel , double height, double startxy_sd, double arm_sd, double movez_r) { @@ -668,22 +678,22 @@ stepcompress_push_delta( if (reversexy_sd <= 0.) // All steps are in down direction return _stepcompress_push_delta( - sc, 0, clock_offset, move_sd, start_sv, accel + sc, 0, print_time, move_sd, start_sv, accel , height, startxy_sd, arm_sd, movez_r); double movexy_r = movez_r ? sqrt(1. - movez_r*movez_r) : 1.; if (reversexy_sd >= move_sd * movexy_r) // All steps are in up direction return _stepcompress_push_delta( - sc, 1, clock_offset, move_sd, start_sv, accel + sc, 1, print_time, move_sd, start_sv, accel , height, startxy_sd, arm_sd, movez_r); // Steps in both up and down direction int res1 = _stepcompress_push_delta( - sc, 1, clock_offset, reversexy_sd / movexy_r, start_sv, accel + sc, 1, print_time, reversexy_sd / movexy_r, start_sv, accel , height, startxy_sd, arm_sd, movez_r); if (res1 == ERROR_RET) return res1; int res2 = _stepcompress_push_delta( - sc, 0, clock_offset, move_sd, start_sv, accel + sc, 0, print_time, move_sd, start_sv, accel , height + res1, startxy_sd, arm_sd, movez_r); if (res2 == ERROR_RET) return res2; @@ -747,6 +757,17 @@ steppersync_free(struct steppersync *ss) free(ss); } +// Set the conversion rate of 'print_time' to mcu clock +void +steppersync_set_time(struct steppersync *ss, double time_offset, double mcu_freq) +{ + int i; + for (i=0; isc_num; i++) { + struct stepcompress *sc = ss->sc_list[i]; + stepcompress_set_time(sc, time_offset, mcu_freq); + } +} + // Implement a binary heap algorithm to track when the next available // 'struct move' in the mcu will be available static void