mirror of https://github.com/Desuuuu/klipper.git
trapq: Keep history of recent trapq moves
Store trapq moves in a separate "history" list after each move is nominally expired. Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
parent
e2f483aea3
commit
45c232b2c7
|
@ -68,6 +68,13 @@ defs_itersolve = """
|
|||
"""
|
||||
|
||||
defs_trapq = """
|
||||
struct pull_move {
|
||||
double print_time, move_t;
|
||||
double start_v, accel;
|
||||
double start_x, start_y, start_z;
|
||||
double x_r, y_r, z_r;
|
||||
};
|
||||
|
||||
void trapq_append(struct trapq *tq, double print_time
|
||||
, double accel_t, double cruise_t, double decel_t
|
||||
, double start_pos_x, double start_pos_y, double start_pos_z
|
||||
|
@ -76,6 +83,8 @@ defs_trapq = """
|
|||
struct trapq *trapq_alloc(void);
|
||||
void trapq_free(struct trapq *tq);
|
||||
void trapq_finalize_moves(struct trapq *tq, double print_time);
|
||||
int trapq_extract_old(struct trapq *tq, struct pull_move *p, int max
|
||||
, double start_time, double end_time);
|
||||
"""
|
||||
|
||||
defs_kin_cartesian = """
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
// Trapezoidal velocity movement queue
|
||||
//
|
||||
// Copyright (C) 2018-2019 Kevin O'Connor <kevin@koconnor.net>
|
||||
// Copyright (C) 2018-2021 Kevin O'Connor <kevin@koconnor.net>
|
||||
//
|
||||
// This file may be distributed under the terms of the GNU GPLv3 license.
|
||||
|
||||
|
@ -95,6 +95,7 @@ trapq_alloc(void)
|
|||
struct trapq *tq = malloc(sizeof(*tq));
|
||||
memset(tq, 0, sizeof(*tq));
|
||||
list_init(&tq->moves);
|
||||
list_init(&tq->history);
|
||||
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);
|
||||
|
@ -111,6 +112,11 @@ trapq_free(struct trapq *tq)
|
|||
list_del(&m->node);
|
||||
free(m);
|
||||
}
|
||||
while (!list_empty(&tq->history)) {
|
||||
struct move *m = list_first_entry(&tq->history, struct move, node);
|
||||
list_del(&m->node);
|
||||
free(m);
|
||||
}
|
||||
free(tq);
|
||||
}
|
||||
|
||||
|
@ -157,21 +163,64 @@ trapq_add_move(struct trapq *tq, struct move *m)
|
|||
tail_sentinel->print_time = 0.;
|
||||
}
|
||||
|
||||
#define HISTORY_EXPIRE (30.0)
|
||||
|
||||
// Expire any moves older than `print_time` from the trapezoid velocity queue
|
||||
void __visible
|
||||
trapq_finalize_moves(struct trapq *tq, double print_time)
|
||||
{
|
||||
struct move *head_sentinel = list_first_entry(&tq->moves, struct move,node);
|
||||
struct move *tail_sentinel = list_last_entry(&tq->moves, struct move, node);
|
||||
// Move expired moves from main "moves" list to "history" list
|
||||
for (;;) {
|
||||
struct move *m = list_next_entry(head_sentinel, node);
|
||||
if (m == tail_sentinel) {
|
||||
tail_sentinel->print_time = NEVER_TIME;
|
||||
return;
|
||||
break;
|
||||
}
|
||||
if (m->print_time + m->move_t > print_time)
|
||||
return;
|
||||
break;
|
||||
list_del(&m->node);
|
||||
list_add_head(&m->node, &tq->history);
|
||||
}
|
||||
// Free old moves from history list
|
||||
if (list_empty(&tq->history))
|
||||
return;
|
||||
struct move *latest = list_first_entry(&tq->history, struct move, node);
|
||||
double expire_time = latest->print_time + latest->move_t - HISTORY_EXPIRE;
|
||||
for (;;) {
|
||||
struct move *m = list_last_entry(&tq->history, struct move, node);
|
||||
if (m == latest || m->print_time + m->move_t > expire_time)
|
||||
break;
|
||||
list_del(&m->node);
|
||||
free(m);
|
||||
}
|
||||
}
|
||||
|
||||
// Return history of movement queue
|
||||
int __visible
|
||||
trapq_extract_old(struct trapq *tq, struct pull_move *p, int max
|
||||
, double start_time, double end_time)
|
||||
{
|
||||
int res = 0;
|
||||
struct move *m;
|
||||
list_for_each_entry(m, &tq->history, node) {
|
||||
if (start_time >= m->print_time + m->move_t || res >= max)
|
||||
break;
|
||||
if (end_time <= m->print_time)
|
||||
continue;
|
||||
p->print_time = m->print_time;
|
||||
p->move_t = m->move_t;
|
||||
p->start_v = m->start_v;
|
||||
p->accel = 2. * m->half_accel;
|
||||
p->start_x = m->start_pos.x;
|
||||
p->start_y = m->start_pos.y;
|
||||
p->start_z = m->start_pos.z;
|
||||
p->x_r = m->axes_r.x;
|
||||
p->y_r = m->axes_r.y;
|
||||
p->z_r = m->axes_r.z;
|
||||
p++;
|
||||
res++;
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
|
|
@ -21,7 +21,14 @@ struct move {
|
|||
};
|
||||
|
||||
struct trapq {
|
||||
struct list_head moves;
|
||||
struct list_head moves, history;
|
||||
};
|
||||
|
||||
struct pull_move {
|
||||
double print_time, move_t;
|
||||
double start_v, accel;
|
||||
double start_x, start_y, start_z;
|
||||
double x_r, y_r, z_r;
|
||||
};
|
||||
|
||||
struct move *move_alloc(void);
|
||||
|
@ -37,5 +44,7 @@ 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_finalize_moves(struct trapq *tq, double print_time);
|
||||
int trapq_extract_old(struct trapq *tq, struct pull_move *p, int max
|
||||
, double start_time, double end_time);
|
||||
|
||||
#endif // trapq.h
|
||||
|
|
Loading…
Reference in New Issue