mirror of https://github.com/Desuuuu/klipper.git
itersolve: 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
8f8c1e2c58
commit
c9cb462f90
|
@ -13,6 +13,65 @@
|
||||||
#include "stepcompress.h" // queue_append_start
|
#include "stepcompress.h" // queue_append_start
|
||||||
#include "trapq.h" // struct move
|
#include "trapq.h" // struct move
|
||||||
|
|
||||||
|
|
||||||
|
/****************************************************************
|
||||||
|
* Filter rapid "step + direction change + step" sequences
|
||||||
|
****************************************************************/
|
||||||
|
|
||||||
|
#define SDS_CHECK_TIME .001
|
||||||
|
#define SDS_FILTER_TIME .000750
|
||||||
|
|
||||||
|
static int
|
||||||
|
sds_commit(struct stepper_kinematics *sk)
|
||||||
|
{
|
||||||
|
double mtime = sk->next_move_print_time, stime = sk->next_step_time;
|
||||||
|
sk->next_move_print_time = 0.;
|
||||||
|
return stepcompress_append(sk->sc, sk->next_step_dir, mtime, stime);
|
||||||
|
}
|
||||||
|
|
||||||
|
static int
|
||||||
|
sds_append(struct stepper_kinematics *sk, int sdir
|
||||||
|
, double move_print_time, double step_time)
|
||||||
|
{
|
||||||
|
if (sk->next_move_print_time) {
|
||||||
|
if (sdir != sk->next_step_dir) {
|
||||||
|
double mt_diff = move_print_time - sk->next_move_print_time;
|
||||||
|
double st_diff = step_time - sk->next_step_time;
|
||||||
|
if (mt_diff + st_diff < SDS_FILTER_TIME) {
|
||||||
|
// Rollback last step
|
||||||
|
sk->next_move_print_time = 0.;
|
||||||
|
sk->next_step_dir = sdir;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
int ret = sds_commit(sk);
|
||||||
|
if (ret)
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
sk->next_move_print_time = move_print_time;
|
||||||
|
sk->next_step_time = step_time;
|
||||||
|
sk->next_step_dir = sdir;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int
|
||||||
|
sds_flush(struct stepper_kinematics *sk
|
||||||
|
, double move_print_time, double step_time)
|
||||||
|
{
|
||||||
|
if (sk->next_move_print_time) {
|
||||||
|
double mt_diff = move_print_time - sk->next_move_print_time;
|
||||||
|
double st_diff = step_time - sk->next_step_time;
|
||||||
|
if (mt_diff + st_diff >= SDS_FILTER_TIME)
|
||||||
|
return sds_commit(sk);
|
||||||
|
}
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/****************************************************************
|
||||||
|
* Main iterative solver
|
||||||
|
****************************************************************/
|
||||||
|
|
||||||
struct timepos {
|
struct timepos {
|
||||||
double time, position;
|
double time, position;
|
||||||
};
|
};
|
||||||
|
@ -66,7 +125,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;
|
double start = move_start - m->print_time, end = move_end - m->print_time;
|
||||||
struct timepos last = { start, sk->commanded_pos }, low = last, high = last;
|
struct timepos last = { start, sk->commanded_pos }, low = last, high = last;
|
||||||
double seek_time_delta = SEEK_TIME_RESET;
|
double seek_time_delta = SEEK_TIME_RESET;
|
||||||
int sdir = !!stepcompress_get_step_dir(sk->sc), is_dir_change = 0;
|
int sdir = sk->next_step_dir, is_dir_change = 0;
|
||||||
for (;;) {
|
for (;;) {
|
||||||
double diff = high.position - last.position, dist = sdir ? diff : -diff;
|
double diff = high.position - last.position, dist = sdir ? diff : -diff;
|
||||||
if (dist >= half_step) {
|
if (dist >= half_step) {
|
||||||
|
@ -74,8 +133,7 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m
|
||||||
double target = last.position + (sdir ? half_step : -half_step);
|
double target = last.position + (sdir ? half_step : -half_step);
|
||||||
struct timepos next = itersolve_find_step(sk, m, low, high, target);
|
struct timepos next = itersolve_find_step(sk, m, low, high, target);
|
||||||
// Add step at given time
|
// Add step at given time
|
||||||
int ret = stepcompress_append(sk->sc, sdir
|
int ret = sds_append(sk, sdir, m->print_time, next.time);
|
||||||
, m->print_time, next.time);
|
|
||||||
if (ret)
|
if (ret)
|
||||||
return ret;
|
return ret;
|
||||||
seek_time_delta = next.time - last.time;
|
seek_time_delta = next.time - last.time;
|
||||||
|
@ -90,6 +148,10 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m
|
||||||
if (low.time < high.time)
|
if (low.time < high.time)
|
||||||
// The existing search range is still valid
|
// The existing search range is still valid
|
||||||
continue;
|
continue;
|
||||||
|
} else if (dist > 0.) {
|
||||||
|
// Avoid rollback if stepper fully reaches target position
|
||||||
|
if (sk->next_move_print_time)
|
||||||
|
sds_commit(sk);
|
||||||
} else if (unlikely(dist < -(half_step + .000000001))) {
|
} else if (unlikely(dist < -(half_step + .000000001))) {
|
||||||
// Found direction change
|
// Found direction change
|
||||||
is_dir_change = 1;
|
is_dir_change = 1;
|
||||||
|
@ -121,12 +183,20 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m
|
||||||
high.time = end;
|
high.time = end;
|
||||||
high.position = calc_position_cb(sk, m, high.time);
|
high.position = calc_position_cb(sk, m, high.time);
|
||||||
}
|
}
|
||||||
|
int ret = sds_flush(sk, m->print_time, end);
|
||||||
|
if (ret)
|
||||||
|
return ret;
|
||||||
sk->commanded_pos = last.position;
|
sk->commanded_pos = last.position;
|
||||||
if (sk->post_cb)
|
if (sk->post_cb)
|
||||||
sk->post_cb(sk);
|
sk->post_cb(sk);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/****************************************************************
|
||||||
|
* Interface functions
|
||||||
|
****************************************************************/
|
||||||
|
|
||||||
// Check if a move is likely to cause movement on a stepper
|
// Check if a move is likely to cause movement on a stepper
|
||||||
static inline int
|
static inline int
|
||||||
check_active(struct stepper_kinematics *sk, struct move *m)
|
check_active(struct stepper_kinematics *sk, struct move *m)
|
||||||
|
@ -149,7 +219,10 @@ itersolve_generate_steps(struct stepper_kinematics *sk, double flush_time)
|
||||||
struct move *m = list_first_entry(&sk->tq->moves, struct move, node);
|
struct move *m = list_first_entry(&sk->tq->moves, struct move, node);
|
||||||
while (last_flush_time >= m->print_time + m->move_t)
|
while (last_flush_time >= m->print_time + m->move_t)
|
||||||
m = list_next_entry(m, node);
|
m = list_next_entry(m, node);
|
||||||
double force_steps_time = sk->last_move_time + sk->gen_steps_post_active;
|
double gen_steps_post_active = sk->gen_steps_post_active;
|
||||||
|
if (gen_steps_post_active < SDS_CHECK_TIME)
|
||||||
|
gen_steps_post_active = SDS_CHECK_TIME;
|
||||||
|
double force_steps_time = sk->last_move_time + gen_steps_post_active;
|
||||||
for (;;) {
|
for (;;) {
|
||||||
if (last_flush_time >= flush_time)
|
if (last_flush_time >= flush_time)
|
||||||
return 0;
|
return 0;
|
||||||
|
@ -174,7 +247,7 @@ itersolve_generate_steps(struct stepper_kinematics *sk, double flush_time)
|
||||||
if (ret)
|
if (ret)
|
||||||
return ret;
|
return ret;
|
||||||
sk->last_move_time = last_flush_time = end;
|
sk->last_move_time = last_flush_time = end;
|
||||||
force_steps_time = end + sk->gen_steps_post_active;
|
force_steps_time = end + gen_steps_post_active;
|
||||||
} else if (start < force_steps_time) {
|
} else if (start < force_steps_time) {
|
||||||
// Must generates steps just past stepper activity
|
// Must generates steps just past stepper activity
|
||||||
if (end > force_steps_time)
|
if (end > force_steps_time)
|
||||||
|
|
|
@ -16,6 +16,9 @@ struct stepper_kinematics {
|
||||||
double step_dist, commanded_pos;
|
double step_dist, commanded_pos;
|
||||||
struct stepcompress *sc;
|
struct stepcompress *sc;
|
||||||
|
|
||||||
|
double next_move_print_time, next_step_time;
|
||||||
|
int next_step_dir;
|
||||||
|
|
||||||
double last_flush_time, last_move_time;
|
double last_flush_time, last_move_time;
|
||||||
struct trapq *tq;
|
struct trapq *tq;
|
||||||
int active_flags;
|
int active_flags;
|
||||||
|
|
|
@ -1,6 +1,6 @@
|
||||||
# Code for coordinating events on the printer toolhead
|
# 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.
|
# This file may be distributed under the terms of the GNU GPLv3 license.
|
||||||
import math, logging, importlib
|
import math, logging, importlib
|
||||||
|
@ -183,6 +183,7 @@ class MoveQueue:
|
||||||
|
|
||||||
MIN_KIN_TIME = 0.100
|
MIN_KIN_TIME = 0.100
|
||||||
MOVE_BATCH_TIME = 0.500
|
MOVE_BATCH_TIME = 0.500
|
||||||
|
SDS_CHECK_TIME = 0.001 # step+dir+step filter in itersolve.c
|
||||||
|
|
||||||
DRIP_SEGMENT_TIME = 0.050
|
DRIP_SEGMENT_TIME = 0.050
|
||||||
DRIP_TIME = 0.100
|
DRIP_TIME = 0.100
|
||||||
|
@ -236,7 +237,7 @@ class ToolHead:
|
||||||
self.print_stall = 0
|
self.print_stall = 0
|
||||||
self.drip_completion = None
|
self.drip_completion = None
|
||||||
# Kinematic step generation scan window time tracking
|
# Kinematic step generation scan window time tracking
|
||||||
self.kin_flush_delay = 0.
|
self.kin_flush_delay = SDS_CHECK_TIME
|
||||||
self.kin_flush_times = []
|
self.kin_flush_times = []
|
||||||
self.last_kin_flush_time = self.last_kin_move_time = 0.
|
self.last_kin_flush_time = self.last_kin_move_time = 0.
|
||||||
# Setup iterative solver
|
# Setup iterative solver
|
||||||
|
@ -512,7 +513,7 @@ class ToolHead:
|
||||||
self.kin_flush_times.pop(self.kin_flush_times.index(old_delay))
|
self.kin_flush_times.pop(self.kin_flush_times.index(old_delay))
|
||||||
if delay:
|
if delay:
|
||||||
self.kin_flush_times.append(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
|
self.kin_flush_delay = new_delay
|
||||||
def register_lookahead_callback(self, callback):
|
def register_lookahead_callback(self, callback):
|
||||||
last_move = self.move_queue.get_last()
|
last_move = self.move_queue.get_last()
|
||||||
|
|
Loading…
Reference in New Issue