mirror of https://github.com/Desuuuu/klipper.git
stepcompress: Add support for tracking history of queue_step commands
Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
parent
4cbcd45ce9
commit
81428265bd
|
@ -35,6 +35,10 @@ defs_stepcompress = """
|
|||
, int32_t set_next_step_dir_msgtag);
|
||||
void stepcompress_free(struct stepcompress *sc);
|
||||
int stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock);
|
||||
int stepcompress_set_last_position(struct stepcompress *sc
|
||||
, int64_t last_position);
|
||||
int64_t stepcompress_find_past_position(struct stepcompress *sc
|
||||
, uint64_t clock);
|
||||
int stepcompress_queue_msg(struct stepcompress *sc
|
||||
, uint32_t *data, int len);
|
||||
|
||||
|
|
|
@ -14,6 +14,7 @@
|
|||
// This code is written in C (instead of python) for processing
|
||||
// efficiency - the repetitive integer math is vastly faster in C.
|
||||
|
||||
#include <math.h> // sqrt
|
||||
#include <stddef.h> // offsetof
|
||||
#include <stdint.h> // uint32_t
|
||||
#include <stdio.h> // fprintf
|
||||
|
@ -42,6 +43,25 @@ struct stepcompress {
|
|||
// Step+dir+step filter
|
||||
uint64_t next_step_clock;
|
||||
int next_step_dir;
|
||||
// History tracking
|
||||
int64_t last_position;
|
||||
struct list_head history_list;
|
||||
};
|
||||
|
||||
struct step_move {
|
||||
uint32_t interval;
|
||||
uint16_t count;
|
||||
int16_t add;
|
||||
};
|
||||
|
||||
#define HISTORY_EXPIRE (30.0)
|
||||
|
||||
struct history_move {
|
||||
struct list_node node;
|
||||
uint64_t first_clock, last_clock;
|
||||
int64_t start_position;
|
||||
int step_count;
|
||||
struct step_move sm;
|
||||
};
|
||||
|
||||
|
||||
|
@ -84,12 +104,6 @@ minmax_point(struct stepcompress *sc, uint32_t *pos)
|
|||
// using 11 works well in practice.
|
||||
#define QUADRATIC_DEV 11
|
||||
|
||||
struct step_move {
|
||||
uint32_t interval;
|
||||
uint16_t count;
|
||||
int16_t add;
|
||||
};
|
||||
|
||||
// Find a 'step_move' that covers a series of step times
|
||||
static struct step_move
|
||||
compress_bisect_add(struct stepcompress *sc)
|
||||
|
@ -237,6 +251,7 @@ stepcompress_alloc(uint32_t oid)
|
|||
struct stepcompress *sc = malloc(sizeof(*sc));
|
||||
memset(sc, 0, sizeof(*sc));
|
||||
list_init(&sc->msg_queue);
|
||||
list_init(&sc->history_list);
|
||||
sc->oid = oid;
|
||||
sc->sdir = -1;
|
||||
return sc;
|
||||
|
@ -254,6 +269,20 @@ stepcompress_fill(struct stepcompress *sc, uint32_t max_error
|
|||
sc->set_next_step_dir_msgtag = set_next_step_dir_msgtag;
|
||||
}
|
||||
|
||||
// Helper to free items from the history_list
|
||||
static void
|
||||
free_history(struct stepcompress *sc, uint64_t end_clock)
|
||||
{
|
||||
while (!list_empty(&sc->history_list)) {
|
||||
struct history_move *hm = list_last_entry(
|
||||
&sc->history_list, struct history_move, node);
|
||||
if (hm->last_clock > end_clock)
|
||||
break;
|
||||
list_del(&hm->node);
|
||||
free(hm);
|
||||
}
|
||||
}
|
||||
|
||||
// Free memory associated with a 'stepcompress' object
|
||||
void __visible
|
||||
stepcompress_free(struct stepcompress *sc)
|
||||
|
@ -262,6 +291,7 @@ stepcompress_free(struct stepcompress *sc)
|
|||
return;
|
||||
free(sc->queue);
|
||||
message_queue_free(&sc->msg_queue);
|
||||
free_history(sc, UINT64_MAX);
|
||||
free(sc);
|
||||
}
|
||||
|
||||
|
@ -283,6 +313,9 @@ 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;
|
||||
|
||||
if (lsc > sc->mcu_freq * HISTORY_EXPIRE)
|
||||
free_history(sc, lsc - sc->mcu_freq * HISTORY_EXPIRE);
|
||||
}
|
||||
|
||||
// Set the conversion rate of 'print_time' to mcu clock
|
||||
|
@ -295,6 +328,39 @@ stepcompress_set_time(struct stepcompress *sc
|
|||
calc_last_step_print_time(sc);
|
||||
}
|
||||
|
||||
// Maximium clock delta between messages in the queue
|
||||
#define CLOCK_DIFF_MAX (3<<28)
|
||||
|
||||
// Helper to create a queue_step command from a 'struct step_move'
|
||||
static void
|
||||
add_move(struct stepcompress *sc, uint64_t first_clock, struct step_move *move)
|
||||
{
|
||||
int32_t addfactor = move->count*(move->count-1)/2;
|
||||
uint32_t ticks = move->add*addfactor + move->interval*(move->count-1);
|
||||
uint64_t last_clock = first_clock + ticks;
|
||||
|
||||
// Create and queue a queue_step command
|
||||
uint32_t msg[5] = {
|
||||
sc->queue_step_msgtag, sc->oid, move->interval, move->count, move->add
|
||||
};
|
||||
struct queue_message *qm = message_alloc_and_encode(msg, 5);
|
||||
qm->min_clock = qm->req_clock = sc->last_step_clock;
|
||||
if (move->count == 1 && first_clock >= sc->last_step_clock + CLOCK_DIFF_MAX)
|
||||
qm->req_clock = first_clock;
|
||||
list_add_tail(&qm->node, &sc->msg_queue);
|
||||
sc->last_step_clock = last_clock;
|
||||
|
||||
// Create and store move in history tracking
|
||||
struct history_move *hm = malloc(sizeof(*hm));
|
||||
hm->first_clock = first_clock;
|
||||
hm->last_clock = last_clock;
|
||||
hm->start_position = sc->last_position;
|
||||
hm->step_count = sc->sdir ? move->count : -move->count;
|
||||
sc->last_position += hm->step_count;
|
||||
memcpy(&hm->sm, move, sizeof(hm->sm));
|
||||
list_add_head(&hm->node, &sc->history_list);
|
||||
}
|
||||
|
||||
// Convert previously scheduled steps into commands for the mcu
|
||||
static int
|
||||
queue_flush(struct stepcompress *sc, uint64_t move_clock)
|
||||
|
@ -307,15 +373,7 @@ queue_flush(struct stepcompress *sc, uint64_t move_clock)
|
|||
if (ret)
|
||||
return ret;
|
||||
|
||||
uint32_t msg[5] = {
|
||||
sc->queue_step_msgtag, sc->oid, move.interval, move.count, move.add
|
||||
};
|
||||
struct queue_message *qm = message_alloc_and_encode(msg, 5);
|
||||
qm->min_clock = qm->req_clock = sc->last_step_clock;
|
||||
int32_t addfactor = move.count*(move.count-1)/2;
|
||||
uint32_t ticks = move.add*addfactor + move.interval*move.count;
|
||||
sc->last_step_clock += ticks;
|
||||
list_add_tail(&qm->node, &sc->msg_queue);
|
||||
add_move(sc, sc->last_step_clock + move.interval, &move);
|
||||
|
||||
if (sc->queue_pos + move.count >= sc->queue_next) {
|
||||
sc->queue_pos = sc->queue_next = sc->queue;
|
||||
|
@ -331,14 +389,8 @@ queue_flush(struct stepcompress *sc, uint64_t move_clock)
|
|||
static int
|
||||
stepcompress_flush_far(struct stepcompress *sc, uint64_t abs_step_clock)
|
||||
{
|
||||
uint32_t msg[5] = {
|
||||
sc->queue_step_msgtag, sc->oid, abs_step_clock - sc->last_step_clock,
|
||||
1, 0
|
||||
};
|
||||
struct queue_message *qm = message_alloc_and_encode(msg, 5);
|
||||
qm->min_clock = sc->last_step_clock;
|
||||
sc->last_step_clock = qm->req_clock = abs_step_clock;
|
||||
list_add_tail(&qm->node, &sc->msg_queue);
|
||||
struct step_move move = { abs_step_clock - sc->last_step_clock, 1, 0 };
|
||||
add_move(sc, abs_step_clock, &move);
|
||||
calc_last_step_print_time(sc);
|
||||
return 0;
|
||||
}
|
||||
|
@ -349,10 +401,10 @@ set_next_step_dir(struct stepcompress *sc, int sdir)
|
|||
{
|
||||
if (sc->sdir == sdir)
|
||||
return 0;
|
||||
sc->sdir = sdir;
|
||||
int ret = queue_flush(sc, UINT64_MAX);
|
||||
if (ret)
|
||||
return ret;
|
||||
sc->sdir = sdir;
|
||||
uint32_t msg[3] = {
|
||||
sc->set_next_step_dir_msgtag, sc->oid, sdir ^ sc->invert_sdir
|
||||
};
|
||||
|
@ -362,9 +414,6 @@ set_next_step_dir(struct stepcompress *sc, int sdir)
|
|||
return 0;
|
||||
}
|
||||
|
||||
// Maximium clock delta between messages in the queue
|
||||
#define CLOCK_DIFF_MAX (3<<28)
|
||||
|
||||
// Slow path for queue_append() - handle next step far in future
|
||||
static int
|
||||
queue_append_far(struct stepcompress *sc)
|
||||
|
@ -502,6 +551,46 @@ stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock)
|
|||
return 0;
|
||||
}
|
||||
|
||||
// Set last_position in the stepcompress object
|
||||
int __visible
|
||||
stepcompress_set_last_position(struct stepcompress *sc, int64_t last_position)
|
||||
{
|
||||
int ret = stepcompress_flush(sc, UINT64_MAX);
|
||||
if (ret)
|
||||
return ret;
|
||||
sc->last_position = last_position;
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Search history of moves to find a past position at a given clock
|
||||
int64_t __visible
|
||||
stepcompress_find_past_position(struct stepcompress *sc, uint64_t clock)
|
||||
{
|
||||
int64_t last_position = sc->last_position;
|
||||
struct history_move *hm;
|
||||
list_for_each_entry(hm, &sc->history_list, node) {
|
||||
if (clock < hm->first_clock) {
|
||||
last_position = hm->start_position;
|
||||
continue;
|
||||
}
|
||||
if (clock >= hm->last_clock)
|
||||
return hm->start_position + hm->step_count;
|
||||
int32_t interval = hm->sm.interval, add = hm->sm.add;
|
||||
int32_t ticks = (int32_t)(clock - hm->first_clock) + interval, offset;
|
||||
if (!add) {
|
||||
offset = ticks / interval;
|
||||
} else {
|
||||
// Solve for "count" using quadratic formula
|
||||
double a = .5 * add, b = interval - .5 * add, c = -ticks;
|
||||
offset = (sqrt(b*b - 4*a*c) - b) / (2. * a);
|
||||
}
|
||||
if (hm->step_count < 0)
|
||||
return hm->start_position - offset;
|
||||
return hm->start_position + offset;
|
||||
}
|
||||
return last_position;
|
||||
}
|
||||
|
||||
// Queue an mcu command to go out in order with stepper commands
|
||||
int __visible
|
||||
stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len)
|
||||
|
|
|
@ -124,6 +124,11 @@ class MCU_stepper:
|
|||
return self._tag_position
|
||||
def set_tag_position(self, position):
|
||||
self._tag_position = position
|
||||
def get_past_commanded_position(self, clock):
|
||||
ffi_main, ffi_lib = chelper.get_ffi()
|
||||
sq = self._stepqueue
|
||||
mcu_pos = ffi_lib.stepcompress_find_past_position(sq, clock)
|
||||
return mcu_pos * self._step_dist - self._mcu_position_offset
|
||||
def set_stepper_kinematics(self, sk):
|
||||
old_sk = self._stepper_kinematics
|
||||
self._stepper_kinematics = sk
|
||||
|
@ -145,7 +150,11 @@ class MCU_stepper:
|
|||
if not did_trigger or self._mcu.is_fileoutput():
|
||||
return
|
||||
params = self._get_position_cmd.send([self._oid])
|
||||
mcu_pos_dist = params['pos'] * self._step_dist
|
||||
last_pos = params['pos']
|
||||
ret = ffi_lib.stepcompress_set_last_position(self._stepqueue, last_pos)
|
||||
if ret:
|
||||
raise error("Internal error in stepcompress")
|
||||
mcu_pos_dist = last_pos * self._step_dist
|
||||
if self._invert_dir:
|
||||
mcu_pos_dist = -mcu_pos_dist
|
||||
self._mcu_position_offset = mcu_pos_dist - self.get_commanded_position()
|
||||
|
|
Loading…
Reference in New Issue