3diy
/
klipper
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

stepcompress: Add support for tracking history of queue_step commands 

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2021-02-22 17:57:36 -05:00
parent 4cbcd45ce9
commit 81428265bd
3 changed files with 130 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
klippy/chelper/__init__.py
View File

@ -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);

143
klippy/chelper/stepcompress.c
View File

@ -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)

11
klippy/stepper.py
View File

@ -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()