extruder: Initial support for "smoothed pressure advance"

Support averaging the extruder position over a time range to "smooth
out" the velocity changes that occur during pressure advance.

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

2 files changed, 47 insertions, 60 deletions

diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py
index 1adce192..b0b11dac 100644
--- a/klippy/chelper/__init__.py
+++ b/klippy/chelper/__init__.py
@@ -91,10 +91,8 @@ defs_kin_winch = """
 
 defs_kin_extruder = """
     struct stepper_kinematics *extruder_stepper_alloc(void);
-    void extruder_add_move(struct trapq *tq, double print_time
-        , double accel_t, double cruise_t, double decel_t, double start_e_pos
-        , double start_v, double cruise_v, double accel
-        , double extra_accel_v, double extra_decel_v);
+    void extruder_set_pressure(struct stepper_kinematics *sk
+        , double pressure_advance, double half_smooth_time);
 """
 
 defs_serialqueue = """
diff --git a/klippy/chelper/kin_extruder.c b/klippy/chelper/kin_extruder.c
index e5e440f4..9617afe8 100644
--- a/klippy/chelper/kin_extruder.c
+++ b/klippy/chelper/kin_extruder.c
@@ -4,6 +4,7 @@
 //
 // This file may be distributed under the terms of the GNU GPLv3 license.
 
+#include <stddef.h> // offsetof
 #include <stdlib.h> // malloc
 #include <string.h> // memset
 #include "compiler.h" // __visible
@@ -11,70 +12,58 @@
 #include "pyhelper.h" // errorf
 #include "trapq.h" // move_get_distance
 
+struct extruder_stepper {
+    struct stepper_kinematics sk;
+    double pressure_advance_factor, half_smooth_time, inv_smooth_time;
+};
+
 static double
 extruder_calc_position(struct stepper_kinematics *sk, struct move *m
                        , double move_time)
 {
-    return m->start_pos.x + move_get_distance(m, move_time);
+    struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk);
+    double hst = es->half_smooth_time;
+    if (!hst)
+        // Pressure advance not enabled
+        return m->start_pos.x + move_get_distance(m, move_time);
+    // Calculate average position over smooth_time
+    double area = trapq_integrate(m, 'x', move_time - hst, move_time + hst);
+    double base_pos = area * es->inv_smooth_time;
+    // Calculate position 'half_smooth_time' in the past
+    double start_time = move_time - hst;
+    struct move *sm = trapq_find_move(m, &start_time);
+    double start_dist = move_get_distance(sm, start_time);
+    double pa_start_pos = sm->start_pos.y + (sm->axes_r.y ? start_dist : 0.);
+    // Calculate position 'half_smooth_time' in the future
+    double end_time = move_time + hst;
+    struct move *em = trapq_find_move(m, &end_time);
+    double end_dist = move_get_distance(em, end_time);
+    double pa_end_pos = em->start_pos.y + (em->axes_r.y ? end_dist : 0.);
+    // Calculate position with pressure advance
+    return base_pos + (pa_end_pos - pa_start_pos) * es->pressure_advance_factor;
 }
 
-struct stepper_kinematics * __visible
-extruder_stepper_alloc(void)
-{
-    struct stepper_kinematics *sk = malloc(sizeof(*sk));
-    memset(sk, 0, sizeof(*sk));
-    sk->calc_position_cb = extruder_calc_position;
-    sk->active_flags = AF_X;
-    return sk;
-}
-
-// Populate a 'struct move' with an extruder velocity trapezoid
 void __visible
-extruder_add_move(struct trapq *tq, double print_time
-                  , double accel_t, double cruise_t, double decel_t
-                  , double start_e_pos
-                  , double start_v, double cruise_v, double accel
-                  , double extra_accel_v, double extra_decel_v)
+extruder_set_pressure(struct stepper_kinematics *sk
+                      , double pressure_advance, double half_smooth_time)
 {
-    struct coord start_pos, axes_r;
-    start_pos.x = start_e_pos;
-    axes_r.x = 1.;
-    start_pos.y = start_pos.z = axes_r.y = axes_r.z = 0.;
-
-    if (accel_t) {
-        struct move *m = move_alloc();
-        m->print_time = print_time;
-        m->move_t = accel_t;
-        m->start_v = start_v + extra_accel_v;
-        m->half_accel = .5 * accel;
-        m->start_pos = start_pos;
-        m->axes_r = axes_r;
-        trapq_add_move(tq, m);
-
-        print_time += accel_t;
-        start_pos.x += move_get_distance(m, accel_t);
+    struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk);
+    if (! half_smooth_time) {
+        es->pressure_advance_factor = es->half_smooth_time = 0.;
+        return;
     }
-    if (cruise_t) {
-        struct move *m = move_alloc();
-        m->print_time = print_time;
-        m->move_t = cruise_t;
-        m->start_v = cruise_v;
-        m->half_accel = 0.;
-        m->start_pos = start_pos;
-        m->axes_r = axes_r;
-        trapq_add_move(tq, m);
+    es->sk.scan_past = es->sk.scan_future = half_smooth_time;
+    es->half_smooth_time = half_smooth_time;
+    es->inv_smooth_time = .5 / half_smooth_time;
+    es->pressure_advance_factor = pressure_advance * es->inv_smooth_time;
+}
 
-        print_time += cruise_t;
-        start_pos.x += move_get_distance(m, cruise_t);
-    }
-    if (decel_t) {
-        struct move *m = move_alloc();
-        m->print_time = print_time;
-        m->move_t = decel_t;
-        m->start_v = cruise_v + extra_decel_v;
-        m->half_accel = -.5 * accel;
-        m->start_pos = start_pos;
-        m->axes_r = axes_r;
-        trapq_add_move(tq, m);
-    }
+struct stepper_kinematics * __visible
+extruder_stepper_alloc(void)
+{
+    struct extruder_stepper *es = malloc(sizeof(*es));
+    memset(es, 0, sizeof(*es));
+    es->sk.calc_position_cb = extruder_calc_position;
+    es->sk.active_flags = AF_X;
+    return &es->sk;
 }