stepcompress: Support extracting stepcompress history

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2021-07-19 14:51:05 -04:00
parent fbfa31a3c3
commit 7013a7b15f
4 changed files with 59 additions and 4 deletions

View File

@ -30,6 +30,12 @@ OTHER_FILES = [
] ]
defs_stepcompress = """ defs_stepcompress = """
struct pull_history_steps {
uint64_t first_clock, last_clock;
int64_t start_position;
int step_count, interval, add;
};
struct stepcompress *stepcompress_alloc(uint32_t oid); struct stepcompress *stepcompress_alloc(uint32_t oid);
void stepcompress_fill(struct stepcompress *sc, uint32_t max_error void stepcompress_fill(struct stepcompress *sc, uint32_t max_error
, uint32_t invert_sdir, int32_t queue_step_msgtag , uint32_t invert_sdir, int32_t queue_step_msgtag
@ -42,6 +48,9 @@ defs_stepcompress = """
, uint64_t clock); , 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);
int stepcompress_extract_old(struct stepcompress *sc
, struct pull_history_steps *p, int max
, uint64_t start_clock, uint64_t end_clock);
struct steppersync *steppersync_alloc(struct serialqueue *sq struct steppersync *steppersync_alloc(struct serialqueue *sq
, struct stepcompress **sc_list, int sc_num, int move_num); , struct stepcompress **sc_list, int sc_num, int move_num);

View File

@ -60,8 +60,7 @@ struct history_steps {
struct list_node node; struct list_node node;
uint64_t first_clock, last_clock; uint64_t first_clock, last_clock;
int64_t start_position; int64_t start_position;
int step_count; int step_count, interval, add;
struct step_move sm;
}; };
@ -355,9 +354,10 @@ add_move(struct stepcompress *sc, uint64_t first_clock, struct step_move *move)
hs->first_clock = first_clock; hs->first_clock = first_clock;
hs->last_clock = last_clock; hs->last_clock = last_clock;
hs->start_position = sc->last_position; hs->start_position = sc->last_position;
hs->interval = move->interval;
hs->add = move->add;
hs->step_count = sc->sdir ? move->count : -move->count; hs->step_count = sc->sdir ? move->count : -move->count;
sc->last_position += hs->step_count; sc->last_position += hs->step_count;
memcpy(&hs->sm, move, sizeof(hs->sm));
list_add_head(&hs->node, &sc->history_list); list_add_head(&hs->node, &sc->history_list);
} }
@ -559,6 +559,13 @@ stepcompress_set_last_position(struct stepcompress *sc, int64_t last_position)
if (ret) if (ret)
return ret; return ret;
sc->last_position = last_position; sc->last_position = last_position;
// Add a marker to the history list
struct history_steps *hs = malloc(sizeof(*hs));
memset(hs, 0, sizeof(*hs));
hs->first_clock = hs->last_clock = sc->last_step_clock;
hs->start_position = last_position;
list_add_head(&hs->node, &sc->history_list);
return 0; return 0;
} }
@ -575,7 +582,7 @@ stepcompress_find_past_position(struct stepcompress *sc, uint64_t clock)
} }
if (clock >= hs->last_clock) if (clock >= hs->last_clock)
return hs->start_position + hs->step_count; return hs->start_position + hs->step_count;
int32_t interval = hs->sm.interval, add = hs->sm.add; int32_t interval = hs->interval, add = hs->add;
int32_t ticks = (int32_t)(clock - hs->first_clock) + interval, offset; int32_t ticks = (int32_t)(clock - hs->first_clock) + interval, offset;
if (!add) { if (!add) {
offset = ticks / interval; offset = ticks / interval;
@ -605,6 +612,30 @@ stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len)
return 0; return 0;
} }
// Return history of queue_step commands
int __visible
stepcompress_extract_old(struct stepcompress *sc, struct pull_history_steps *p
, int max, uint64_t start_clock, uint64_t end_clock)
{
int res = 0;
struct history_steps *hs;
list_for_each_entry(hs, &sc->history_list, node) {
if (start_clock >= hs->last_clock || res >= max)
break;
if (end_clock <= hs->first_clock)
continue;
p->first_clock = hs->first_clock;
p->last_clock = hs->last_clock;
p->start_position = hs->start_position;
p->step_count = hs->step_count;
p->interval = hs->interval;
p->add = hs->add;
p++;
res++;
}
return res;
}
/**************************************************************** /****************************************************************
* Step compress synchronization * Step compress synchronization

View File

@ -5,6 +5,12 @@
#define ERROR_RET -989898989 #define ERROR_RET -989898989
struct pull_history_steps {
uint64_t first_clock, last_clock;
int64_t start_position;
int step_count, interval, add;
};
struct stepcompress *stepcompress_alloc(uint32_t oid); struct stepcompress *stepcompress_alloc(uint32_t oid);
void stepcompress_fill(struct stepcompress *sc, uint32_t max_error void stepcompress_fill(struct stepcompress *sc, uint32_t max_error
, uint32_t invert_sdir, int32_t queue_step_msgtag , uint32_t invert_sdir, int32_t queue_step_msgtag
@ -21,6 +27,9 @@ int stepcompress_set_last_position(struct stepcompress *sc
int64_t stepcompress_find_past_position(struct stepcompress *sc int64_t stepcompress_find_past_position(struct stepcompress *sc
, uint64_t clock); , uint64_t 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);
int stepcompress_extract_old(struct stepcompress *sc
, struct pull_history_steps *p, int max
, uint64_t start_clock, uint64_t end_clock);
struct serialqueue; struct serialqueue;
struct steppersync *steppersync_alloc( struct steppersync *steppersync_alloc(

View File

@ -119,6 +119,12 @@ class MCU_stepper:
def get_past_commanded_position(self, print_time): def get_past_commanded_position(self, print_time):
mcu_pos = self.get_past_mcu_position(print_time) mcu_pos = self.get_past_mcu_position(print_time)
return mcu_pos * self._step_dist - self._mcu_position_offset return mcu_pos * self._step_dist - self._mcu_position_offset
def dump_steps(self, count, start_clock, end_clock):
ffi_main, ffi_lib = chelper.get_ffi()
data = ffi_main.new('struct pull_history_steps[]', count)
count = ffi_lib.stepcompress_extract_old(self._stepqueue, data, count,
start_clock, end_clock)
return (data, count)
def set_stepper_kinematics(self, sk): def set_stepper_kinematics(self, sk):
old_sk = self._stepper_kinematics old_sk = self._stepper_kinematics
mcu_pos = 0 mcu_pos = 0