mirror of https://github.com/Desuuuu/klipper.git
stepcompress: Do all step rounding in C code
Commitsf0cefebfand8f331f08changed 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 <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor
parent
79da35d023
commit
7554c7f694
|
|
@ -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
|
||||
accel_multiplier = 2.0 * move_step_d * inv_accel
|
||||
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_multiplier = 2.0 * move_step_d * inv_accel
|
||||
accel_steps = move.accel_r * steps
|
||||
step_offset = so.step_sqrt(
|
||||
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
|
||||
if move.cruise_r:
|
||||
#t = pos/cruise_v
|
||||
cruise_multiplier = move_step_d * inv_cruise_v
|
||||
cruise_steps = move.cruise_r * steps
|
||||
step_offset = so.step_factor(
|
||||
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
|
||||
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
|
||||
so.step_sqrt(
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
move_step_d = forward_d / steps
|
||||
inv_move_step_d = 1. / move_step_d
|
||||
step_dist = self.stepper.step_dist
|
||||
mcu_time, so = self.stepper.prep_move(move_time)
|
||||
step_offset = stepper_pos - start_pos * inv_step_dist
|
||||
|
||||
# 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
|
||||
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
|
||||
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_steps = -retract_d * inv_step_dist
|
||||
count = so.step_sqrt(
|
||||
mcu_time - accel_time_offset, accel_steps, step_offset
|
||||
, accel_sqrt_offset, accel_multiplier)
|
||||
self.extrude_pos = end_pos
|
||||
stepper_pos += count
|
||||
|
||||
self.stepper_pos = stepper_pos
|
||||
self.extrude_pos = start_pos + accel_d + cruise_d + decel_d - retract_d
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
int sdir = 1;
|
||||
if (steps < 0) {
|
||||
sdir = 0;
|
||||
steps = -steps;
|
||||
step_offset = -step_offset;
|
||||
}
|
||||
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 next_step_offset;
|
||||
return 0;
|
||||
}
|
||||
check_expand(sc, count);
|
||||
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) {
|
||||
int sdir = 1;
|
||||
if (steps < 0) {
|
||||
sdir = 0;
|
||||
steps = -steps;
|
||||
step_offset = -step_offset;
|
||||
}
|
||||
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 next_step_offset;
|
||||
return 0;
|
||||
}
|
||||
check_expand(sc, count);
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue