From 4dbe795ac2b9af24c3c45d4d25ac0eedf86f3d12 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Fri, 1 Nov 2019 18:32:06 -0400 Subject: [PATCH] trapq: Implement sentinel nodes on the trapq list Use sentinels to make list traversal code simpler. Also add in null moves so that there are no time gaps in the list. Signed-off-by: Kevin O'Connor --- klippy/chelper/itersolve.c | 23 +++++++----------- klippy/chelper/trapq.c | 48 +++++++++++++++++++++++++++++++++++--- klippy/chelper/trapq.h | 1 + 3 files changed, 55 insertions(+), 17 deletions(-) diff --git a/klippy/chelper/itersolve.c b/klippy/chelper/itersolve.c index 5eafd2a7..84fd5f71 100644 --- a/klippy/chelper/itersolve.c +++ b/klippy/chelper/itersolve.c @@ -144,14 +144,12 @@ itersolve_generate_steps(struct stepper_kinematics *sk, double flush_time) { double last_flush_time = sk->last_flush_time; sk->last_flush_time = flush_time; - if (!sk->tq || list_empty(&sk->tq->moves)) + if (!sk->tq) return 0; + trapq_check_sentinels(sk->tq); struct move *m = list_first_entry(&sk->tq->moves, struct move, node); - while (last_flush_time >= m->print_time + m->move_t) { - if (list_is_last(&m->node, &sk->tq->moves)) - return 0; + while (last_flush_time >= m->print_time + m->move_t) m = list_next_entry(m, node); - } for (;;) { double start = m->print_time, end = start + m->move_t; if (start < last_flush_time) @@ -166,7 +164,7 @@ itersolve_generate_steps(struct stepper_kinematics *sk, double flush_time) return ret; } last_flush_time = end; - if (list_is_last(&m->node, &sk->tq->moves)) + if (flush_time <= m->print_time + m->move_t) return 0; m = list_next_entry(m, node); } @@ -176,22 +174,19 @@ itersolve_generate_steps(struct stepper_kinematics *sk, double flush_time) double __visible itersolve_check_active(struct stepper_kinematics *sk, double flush_time) { - if (!sk->tq || list_empty(&sk->tq->moves)) + if (!sk->tq) return 0.; + trapq_check_sentinels(sk->tq); struct move *m = list_first_entry(&sk->tq->moves, struct move, node); - while (sk->last_flush_time >= m->print_time + m->move_t) { - if (list_is_last(&m->node, &sk->tq->moves)) - return 0.; + while (sk->last_flush_time >= m->print_time + m->move_t) m = list_next_entry(m, node); - } - while (m->print_time < flush_time) { + for (;;) { if (check_active(sk, m)) return m->print_time; - if (list_is_last(&m->node, &sk->tq->moves)) + if (flush_time <= m->print_time + m->move_t) return 0.; m = list_next_entry(m, node); } - return 0.; } void __visible diff --git a/klippy/chelper/trapq.c b/klippy/chelper/trapq.c index ed6535dc..603ce9e2 100644 --- a/klippy/chelper/trapq.c +++ b/klippy/chelper/trapq.c @@ -86,6 +86,8 @@ move_get_coord(struct move *m, double move_time) .z = m->start_pos.z + m->axes_r.z * move_dist }; } +#define NEVER_TIME 9999999999999999.9 + // Allocate a new 'trapq' object struct trapq * __visible trapq_alloc(void) @@ -93,6 +95,10 @@ trapq_alloc(void) struct trapq *tq = malloc(sizeof(*tq)); memset(tq, 0, sizeof(*tq)); list_init(&tq->moves); + struct move *head_sentinel = move_alloc(), *tail_sentinel = move_alloc(); + tail_sentinel->print_time = tail_sentinel->move_t = NEVER_TIME; + list_add_head(&head_sentinel->node, &tq->moves); + list_add_tail(&tail_sentinel->node, &tq->moves); return tq; } @@ -108,19 +114,55 @@ trapq_free(struct trapq *tq) free(tq); } +// Update the list sentinels +void +trapq_check_sentinels(struct trapq *tq) +{ + struct move *tail_sentinel = list_last_entry(&tq->moves, struct move, node); + if (tail_sentinel->print_time) + // Already up to date + return; + struct move *m = list_prev_entry(tail_sentinel, node); + struct move *head_sentinel = list_first_entry(&tq->moves, struct move,node); + if (m == head_sentinel) { + // No moves at all on this list + tail_sentinel->print_time = NEVER_TIME; + return; + } + tail_sentinel->print_time = m->print_time + m->move_t; + tail_sentinel->start_pos = move_get_coord(m, m->move_t); +} + // Add a move to the trapezoid velocity queue void trapq_add_move(struct trapq *tq, struct move *m) { - list_add_tail(&m->node, &tq->moves); + struct move *tail_sentinel = list_last_entry(&tq->moves, struct move, node); + struct move *prev = list_prev_entry(tail_sentinel, node); + if (prev->print_time + prev->move_t < m->print_time) { + // Add a null move to fill time gap + struct move *null_move = move_alloc(); + null_move->start_pos = m->start_pos; + null_move->print_time = prev->print_time + prev->move_t; + null_move->move_t = m->print_time - null_move->print_time; + list_add_before(&null_move->node, &tail_sentinel->node); + } + list_add_before(&m->node, &tail_sentinel->node); + tail_sentinel->print_time = 0.; } // Free any moves older than `print_time` from the trapezoid velocity queue void __visible trapq_free_moves(struct trapq *tq, double print_time) { - while (!list_empty(&tq->moves)) { - struct move *m = list_first_entry(&tq->moves, struct move, node); + struct move *head_sentinel = list_first_entry(&tq->moves, struct move,node); + struct move *tail_sentinel = list_last_entry(&tq->moves, struct move, node); + for (;;) { + struct move *m = list_next_entry(head_sentinel, node); + if (m == tail_sentinel) { + tail_sentinel->print_time = NEVER_TIME; + return; + } if (m->print_time + m->move_t > print_time) return; list_del(&m->node); diff --git a/klippy/chelper/trapq.h b/klippy/chelper/trapq.h index ec762621..4572ef1a 100644 --- a/klippy/chelper/trapq.h +++ b/klippy/chelper/trapq.h @@ -29,6 +29,7 @@ double move_get_distance(struct move *m, double move_time); struct coord move_get_coord(struct move *m, double move_time); struct trapq *trapq_alloc(void); void trapq_free(struct trapq *tq); +void trapq_check_sentinels(struct trapq *tq); void trapq_add_move(struct trapq *tq, struct move *m); void trapq_free_moves(struct trapq *tq, double print_time);