mirror of https://github.com/Desuuuu/klipper.git
stepcompress: Remove "queue append" abstraction
The queue_append system predates the iterative solver - it was useful when many different kinematic functions directly added steps to the step compression queues. With the iterative solver being the only source of step generation, it is simpler to directly add steps from the iterative solver code. Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
parent
d6c2e24f4c
commit
bc6c3ba92f
|
@ -1,6 +1,6 @@
|
||||||
// Iterative solver for kinematic moves
|
// Iterative solver for kinematic moves
|
||||||
//
|
//
|
||||||
// Copyright (C) 2018-2019 Kevin O'Connor <kevin@koconnor.net>
|
// Copyright (C) 2018-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.
|
||||||
|
|
||||||
|
@ -59,15 +59,12 @@ static int32_t
|
||||||
itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m
|
itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m
|
||||||
, double move_start, double move_end)
|
, double move_start, double move_end)
|
||||||
{
|
{
|
||||||
struct stepcompress *sc = sk->sc;
|
|
||||||
sk_calc_callback calc_position_cb = sk->calc_position_cb;
|
sk_calc_callback calc_position_cb = sk->calc_position_cb;
|
||||||
double half_step = .5 * sk->step_dist;
|
double half_step = .5 * sk->step_dist;
|
||||||
double mcu_freq = stepcompress_get_mcu_freq(sc);
|
|
||||||
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 = 0.000100;
|
double seek_time_delta = 0.000100;
|
||||||
int sdir = stepcompress_get_step_dir(sc);
|
int sdir = stepcompress_get_step_dir(sk->sc);
|
||||||
struct queue_append qa = queue_append_start(sc, m->print_time, .5);
|
|
||||||
for (;;) {
|
for (;;) {
|
||||||
// Determine if next step is in forward or reverse direction
|
// Determine if next step is in forward or reverse direction
|
||||||
double dist = high.position - last.position;
|
double dist = high.position - last.position;
|
||||||
|
@ -101,16 +98,13 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m
|
||||||
high.position = calc_position_cb(sk, m, high.time);
|
high.position = calc_position_cb(sk, m, high.time);
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
int ret = queue_append_set_next_step_dir(&qa, next_sdir);
|
|
||||||
if (ret)
|
|
||||||
return ret;
|
|
||||||
sdir = next_sdir;
|
sdir = next_sdir;
|
||||||
}
|
}
|
||||||
// Find step
|
// Find step
|
||||||
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 = queue_append(&qa, next.time * mcu_freq);
|
int ret = stepcompress_append(sk->sc, sdir, 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;
|
||||||
|
@ -123,7 +117,6 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m
|
||||||
// The high range is no longer valid - recalculate it
|
// The high range is no longer valid - recalculate it
|
||||||
goto seek_new_high_range;
|
goto seek_new_high_range;
|
||||||
}
|
}
|
||||||
queue_append_finish(qa);
|
|
||||||
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);
|
||||||
|
|
|
@ -1,9 +1,9 @@
|
||||||
// Stepper pulse schedule compression
|
// Stepper pulse schedule compression
|
||||||
//
|
//
|
||||||
// Copyright (C) 2016-2018 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.
|
||||||
//
|
|
||||||
// The goal of this code is to take a series of scheduled stepper
|
// The goal of this code is to take a series of scheduled stepper
|
||||||
// pulse times and compress them into a handful of commands that can
|
// pulse times and compress them into a handful of commands that can
|
||||||
// be efficiently transmitted and executed on a microcontroller (mcu).
|
// be efficiently transmitted and executed on a microcontroller (mcu).
|
||||||
|
@ -32,7 +32,7 @@ struct stepcompress {
|
||||||
uint32_t *queue, *queue_end, *queue_pos, *queue_next;
|
uint32_t *queue, *queue_end, *queue_pos, *queue_next;
|
||||||
// Internal tracking
|
// Internal tracking
|
||||||
uint32_t max_error;
|
uint32_t max_error;
|
||||||
double mcu_time_offset, mcu_freq;
|
double mcu_time_offset, mcu_freq, last_step_print_time;
|
||||||
// Message generation
|
// Message generation
|
||||||
uint64_t last_step_clock;
|
uint64_t last_step_clock;
|
||||||
struct list_head msg_queue;
|
struct list_head msg_queue;
|
||||||
|
@ -261,6 +261,14 @@ stepcompress_free(struct stepcompress *sc)
|
||||||
free(sc);
|
free(sc);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Determine the "print time" of the last_step_clock
|
||||||
|
static void
|
||||||
|
calc_last_step_print_time(struct stepcompress *sc)
|
||||||
|
{
|
||||||
|
double lsc = sc->last_step_clock;
|
||||||
|
sc->last_step_print_time = sc->mcu_time_offset + (lsc - .5) / sc->mcu_freq;
|
||||||
|
}
|
||||||
|
|
||||||
// Convert previously scheduled steps into commands for the mcu
|
// Convert previously scheduled steps into commands for the mcu
|
||||||
static int
|
static int
|
||||||
stepcompress_flush(struct stepcompress *sc, uint64_t move_clock)
|
stepcompress_flush(struct stepcompress *sc, uint64_t move_clock)
|
||||||
|
@ -289,6 +297,7 @@ stepcompress_flush(struct stepcompress *sc, uint64_t move_clock)
|
||||||
}
|
}
|
||||||
sc->queue_pos += move.count;
|
sc->queue_pos += move.count;
|
||||||
}
|
}
|
||||||
|
calc_last_step_print_time(sc);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -304,6 +313,7 @@ stepcompress_flush_far(struct stepcompress *sc, uint64_t abs_step_clock)
|
||||||
qm->min_clock = sc->last_step_clock;
|
qm->min_clock = sc->last_step_clock;
|
||||||
sc->last_step_clock = qm->req_clock = abs_step_clock;
|
sc->last_step_clock = qm->req_clock = abs_step_clock;
|
||||||
list_add_tail(&qm->node, &sc->msg_queue);
|
list_add_tail(&qm->node, &sc->msg_queue);
|
||||||
|
calc_last_step_print_time(sc);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -335,6 +345,7 @@ stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock)
|
||||||
return ret;
|
return ret;
|
||||||
sc->last_step_clock = last_step_clock;
|
sc->last_step_clock = last_step_clock;
|
||||||
sc->sdir = -1;
|
sc->sdir = -1;
|
||||||
|
calc_last_step_print_time(sc);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -359,12 +370,7 @@ stepcompress_set_time(struct stepcompress *sc
|
||||||
{
|
{
|
||||||
sc->mcu_time_offset = time_offset;
|
sc->mcu_time_offset = time_offset;
|
||||||
sc->mcu_freq = mcu_freq;
|
sc->mcu_freq = mcu_freq;
|
||||||
}
|
calc_last_step_print_time(sc);
|
||||||
|
|
||||||
double
|
|
||||||
stepcompress_get_mcu_freq(struct stepcompress *sc)
|
|
||||||
{
|
|
||||||
return sc->mcu_freq;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
uint32_t
|
uint32_t
|
||||||
|
@ -379,33 +385,10 @@ stepcompress_get_step_dir(struct stepcompress *sc)
|
||||||
return sc->sdir;
|
return sc->sdir;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/****************************************************************
|
|
||||||
* Queue management
|
|
||||||
****************************************************************/
|
|
||||||
|
|
||||||
// Maximium clock delta between messages in the queue
|
// Maximium clock delta between messages in the queue
|
||||||
#define CLOCK_DIFF_MAX (3<<28)
|
#define CLOCK_DIFF_MAX (3<<28)
|
||||||
|
|
||||||
// Create a cursor for inserting clock times into the queue
|
// Slow path for stepcompress_append()
|
||||||
inline struct queue_append
|
|
||||||
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 = (print_clock - (double)sc->last_step_clock) + adjust };
|
|
||||||
}
|
|
||||||
|
|
||||||
// Finalize a cursor created with queue_append_start()
|
|
||||||
inline void
|
|
||||||
queue_append_finish(struct queue_append qa)
|
|
||||||
{
|
|
||||||
qa.sc->queue_next = qa.qnext;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Slow path for queue_append()
|
|
||||||
static int
|
static int
|
||||||
queue_append_slow(struct stepcompress *sc, double rel_sc)
|
queue_append_slow(struct stepcompress *sc, double rel_sc)
|
||||||
{
|
{
|
||||||
|
@ -453,42 +436,23 @@ queue_append_slow(struct stepcompress *sc, double rel_sc)
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Add a clock time to the queue (flushing the queue if needed)
|
// Add a step time to the queue (flushing the queue if needed)
|
||||||
inline int
|
inline int
|
||||||
queue_append(struct queue_append *qa, double step_clock)
|
stepcompress_append(struct stepcompress *sc, int sdir
|
||||||
|
, double print_time, double step_time)
|
||||||
{
|
{
|
||||||
double rel_sc = step_clock + qa->clock_offset;
|
if (unlikely(sdir != sc->sdir)) {
|
||||||
if (likely(!(qa->qnext >= qa->qend || rel_sc >= (double)CLOCK_DIFF_MAX))) {
|
int ret = set_next_step_dir(sc, sdir);
|
||||||
*qa->qnext++ = qa->last_step_clock_32 + (uint32_t)rel_sc;
|
if (ret)
|
||||||
return 0;
|
return ret;
|
||||||
}
|
}
|
||||||
// Call queue_append_slow() to handle queue expansion and integer overflow
|
double offset = print_time - sc->last_step_print_time;
|
||||||
struct stepcompress *sc = qa->sc;
|
double rel_sc = (step_time + offset) * sc->mcu_freq;
|
||||||
uint64_t old_last_step_clock = sc->last_step_clock;
|
if (unlikely(sc->queue_next >= sc->queue_end
|
||||||
sc->queue_next = qa->qnext;
|
|| rel_sc >= (double)CLOCK_DIFF_MAX))
|
||||||
int ret = queue_append_slow(sc, rel_sc);
|
// Slow path to handle queue expansion and integer overflow
|
||||||
if (ret)
|
return queue_append_slow(sc, rel_sc);
|
||||||
return ret;
|
*sc->queue_next++ = (uint32_t)sc->last_step_clock + (uint32_t)rel_sc;
|
||||||
qa->qnext = sc->queue_next;
|
|
||||||
qa->qend = sc->queue_end;
|
|
||||||
qa->last_step_clock_32 = sc->last_step_clock;
|
|
||||||
qa->clock_offset -= sc->last_step_clock - old_last_step_clock;
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
inline int
|
|
||||||
queue_append_set_next_step_dir(struct queue_append *qa, int sdir)
|
|
||||||
{
|
|
||||||
struct stepcompress *sc = qa->sc;
|
|
||||||
uint64_t old_last_step_clock = sc->last_step_clock;
|
|
||||||
sc->queue_next = qa->qnext;
|
|
||||||
int ret = set_next_step_dir(sc, sdir);
|
|
||||||
if (ret)
|
|
||||||
return ret;
|
|
||||||
qa->qnext = sc->queue_next;
|
|
||||||
qa->qend = sc->queue_end;
|
|
||||||
qa->last_step_clock_32 = sc->last_step_clock;
|
|
||||||
qa->clock_offset -= sc->last_step_clock - old_last_step_clock;
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -12,20 +12,10 @@ void stepcompress_fill(struct stepcompress *sc, uint32_t max_error
|
||||||
void stepcompress_free(struct stepcompress *sc);
|
void stepcompress_free(struct stepcompress *sc);
|
||||||
int stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock);
|
int stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock);
|
||||||
int stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len);
|
int stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len);
|
||||||
double stepcompress_get_mcu_freq(struct stepcompress *sc);
|
|
||||||
uint32_t stepcompress_get_oid(struct stepcompress *sc);
|
uint32_t stepcompress_get_oid(struct stepcompress *sc);
|
||||||
int stepcompress_get_step_dir(struct stepcompress *sc);
|
int stepcompress_get_step_dir(struct stepcompress *sc);
|
||||||
|
int stepcompress_append(struct stepcompress *sc, int sdir
|
||||||
struct queue_append {
|
, double print_time, double step_time);
|
||||||
struct stepcompress *sc;
|
|
||||||
uint32_t *qnext, *qend, last_step_clock_32;
|
|
||||||
double clock_offset;
|
|
||||||
};
|
|
||||||
struct queue_append queue_append_start(
|
|
||||||
struct stepcompress *sc, double print_time, double adjust);
|
|
||||||
void queue_append_finish(struct queue_append qa);
|
|
||||||
int queue_append(struct queue_append *qa, double step_clock);
|
|
||||||
int queue_append_set_next_step_dir(struct queue_append *qa, int sdir);
|
|
||||||
|
|
||||||
struct serialqueue;
|
struct serialqueue;
|
||||||
struct steppersync *steppersync_alloc(
|
struct steppersync *steppersync_alloc(
|
||||||
|
|
Loading…
Reference in New Issue