mirror of https://github.com/Desuuuu/klipper.git
stepcompress: Implement a step+dir+step filter
Some stepper motor drivers do not respond well to rapid "step + direction change + step" events. In particular, it is believed this can cause "over current" events on the tmc2208 drivers when they are in "stealthchop" mode. Detect these events and remove them from the generated step times. Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
parent
d86bf0b927
commit
acd165cbea
|
@ -13,6 +13,11 @@
|
|||
#include "stepcompress.h" // queue_append_start
|
||||
#include "trapq.h" // struct move
|
||||
|
||||
|
||||
/****************************************************************
|
||||
* Main iterative solver
|
||||
****************************************************************/
|
||||
|
||||
struct timepos {
|
||||
double time, position;
|
||||
};
|
||||
|
@ -66,7 +71,7 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m
|
|||
double start = move_start - m->print_time, end = move_end - m->print_time;
|
||||
struct timepos last = { start, sk->commanded_pos }, low = last, high = last;
|
||||
double seek_time_delta = SEEK_TIME_RESET;
|
||||
int sdir = !!stepcompress_get_step_dir(sk->sc), is_dir_change = 0;
|
||||
int sdir = stepcompress_get_step_dir(sk->sc), is_dir_change = 0;
|
||||
for (;;) {
|
||||
double diff = high.position - last.position, dist = sdir ? diff : -diff;
|
||||
if (dist >= half_step) {
|
||||
|
@ -90,6 +95,9 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m
|
|||
if (low.time < high.time)
|
||||
// The existing search range is still valid
|
||||
continue;
|
||||
} else if (dist > 0.) {
|
||||
// Avoid rollback if stepper fully reaches target position
|
||||
stepcompress_commit(sk->sc);
|
||||
} else if (unlikely(dist < -(half_step + .000000001))) {
|
||||
// Found direction change
|
||||
is_dir_change = 1;
|
||||
|
@ -127,6 +135,11 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m
|
|||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/****************************************************************
|
||||
* Interface functions
|
||||
****************************************************************/
|
||||
|
||||
// Check if a move is likely to cause movement on a stepper
|
||||
static inline int
|
||||
check_active(struct stepper_kinematics *sk, struct move *m)
|
||||
|
|
|
@ -38,6 +38,9 @@ struct stepcompress {
|
|||
struct list_head msg_queue;
|
||||
uint32_t queue_step_msgid, set_next_step_dir_msgid, oid;
|
||||
int sdir, invert_sdir;
|
||||
// Step+dir+step filter
|
||||
uint64_t next_step_clock;
|
||||
int next_step_dir;
|
||||
};
|
||||
|
||||
|
||||
|
@ -270,7 +273,7 @@ stepcompress_get_oid(struct stepcompress *sc)
|
|||
int
|
||||
stepcompress_get_step_dir(struct stepcompress *sc)
|
||||
{
|
||||
return sc->sdir;
|
||||
return sc->next_step_dir;
|
||||
}
|
||||
|
||||
// Determine the "print time" of the last_step_clock
|
||||
|
@ -293,7 +296,7 @@ stepcompress_set_time(struct stepcompress *sc
|
|||
|
||||
// Convert previously scheduled steps into commands for the mcu
|
||||
static int
|
||||
stepcompress_flush(struct stepcompress *sc, uint64_t move_clock)
|
||||
queue_flush(struct stepcompress *sc, uint64_t move_clock)
|
||||
{
|
||||
if (sc->queue_pos >= sc->queue_next)
|
||||
return 0;
|
||||
|
@ -346,7 +349,7 @@ set_next_step_dir(struct stepcompress *sc, int sdir)
|
|||
if (sc->sdir == sdir)
|
||||
return 0;
|
||||
sc->sdir = sdir;
|
||||
int ret = stepcompress_flush(sc, UINT64_MAX);
|
||||
int ret = queue_flush(sc, UINT64_MAX);
|
||||
if (ret)
|
||||
return ret;
|
||||
uint32_t msg[3] = {
|
||||
|
@ -361,26 +364,30 @@ set_next_step_dir(struct stepcompress *sc, int sdir)
|
|||
// Maximium clock delta between messages in the queue
|
||||
#define CLOCK_DIFF_MAX (3<<28)
|
||||
|
||||
// Slow path for stepcompress_append()
|
||||
// Slow path for queue_append() - handle next step far in future
|
||||
static int
|
||||
queue_append_slow(struct stepcompress *sc, double rel_sc)
|
||||
queue_append_far(struct stepcompress *sc)
|
||||
{
|
||||
uint64_t abs_step_clock = (uint64_t)rel_sc + sc->last_step_clock;
|
||||
if (abs_step_clock >= sc->last_step_clock + CLOCK_DIFF_MAX) {
|
||||
// Avoid integer overflow on steps far in the future
|
||||
int ret = stepcompress_flush(sc, abs_step_clock - CLOCK_DIFF_MAX + 1);
|
||||
if (ret)
|
||||
return ret;
|
||||
|
||||
if (abs_step_clock >= sc->last_step_clock + CLOCK_DIFF_MAX)
|
||||
return stepcompress_flush_far(sc, abs_step_clock);
|
||||
}
|
||||
uint64_t step_clock = sc->next_step_clock;
|
||||
sc->next_step_clock = 0;
|
||||
int ret = queue_flush(sc, step_clock - CLOCK_DIFF_MAX + 1);
|
||||
if (ret)
|
||||
return ret;
|
||||
if (step_clock >= sc->last_step_clock + CLOCK_DIFF_MAX)
|
||||
return stepcompress_flush_far(sc, step_clock);
|
||||
*sc->queue_next++ = step_clock;
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Slow path for queue_append() - expand the internal queue storage
|
||||
static int
|
||||
queue_append_extend(struct stepcompress *sc)
|
||||
{
|
||||
if (sc->queue_next - sc->queue_pos > 65535 + 2000) {
|
||||
// No point in keeping more than 64K steps in memory
|
||||
uint32_t flush = (*(sc->queue_next-65535)
|
||||
- (uint32_t)sc->last_step_clock);
|
||||
int ret = stepcompress_flush(sc, sc->last_step_clock + flush);
|
||||
int ret = queue_flush(sc, sc->last_step_clock + flush);
|
||||
if (ret)
|
||||
return ret;
|
||||
}
|
||||
|
@ -405,30 +412,82 @@ queue_append_slow(struct stepcompress *sc, double rel_sc)
|
|||
sc->queue_next = sc->queue + in_use;
|
||||
}
|
||||
|
||||
*sc->queue_next++ = abs_step_clock;
|
||||
*sc->queue_next++ = sc->next_step_clock;
|
||||
sc->next_step_clock = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Add a step time to the queue (flushing the queue if needed)
|
||||
inline int
|
||||
stepcompress_append(struct stepcompress *sc, int sdir
|
||||
, double print_time, double step_time)
|
||||
static int
|
||||
queue_append(struct stepcompress *sc)
|
||||
{
|
||||
if (unlikely(sdir != sc->sdir)) {
|
||||
int ret = set_next_step_dir(sc, sdir);
|
||||
if (unlikely(sc->next_step_dir != sc->sdir)) {
|
||||
int ret = set_next_step_dir(sc, sc->next_step_dir);
|
||||
if (ret)
|
||||
return ret;
|
||||
}
|
||||
if (unlikely(sc->next_step_clock >= sc->last_step_clock + CLOCK_DIFF_MAX))
|
||||
return queue_append_far(sc);
|
||||
if (unlikely(sc->queue_next >= sc->queue_end))
|
||||
return queue_append_extend(sc);
|
||||
*sc->queue_next++ = sc->next_step_clock;
|
||||
sc->next_step_clock = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
#define SDS_FILTER_TIME .000750
|
||||
|
||||
// Add next step time
|
||||
int
|
||||
stepcompress_append(struct stepcompress *sc, int sdir
|
||||
, double print_time, double step_time)
|
||||
{
|
||||
// Calculate step clock
|
||||
double offset = print_time - sc->last_step_print_time;
|
||||
double rel_sc = (step_time + offset) * sc->mcu_freq;
|
||||
if (unlikely(sc->queue_next >= sc->queue_end
|
||||
|| rel_sc >= (double)CLOCK_DIFF_MAX))
|
||||
// Slow path to handle queue expansion and integer overflow
|
||||
return queue_append_slow(sc, rel_sc);
|
||||
*sc->queue_next++ = (uint32_t)sc->last_step_clock + (uint32_t)rel_sc;
|
||||
uint64_t step_clock = sc->last_step_clock + (uint64_t)rel_sc;
|
||||
// Flush previous pending step (if any)
|
||||
if (sc->next_step_clock) {
|
||||
if (unlikely(sdir != sc->next_step_dir)) {
|
||||
double diff = step_clock - sc->next_step_clock;
|
||||
if (diff < SDS_FILTER_TIME * sc->mcu_freq) {
|
||||
// Rollback last step to avoid rapid step+dir+step
|
||||
sc->next_step_clock = 0;
|
||||
sc->next_step_dir = sdir;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
int ret = queue_append(sc);
|
||||
if (ret)
|
||||
return ret;
|
||||
}
|
||||
// Store this step as the next pending step
|
||||
sc->next_step_clock = step_clock;
|
||||
sc->next_step_dir = sdir;
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Commit next pending step (ie, do not allow a rollback)
|
||||
int
|
||||
stepcompress_commit(struct stepcompress *sc)
|
||||
{
|
||||
if (sc->next_step_clock)
|
||||
return queue_append(sc);
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Flush pending steps
|
||||
static int
|
||||
stepcompress_flush(struct stepcompress *sc, uint64_t move_clock)
|
||||
{
|
||||
if (sc->next_step_clock && move_clock >= sc->next_step_clock) {
|
||||
int ret = queue_append(sc);
|
||||
if (ret)
|
||||
return ret;
|
||||
}
|
||||
return queue_flush(sc, move_clock);
|
||||
}
|
||||
|
||||
// Reset the internal state of the stepcompress object
|
||||
int __visible
|
||||
stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock)
|
||||
|
|
|
@ -14,6 +14,7 @@ uint32_t stepcompress_get_oid(struct stepcompress *sc);
|
|||
int stepcompress_get_step_dir(struct stepcompress *sc);
|
||||
int stepcompress_append(struct stepcompress *sc, int sdir
|
||||
, double print_time, double step_time);
|
||||
int stepcompress_commit(struct stepcompress *sc);
|
||||
int stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock);
|
||||
int stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len);
|
||||
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
# Code for coordinating events on the printer toolhead
|
||||
#
|
||||
# Copyright (C) 2016-2019 Kevin O'Connor <kevin@koconnor.net>
|
||||
# Copyright (C) 2016-2020 Kevin O'Connor <kevin@koconnor.net>
|
||||
#
|
||||
# This file may be distributed under the terms of the GNU GPLv3 license.
|
||||
import math, logging, importlib
|
||||
|
@ -183,6 +183,7 @@ class MoveQueue:
|
|||
|
||||
MIN_KIN_TIME = 0.100
|
||||
MOVE_BATCH_TIME = 0.500
|
||||
SDS_CHECK_TIME = 0.001 # step+dir+step filter in stepcompress.c
|
||||
|
||||
DRIP_SEGMENT_TIME = 0.050
|
||||
DRIP_TIME = 0.100
|
||||
|
@ -236,7 +237,7 @@ class ToolHead:
|
|||
self.print_stall = 0
|
||||
self.drip_completion = None
|
||||
# Kinematic step generation scan window time tracking
|
||||
self.kin_flush_delay = 0.
|
||||
self.kin_flush_delay = SDS_CHECK_TIME
|
||||
self.kin_flush_times = []
|
||||
self.last_kin_flush_time = self.last_kin_move_time = 0.
|
||||
# Setup iterative solver
|
||||
|
@ -512,7 +513,7 @@ class ToolHead:
|
|||
self.kin_flush_times.pop(self.kin_flush_times.index(old_delay))
|
||||
if delay:
|
||||
self.kin_flush_times.append(delay)
|
||||
new_delay = max(self.kin_flush_times + [0.])
|
||||
new_delay = max(self.kin_flush_times + [SDS_CHECK_TIME])
|
||||
self.kin_flush_delay = new_delay
|
||||
def register_lookahead_callback(self, callback):
|
||||
last_move = self.move_queue.get_last()
|
||||
|
|
Loading…
Reference in New Issue