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);
|
, int32_t set_next_step_dir_msgtag);
|
||||||
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_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
|
int stepcompress_queue_msg(struct stepcompress *sc
|
||||||
, uint32_t *data, int len);
|
, uint32_t *data, int len);
|
||||||
|
|
||||||
|
|
|
@ -14,6 +14,7 @@
|
||||||
// This code is written in C (instead of python) for processing
|
// This code is written in C (instead of python) for processing
|
||||||
// efficiency - the repetitive integer math is vastly faster in C.
|
// efficiency - the repetitive integer math is vastly faster in C.
|
||||||
|
|
||||||
|
#include <math.h> // sqrt
|
||||||
#include <stddef.h> // offsetof
|
#include <stddef.h> // offsetof
|
||||||
#include <stdint.h> // uint32_t
|
#include <stdint.h> // uint32_t
|
||||||
#include <stdio.h> // fprintf
|
#include <stdio.h> // fprintf
|
||||||
|
@ -42,6 +43,25 @@ struct stepcompress {
|
||||||
// Step+dir+step filter
|
// Step+dir+step filter
|
||||||
uint64_t next_step_clock;
|
uint64_t next_step_clock;
|
||||||
int next_step_dir;
|
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.
|
// using 11 works well in practice.
|
||||||
#define QUADRATIC_DEV 11
|
#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
|
// Find a 'step_move' that covers a series of step times
|
||||||
static struct step_move
|
static struct step_move
|
||||||
compress_bisect_add(struct stepcompress *sc)
|
compress_bisect_add(struct stepcompress *sc)
|
||||||
|
@ -237,6 +251,7 @@ stepcompress_alloc(uint32_t oid)
|
||||||
struct stepcompress *sc = malloc(sizeof(*sc));
|
struct stepcompress *sc = malloc(sizeof(*sc));
|
||||||
memset(sc, 0, sizeof(*sc));
|
memset(sc, 0, sizeof(*sc));
|
||||||
list_init(&sc->msg_queue);
|
list_init(&sc->msg_queue);
|
||||||
|
list_init(&sc->history_list);
|
||||||
sc->oid = oid;
|
sc->oid = oid;
|
||||||
sc->sdir = -1;
|
sc->sdir = -1;
|
||||||
return sc;
|
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;
|
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
|
// Free memory associated with a 'stepcompress' object
|
||||||
void __visible
|
void __visible
|
||||||
stepcompress_free(struct stepcompress *sc)
|
stepcompress_free(struct stepcompress *sc)
|
||||||
|
@ -262,6 +291,7 @@ stepcompress_free(struct stepcompress *sc)
|
||||||
return;
|
return;
|
||||||
free(sc->queue);
|
free(sc->queue);
|
||||||
message_queue_free(&sc->msg_queue);
|
message_queue_free(&sc->msg_queue);
|
||||||
|
free_history(sc, UINT64_MAX);
|
||||||
free(sc);
|
free(sc);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -283,6 +313,9 @@ calc_last_step_print_time(struct stepcompress *sc)
|
||||||
{
|
{
|
||||||
double lsc = sc->last_step_clock;
|
double lsc = sc->last_step_clock;
|
||||||
sc->last_step_print_time = sc->mcu_time_offset + (lsc - .5) / sc->mcu_freq;
|
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
|
// 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);
|
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
|
// Convert previously scheduled steps into commands for the mcu
|
||||||
static int
|
static int
|
||||||
queue_flush(struct stepcompress *sc, uint64_t move_clock)
|
queue_flush(struct stepcompress *sc, uint64_t move_clock)
|
||||||
|
@ -307,15 +373,7 @@ queue_flush(struct stepcompress *sc, uint64_t move_clock)
|
||||||
if (ret)
|
if (ret)
|
||||||
return ret;
|
return ret;
|
||||||
|
|
||||||
uint32_t msg[5] = {
|
add_move(sc, sc->last_step_clock + move.interval, &move);
|
||||||
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);
|
|
||||||
|
|
||||||
if (sc->queue_pos + move.count >= sc->queue_next) {
|
if (sc->queue_pos + move.count >= sc->queue_next) {
|
||||||
sc->queue_pos = sc->queue_next = sc->queue;
|
sc->queue_pos = sc->queue_next = sc->queue;
|
||||||
|
@ -331,14 +389,8 @@ queue_flush(struct stepcompress *sc, uint64_t move_clock)
|
||||||
static int
|
static int
|
||||||
stepcompress_flush_far(struct stepcompress *sc, uint64_t abs_step_clock)
|
stepcompress_flush_far(struct stepcompress *sc, uint64_t abs_step_clock)
|
||||||
{
|
{
|
||||||
uint32_t msg[5] = {
|
struct step_move move = { abs_step_clock - sc->last_step_clock, 1, 0 };
|
||||||
sc->queue_step_msgtag, sc->oid, abs_step_clock - sc->last_step_clock,
|
add_move(sc, abs_step_clock, &move);
|
||||||
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);
|
|
||||||
calc_last_step_print_time(sc);
|
calc_last_step_print_time(sc);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
@ -349,10 +401,10 @@ set_next_step_dir(struct stepcompress *sc, int sdir)
|
||||||
{
|
{
|
||||||
if (sc->sdir == sdir)
|
if (sc->sdir == sdir)
|
||||||
return 0;
|
return 0;
|
||||||
sc->sdir = sdir;
|
|
||||||
int ret = queue_flush(sc, UINT64_MAX);
|
int ret = queue_flush(sc, UINT64_MAX);
|
||||||
if (ret)
|
if (ret)
|
||||||
return ret;
|
return ret;
|
||||||
|
sc->sdir = sdir;
|
||||||
uint32_t msg[3] = {
|
uint32_t msg[3] = {
|
||||||
sc->set_next_step_dir_msgtag, sc->oid, sdir ^ sc->invert_sdir
|
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;
|
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
|
// Slow path for queue_append() - handle next step far in future
|
||||||
static int
|
static int
|
||||||
queue_append_far(struct stepcompress *sc)
|
queue_append_far(struct stepcompress *sc)
|
||||||
|
@ -502,6 +551,46 @@ stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock)
|
||||||
return 0;
|
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
|
// Queue an mcu command to go out in order with stepper commands
|
||||||
int __visible
|
int __visible
|
||||||
stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len)
|
stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len)
|
||||||
|
|
|
@ -124,6 +124,11 @@ class MCU_stepper:
|
||||||
return self._tag_position
|
return self._tag_position
|
||||||
def set_tag_position(self, position):
|
def set_tag_position(self, position):
|
||||||
self._tag_position = 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):
|
def set_stepper_kinematics(self, sk):
|
||||||
old_sk = self._stepper_kinematics
|
old_sk = self._stepper_kinematics
|
||||||
self._stepper_kinematics = sk
|
self._stepper_kinematics = sk
|
||||||
|
@ -145,7 +150,11 @@ class MCU_stepper:
|
||||||
if not did_trigger or self._mcu.is_fileoutput():
|
if not did_trigger or self._mcu.is_fileoutput():
|
||||||
return
|
return
|
||||||
params = self._get_position_cmd.send([self._oid])
|
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:
|
if self._invert_dir:
|
||||||
mcu_pos_dist = -mcu_pos_dist
|
mcu_pos_dist = -mcu_pos_dist
|
||||||
self._mcu_position_offset = mcu_pos_dist - self.get_commanded_position()
|
self._mcu_position_offset = mcu_pos_dist - self.get_commanded_position()
|
||||||
|
|
Loading…
Reference in New Issue