mirror of https://github.com/Desuuuu/klipper.git
stepcompress: Refactor queue insertion to use a cursor
Create an insertion "cursor" for adding items to the step compression queue. This makes the calling code simpler and it makes it easier to update the queue memory management in the future. Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
parent
78f4c25a14
commit
f5cc355044
|
@ -39,42 +39,6 @@ struct stepcompress {
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
/****************************************************************
|
|
||||||
* Queue management
|
|
||||||
****************************************************************/
|
|
||||||
|
|
||||||
// Shuffle the internal queue to avoid having to allocate more ram
|
|
||||||
static void
|
|
||||||
clean_queue(struct stepcompress *sc)
|
|
||||||
{
|
|
||||||
int in_use = sc->queue_next - sc->queue_pos;
|
|
||||||
memmove(sc->queue, sc->queue_pos, in_use * sizeof(*sc->queue));
|
|
||||||
sc->queue_pos = sc->queue;
|
|
||||||
sc->queue_next = sc->queue + in_use;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Expand the internal queue of step times
|
|
||||||
static void
|
|
||||||
expand_queue(struct stepcompress *sc, int count)
|
|
||||||
{
|
|
||||||
int alloc = sc->queue_end - sc->queue;
|
|
||||||
if (count + sc->queue_next - sc->queue_pos <= alloc) {
|
|
||||||
clean_queue(sc);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
int pos = sc->queue_pos - sc->queue;
|
|
||||||
int next = sc->queue_next - sc->queue;
|
|
||||||
if (!alloc)
|
|
||||||
alloc = QUEUE_START_SIZE;
|
|
||||||
while (next + count > alloc)
|
|
||||||
alloc *= 2;
|
|
||||||
sc->queue = realloc(sc->queue, alloc * sizeof(*sc->queue));
|
|
||||||
sc->queue_end = sc->queue + alloc;
|
|
||||||
sc->queue_pos = sc->queue + pos;
|
|
||||||
sc->queue_next = sc->queue + next;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
/****************************************************************
|
/****************************************************************
|
||||||
* Step compression
|
* Step compression
|
||||||
****************************************************************/
|
****************************************************************/
|
||||||
|
@ -362,35 +326,6 @@ set_next_step_dir(struct stepcompress *sc, int sdir)
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Check if the internal queue needs to be expanded, and expand if so
|
|
||||||
static int
|
|
||||||
_check_push(struct stepcompress *sc)
|
|
||||||
{
|
|
||||||
if (sc->queue_next - sc->queue_pos > 65535 + 2000) {
|
|
||||||
// No point in keeping more than 64K steps in memory
|
|
||||||
int ret = stepcompress_flush(sc, *(sc->queue_next - 65535));
|
|
||||||
if (ret)
|
|
||||||
return ret;
|
|
||||||
}
|
|
||||||
expand_queue(sc, 1);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
static inline int
|
|
||||||
check_push(struct stepcompress *sc, uint64_t **pqnext, uint64_t **pqend
|
|
||||||
, uint64_t c)
|
|
||||||
{
|
|
||||||
if (unlikely(*pqnext >= *pqend)) {
|
|
||||||
sc->queue_next = *pqnext;
|
|
||||||
int ret = _check_push(sc);
|
|
||||||
if (ret)
|
|
||||||
return ret;
|
|
||||||
*pqnext = sc->queue_next;
|
|
||||||
*pqend = sc->queue_end;
|
|
||||||
}
|
|
||||||
*(*pqnext)++ = c;
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Reset the internal state of the stepcompress object
|
// Reset the internal state of the stepcompress object
|
||||||
int
|
int
|
||||||
stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock)
|
stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock)
|
||||||
|
@ -429,10 +364,96 @@ stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len)
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/****************************************************************
|
||||||
|
* Queue management
|
||||||
|
****************************************************************/
|
||||||
|
|
||||||
|
struct queue_append {
|
||||||
|
struct stepcompress *sc;
|
||||||
|
uint64_t *qnext, *qend;
|
||||||
|
double clock_offset;
|
||||||
|
};
|
||||||
|
|
||||||
|
// 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)
|
||||||
|
{
|
||||||
|
return (struct queue_append) {
|
||||||
|
.sc = sc, .qnext = sc->queue_next, .qend = sc->queue_end,
|
||||||
|
.clock_offset = clock_offset + adjust };
|
||||||
|
}
|
||||||
|
|
||||||
|
// Finalize a cursor created with queue_append_start()
|
||||||
|
static inline void
|
||||||
|
queue_append_finish(struct queue_append qa)
|
||||||
|
{
|
||||||
|
qa.sc->queue_next = qa.qnext;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Slow path for queue_append()
|
||||||
|
static int
|
||||||
|
queue_append_slow(struct stepcompress *sc, double abs_step_clock)
|
||||||
|
{
|
||||||
|
if (sc->queue_next - sc->queue_pos > 65535 + 2000) {
|
||||||
|
// No point in keeping more than 64K steps in memory
|
||||||
|
int ret = stepcompress_flush(sc, *(sc->queue_next - 65535));
|
||||||
|
if (ret)
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
int in_use = sc->queue_next - sc->queue_pos;
|
||||||
|
if (sc->queue_pos > sc->queue) {
|
||||||
|
// Shuffle the internal queue to avoid having to allocate more ram
|
||||||
|
memmove(sc->queue, sc->queue_pos, in_use * sizeof(*sc->queue));
|
||||||
|
} else {
|
||||||
|
// Expand the internal queue of step times
|
||||||
|
int alloc = sc->queue_end - sc->queue;
|
||||||
|
if (!alloc)
|
||||||
|
alloc = QUEUE_START_SIZE;
|
||||||
|
while (in_use >= alloc)
|
||||||
|
alloc *= 2;
|
||||||
|
sc->queue = realloc(sc->queue, alloc * sizeof(*sc->queue));
|
||||||
|
sc->queue_end = sc->queue + alloc;
|
||||||
|
}
|
||||||
|
sc->queue_pos = sc->queue;
|
||||||
|
sc->queue_next = sc->queue + in_use;
|
||||||
|
*sc->queue_next++ = abs_step_clock;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Add a clock time to the queue (flushing the queue if needed)
|
||||||
|
static inline int
|
||||||
|
queue_append(struct queue_append *qa, double step_clock)
|
||||||
|
{
|
||||||
|
double abs_step_clock = step_clock + qa->clock_offset;
|
||||||
|
if (likely(qa->qnext < qa->qend)) {
|
||||||
|
*qa->qnext++ = abs_step_clock;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
// Call queue_append_slow() to handle queue expansion
|
||||||
|
struct stepcompress *sc = qa->sc;
|
||||||
|
sc->queue_next = qa->qnext;
|
||||||
|
int ret = queue_append_slow(sc, abs_step_clock);
|
||||||
|
if (ret)
|
||||||
|
return ret;
|
||||||
|
qa->qnext = sc->queue_next;
|
||||||
|
qa->qend = sc->queue_end;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
/****************************************************************
|
/****************************************************************
|
||||||
* Motion to step conversions
|
* Motion to step conversions
|
||||||
****************************************************************/
|
****************************************************************/
|
||||||
|
|
||||||
|
// 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.
|
||||||
|
|
||||||
// Wrapper around sqrt() to handle small negative numbers
|
// Wrapper around sqrt() to handle small negative numbers
|
||||||
static double
|
static double
|
||||||
_safe_sqrt(double v)
|
_safe_sqrt(double v)
|
||||||
|
@ -454,23 +475,14 @@ stepcompress_push(struct stepcompress *sc, double step_clock, int32_t sdir)
|
||||||
int ret = set_next_step_dir(sc, !!sdir);
|
int ret = set_next_step_dir(sc, !!sdir);
|
||||||
if (ret)
|
if (ret)
|
||||||
return ret;
|
return ret;
|
||||||
step_clock += 0.5;
|
struct queue_append qa = queue_append_start(sc, step_clock, 0.5);
|
||||||
uint64_t *qnext = sc->queue_next, *qend = sc->queue_end;
|
ret = queue_append(&qa, 0.);
|
||||||
ret = check_push(sc, &qnext, &qend, step_clock);
|
|
||||||
if (ret)
|
if (ret)
|
||||||
return ret;
|
return ret;
|
||||||
sc->queue_next = qnext;
|
queue_append_finish(qa);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 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.
|
|
||||||
|
|
||||||
// Schedule 'steps' number of steps at constant acceleration. If
|
// Schedule 'steps' number of steps at constant acceleration. If
|
||||||
// acceleration is zero (ie, constant velocity) it uses the formula:
|
// acceleration is zero (ie, constant velocity) it uses the formula:
|
||||||
// step_clock = clock_offset + step_num/start_sv
|
// step_clock = clock_offset + step_num/start_sv
|
||||||
|
@ -505,35 +517,34 @@ stepcompress_push_const(
|
||||||
int res = sdir ? count : -count;
|
int res = sdir ? count : -count;
|
||||||
|
|
||||||
// Calculate each step time
|
// Calculate each step time
|
||||||
clock_offset += 0.5;
|
|
||||||
double pos = step_offset + .5;
|
double pos = step_offset + .5;
|
||||||
uint64_t *qnext = sc->queue_next, *qend = sc->queue_end;
|
|
||||||
if (!accel) {
|
if (!accel) {
|
||||||
// Move at constant velocity (zero acceleration)
|
// Move at constant velocity (zero acceleration)
|
||||||
|
struct queue_append qa = queue_append_start(sc, clock_offset, .5);
|
||||||
double inv_cruise_sv = 1. / start_sv;
|
double inv_cruise_sv = 1. / start_sv;
|
||||||
while (count--) {
|
while (count--) {
|
||||||
uint64_t c = clock_offset + pos*inv_cruise_sv;
|
ret = queue_append(&qa, pos * inv_cruise_sv);
|
||||||
int ret = check_push(sc, &qnext, &qend, c);
|
|
||||||
if (ret)
|
if (ret)
|
||||||
return ret;
|
return ret;
|
||||||
pos += 1.0;
|
pos += 1.0;
|
||||||
}
|
}
|
||||||
|
queue_append_finish(qa);
|
||||||
} else {
|
} else {
|
||||||
// Move with constant acceleration
|
// Move with constant acceleration
|
||||||
double inv_accel = 1. / accel;
|
double inv_accel = 1. / accel;
|
||||||
clock_offset -= start_sv * inv_accel;
|
|
||||||
pos += .5 * start_sv*start_sv * inv_accel;
|
pos += .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;
|
double accel_multiplier = 2. * inv_accel;
|
||||||
while (count--) {
|
while (count--) {
|
||||||
double v = safe_sqrt(pos * accel_multiplier);
|
double v = safe_sqrt(pos * accel_multiplier);
|
||||||
uint64_t c = clock_offset + (accel_multiplier >= 0. ? v : -v);
|
int ret = queue_append(&qa, accel_multiplier >= 0. ? v : -v);
|
||||||
int ret = check_push(sc, &qnext, &qend, c);
|
|
||||||
if (ret)
|
if (ret)
|
||||||
return ret;
|
return ret;
|
||||||
pos += 1.0;
|
pos += 1.0;
|
||||||
}
|
}
|
||||||
|
queue_append_finish(qa);
|
||||||
}
|
}
|
||||||
sc->queue_next = qnext;
|
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -565,19 +576,17 @@ _stepcompress_push_delta(
|
||||||
int res = sdir ? count : -count;
|
int res = sdir ? count : -count;
|
||||||
|
|
||||||
// Calculate each step time
|
// Calculate each step time
|
||||||
clock_offset += 0.5;
|
|
||||||
height += (sdir ? .5 : -.5);
|
height += (sdir ? .5 : -.5);
|
||||||
uint64_t *qnext = sc->queue_next, *qend = sc->queue_end;
|
|
||||||
if (!accel) {
|
if (!accel) {
|
||||||
// Move at constant velocity (zero acceleration)
|
// Move at constant velocity (zero acceleration)
|
||||||
|
struct queue_append qa = queue_append_start(sc, clock_offset, .5);
|
||||||
double inv_cruise_sv = 1. / start_sv;
|
double inv_cruise_sv = 1. / start_sv;
|
||||||
if (!movez_r) {
|
if (!movez_r) {
|
||||||
// Optimized case for common XY only moves (no Z movement)
|
// Optimized case for common XY only moves (no Z movement)
|
||||||
while (count--) {
|
while (count--) {
|
||||||
double v = safe_sqrt(arm_sd2 - height*height);
|
double v = safe_sqrt(arm_sd2 - height*height);
|
||||||
double pos = startxy_sd + (sdir ? -v : v);
|
double pos = startxy_sd + (sdir ? -v : v);
|
||||||
uint64_t c = clock_offset + pos * inv_cruise_sv;
|
int ret = queue_append(&qa, pos * inv_cruise_sv);
|
||||||
int ret = check_push(sc, &qnext, &qend, c);
|
|
||||||
if (ret)
|
if (ret)
|
||||||
return ret;
|
return ret;
|
||||||
height += (sdir ? 1. : -1.);
|
height += (sdir ? 1. : -1.);
|
||||||
|
@ -586,8 +595,7 @@ _stepcompress_push_delta(
|
||||||
// Optimized case for Z only moves
|
// Optimized case for Z only moves
|
||||||
double pos = (sdir ? height-end_height : end_height-height);
|
double pos = (sdir ? height-end_height : end_height-height);
|
||||||
while (count--) {
|
while (count--) {
|
||||||
uint64_t c = clock_offset + pos * inv_cruise_sv;
|
int ret = queue_append(&qa, pos * inv_cruise_sv);
|
||||||
int ret = check_push(sc, &qnext, &qend, c);
|
|
||||||
if (ret)
|
if (ret)
|
||||||
return ret;
|
return ret;
|
||||||
pos += 1.;
|
pos += 1.;
|
||||||
|
@ -599,33 +607,33 @@ _stepcompress_push_delta(
|
||||||
double relheight = movexy_r*height - zoffset;
|
double relheight = movexy_r*height - zoffset;
|
||||||
double v = safe_sqrt(arm_sd2 - relheight*relheight);
|
double v = safe_sqrt(arm_sd2 - relheight*relheight);
|
||||||
double pos = start_pos + movez_r*height + (sdir ? -v : v);
|
double pos = start_pos + movez_r*height + (sdir ? -v : v);
|
||||||
uint64_t c = clock_offset + pos * inv_cruise_sv;
|
int ret = queue_append(&qa, pos * inv_cruise_sv);
|
||||||
int ret = check_push(sc, &qnext, &qend, c);
|
|
||||||
if (ret)
|
if (ret)
|
||||||
return ret;
|
return ret;
|
||||||
height += (sdir ? 1. : -1.);
|
height += (sdir ? 1. : -1.);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
queue_append_finish(qa);
|
||||||
} else {
|
} else {
|
||||||
// Move with constant acceleration
|
// Move with constant acceleration
|
||||||
double start_pos = movexy_r*startxy_sd, zoffset = movez_r*startxy_sd;
|
double start_pos = movexy_r*startxy_sd, zoffset = movez_r*startxy_sd;
|
||||||
double inv_accel = 1. / accel;
|
double inv_accel = 1. / accel;
|
||||||
clock_offset -= start_sv * inv_accel;
|
|
||||||
start_pos += 0.5 * start_sv*start_sv * inv_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;
|
double accel_multiplier = 2. * inv_accel;
|
||||||
while (count--) {
|
while (count--) {
|
||||||
double relheight = movexy_r*height - zoffset;
|
double relheight = movexy_r*height - zoffset;
|
||||||
double v = safe_sqrt(arm_sd2 - relheight*relheight);
|
double v = safe_sqrt(arm_sd2 - relheight*relheight);
|
||||||
double pos = start_pos + movez_r*height + (sdir ? -v : v);
|
double pos = start_pos + movez_r*height + (sdir ? -v : v);
|
||||||
v = safe_sqrt(pos * accel_multiplier);
|
v = safe_sqrt(pos * accel_multiplier);
|
||||||
uint64_t c = clock_offset + (accel_multiplier >= 0. ? v : -v);
|
int ret = queue_append(&qa, accel_multiplier >= 0. ? v : -v);
|
||||||
int ret = check_push(sc, &qnext, &qend, c);
|
|
||||||
if (ret)
|
if (ret)
|
||||||
return ret;
|
return ret;
|
||||||
height += (sdir ? 1. : -1.);
|
height += (sdir ? 1. : -1.);
|
||||||
}
|
}
|
||||||
|
queue_append_finish(qa);
|
||||||
}
|
}
|
||||||
sc->queue_next = qnext;
|
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue