trapq: Initial support for building a queue of trapezoidal velocity moves

Add support for building a list of moves in the trapq.c code.  Update
the toolhead code so that moves generated from the look-ahead code are
added to that list.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

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 = """

klippy/chelper/trapq.c

@@ -5,11 +5,13 @@
 // This file may be distributed under the terms of the GNU GPLv3 license.
 
 #include <math.h> // sqrt
+#include <stddef.h> // offsetof
 #include <stdlib.h> // malloc
 #include <string.h> // 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);
+    }
+}

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

klippy/toolhead.py

@@ -1,6 +1,6 @@
 # Code for coordinating events on the printer toolhead
 #
-# Copyright (C) 2016-2018  Kevin O'Connor <kevin@koconnor.net>
+# Copyright (C) 2016-2019  Kevin O'Connor <kevin@koconnor.net>
 #
 # 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):
+    def update_move_time(self, movetime, lazy=True):
-        self.print_time += movetime
+        self.print_time = flush_to_time = self.print_time + movetime
-        flush_to_time = self.print_time - self.move_flush_time
+        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:
+        self.update_move_time(0., lazy=False)
-            m.flush_moves(self.print_time)
     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):