stepcompress: Support extracting stepcompress history
Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
parent
fbfa31a3c3
commit
7013a7b15f
|
@ -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);
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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(
|
||||||
|
|
|
@ -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
|
||||||
|
|
Loading…
Reference in New Issue