diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index 9bbb1199..91929747 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -60,6 +60,10 @@ defs_trapq = """ , double start_pos_x, double start_pos_y, double start_pos_z , double axes_d_x, double axes_d_y, double axes_d_z , double start_v, double cruise_v, double accel); + struct trapq *trapq_alloc(void); + void trapq_free(struct trapq *tq); + void trapq_add_move(struct trapq *tq, struct move *m); + void trapq_free_moves(struct trapq *tq, double print_time); """ defs_kin_cartesian = """ diff --git a/klippy/chelper/trapq.c b/klippy/chelper/trapq.c index c17471e7..828d48cf 100644 --- a/klippy/chelper/trapq.c +++ b/klippy/chelper/trapq.c @@ -5,11 +5,13 @@ // This file may be distributed under the terms of the GNU GPLv3 license. #include // sqrt +#include // offsetof #include // malloc #include // memset #include "compiler.h" // unlikely #include "trapq.h" // move_get_coord +// Allocate a new 'move' object struct move * __visible move_alloc(void) { @@ -85,3 +87,47 @@ move_get_coord(struct move *m, double move_time) .y = m->start_pos.y + m->axes_r.y * move_dist, .z = m->start_pos.z + m->axes_r.z * move_dist }; } + +// Allocate a new 'trapq' object +struct trapq * __visible +trapq_alloc(void) +{ + struct trapq *tq = malloc(sizeof(*tq)); + memset(tq, 0, sizeof(*tq)); + list_init(&tq->moves); + return tq; +} + +// Free memory associated with a 'trapq' object +void __visible +trapq_free(struct trapq *tq) +{ + while (!list_empty(&tq->moves)) { + struct move *m = list_first_entry(&tq->moves, struct move, node); + list_del(&m->node); + free(m); + } + free(tq); +} + +// Add a move to the trapezoid velocity queue +void __visible +trapq_add_move(struct trapq *tq, struct move *m) +{ + struct move *nm = move_alloc(); + memcpy(nm, m, sizeof(*nm)); + list_add_tail(&nm->node, &tq->moves); +} + +// 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); + if (m->print_time + m->move_t > print_time) + return; + list_del(&m->node); + free(m); + } +} diff --git a/klippy/chelper/trapq.h b/klippy/chelper/trapq.h index 15d7b44c..e568a1ab 100644 --- a/klippy/chelper/trapq.h +++ b/klippy/chelper/trapq.h @@ -1,6 +1,8 @@ #ifndef TRAPQ_H #define TRAPQ_H +#include "list.h" // list_node + struct coord { double x, y, z; }; @@ -16,6 +18,8 @@ struct move { double cruise_v; struct move_accel accel, decel; struct coord start_pos, axes_r; + + struct list_node node; }; struct move *move_alloc(void); @@ -27,4 +31,13 @@ void move_fill(struct move *m, double print_time double move_get_distance(struct move *m, double move_time); struct coord move_get_coord(struct move *m, double move_time); +struct trapq { + struct list_head moves; +}; + +struct trapq *trapq_alloc(void); +void trapq_free(struct trapq *tq); +void trapq_add_move(struct trapq *tq, struct move *m); +void trapq_free_moves(struct trapq *tq, double print_time); + #endif // trapq.h diff --git a/klippy/toolhead.py b/klippy/toolhead.py index b354f807..d383f3ca 100644 --- a/klippy/toolhead.py +++ b/klippy/toolhead.py @@ -1,6 +1,6 @@ # Code for coordinating events on the printer toolhead # -# Copyright (C) 2016-2018 Kevin O'Connor +# Copyright (C) 2016-2019 Kevin O'Connor # # This file may be distributed under the terms of the GNU GPLv3 license. import math, logging, importlib @@ -103,6 +103,7 @@ class Move: self.start_pos[0], self.start_pos[1], self.start_pos[2], self.axes_d[0], self.axes_d[1], self.axes_d[2], self.start_v, self.cruise_v, self.accel) + self.toolhead.trapq_add_move(self.toolhead.trapq, self.cmove) self.toolhead.kin.move(next_move_time, self) if self.axes_d[3]: self.toolhead.extruder.move(next_move_time, self) @@ -249,6 +250,10 @@ class ToolHead: ffi_main, ffi_lib = chelper.get_ffi() self.cmove = ffi_main.gc(ffi_lib.move_alloc(), ffi_lib.free) self.move_fill = ffi_lib.move_fill + self.trapq = ffi_main.gc(ffi_lib.trapq_alloc(), ffi_lib.trapq_free) + self.trapq_add_move = ffi_lib.trapq_add_move + self.trapq_free_moves = ffi_lib.trapq_free_moves + self.move_handlers = [] # Create kinematics class self.extruder = kinematics.extruder.DummyExtruder() self.move_queue.set_extruder(self.extruder) @@ -276,9 +281,13 @@ class ToolHead: self.printer.try_load_module(config, "manual_probe") self.printer.try_load_module(config, "tuning_tower") # Print time tracking - def update_move_time(self, movetime): - self.print_time += movetime - flush_to_time = self.print_time - self.move_flush_time + def update_move_time(self, movetime, lazy=True): + self.print_time = flush_to_time = self.print_time + movetime + for mh in self.move_handlers: + mh(flush_to_time) + self.trapq_free_moves(self.trapq, flush_to_time) + if lazy: + flush_to_time -= self.move_flush_time for m in self.all_mcus: m.flush_moves(flush_to_time) def _calc_print_time(self): @@ -317,8 +326,7 @@ class ToolHead: self.reactor.update_timer(self.flush_timer, self.reactor.NEVER) self.move_queue.set_flush_time(self.buffer_time_high) self.idle_flush_print_time = 0. - for m in self.all_mcus: - m.flush_moves(self.print_time) + self.update_move_time(0., lazy=False) def _flush_lookahead(self): if self.special_queuing_state: return self._full_flush() @@ -490,6 +498,10 @@ class ToolHead: self.move_queue.reset() def get_kinematics(self): return self.kin + def get_trapq(self): + return self.trapq + def register_move_handler(self, handler): + self.move_handlers.append(handler) def get_max_velocity(self): return self.max_velocity, self.max_accel def get_max_axis_halt(self):