kin_extruder: Convert pressure advance to use "weighted average"

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

1 files changed, 54 insertions, 19 deletions

diff --git a/klippy/chelper/kin_extruder.c b/klippy/chelper/kin_extruder.c
index d630f7dd..2fce006b 100644
--- a/klippy/chelper/kin_extruder.c
+++ b/klippy/chelper/kin_extruder.c
@@ -12,55 +12,90 @@
 #include "pyhelper.h" // errorf
 #include "trapq.h" // move_get_distance
 
-// Calculate the definitive integral of the move distance
+// Without pressure advance, the extruder stepper position is:
+//     extruder_position(t) = nominal_position(t)
+// When pressure advance is enabled, additional filament is pushed
+// into the extruder during acceleration (and retracted during
+// deceleration). The formula is:
+//     pa_position(t) = (nominal_position(t)
+//                       + pressure_advance * nominal_velocity(t))
+// Which is then "smoothed" using a weighted average:
+//     smooth_position(t) = (
+//         definitive_integral(pa_position(x) * (smooth_time/2 - abs(t-x)) * dx,
+//                             from=t-smooth_time/2, to=t+smooth_time/2)
+//         / ((smooth_time/2)**2))
+
+// Calculate the definitive integral of the motion formula:
+//   position(t) = base + t * (start_v + t * half_accel)
+static double
+extruder_integrate(double base, double start_v, double half_accel
+                   , double start, double end)
+{
+    double half_v = .5 * start_v, sixth_a = (1. / 3.) * half_accel;
+    double si = start * (base + start * (half_v + start * sixth_a));
+    double ei = end * (base + end * (half_v + end * sixth_a));
+    return ei - si;
+}
+
+// Calculate the definitive integral of time weighted position:
+//   weighted_position(t) = t * (base + t * (start_v + t * half_accel))
 static double
-move_integrate_distance(struct move *m, double start, double end)
+extruder_integrate_time(double base, double start_v, double half_accel
+                        , double start, double end)
 {
-    double half_v = .5 * m->start_v, sixth_a = (1. / 3.) * m->half_accel;
-    double si = start * start * (half_v + sixth_a * start);
-    double ei = end * end * (half_v + sixth_a * end);
+    double half_b = .5 * base, third_v = (1. / 3.) * start_v;
+    double eighth_a = .25 * half_accel;
+    double si = start * start * (half_b + start * (third_v + start * eighth_a));
+    double ei = end * end * (half_b + end * (third_v + end * eighth_a));
     return ei - si;
 }
 
-// Calculate the definitive integral of extruder with pressure advance
+// Calculate the definitive integral of extruder for a given move
 static double
-pa_move_integrate(struct move *m, double start, double end)
+pa_move_integrate(struct move *m, double start, double end, double time_offset)
 {
     if (start < 0.)
         start = 0.;
     if (end > m->move_t)
         end = m->move_t;
+    // Calculate base position and velocity with pressure advance
     double pressure_advance = m->axes_r.y;
-    double avg_v = m->start_v + (start + end) * m->half_accel;
-    double pa_add = pressure_advance * avg_v;
-    double base = (m->start_pos.x + pa_add) * (end - start);
-    return base + move_integrate_distance(m, start, end);
+    double base = m->start_pos.x + pressure_advance * m->start_v;
+    double start_v = m->start_v + pressure_advance * 2. * m->half_accel;
+    // Calculate definitive integral
+    double ha = m->half_accel;
+    double iext = extruder_integrate(base, start_v, ha, start, end);
+    double wgt_ext = extruder_integrate_time(base, start_v, ha, start, end);
+    return wgt_ext - time_offset * iext;
 }
 
 // Calculate the definitive integral of the extruder over a range of moves
 static double
-pa_range_integrate(struct move *m, double start, double end)
+pa_range_integrate(struct move *m, double move_time, double hst)
 {
-    double res = pa_move_integrate(m, start, end);
+    // Calculate integral for the current move
+    double res = 0., start = move_time - hst, end = move_time + hst;
+    res += pa_move_integrate(m, start, move_time, start);
+    res -= pa_move_integrate(m, move_time, end, end);
     // Integrate over previous moves
     struct move *prev = m;
     while (unlikely(start < 0.)) {
         prev = list_prev_entry(prev, node);
         start += prev->move_t;
-        res += pa_move_integrate(prev, start, prev->move_t);
+        res += pa_move_integrate(prev, start, prev->move_t, start);
     }
     // Integrate over future moves
     while (unlikely(end > m->move_t)) {
         end -= m->move_t;
         m = list_next_entry(m, node);
-        res += pa_move_integrate(m, 0., end);
+        res -= pa_move_integrate(m, 0., end, end);
     }
     return res;
 }
 
 struct extruder_stepper {
     struct stepper_kinematics sk;
-    double half_smooth_time, inv_smooth_time;
+    double half_smooth_time, inv_half_smooth_time2;
 };
 
 static double
@@ -73,8 +108,8 @@ extruder_calc_position(struct stepper_kinematics *sk, struct move *m
         // Pressure advance not enabled
         return m->start_pos.x + move_get_distance(m, move_time);
     // Apply pressure advance and average over smooth_time
-    double area = pa_range_integrate(m, move_time - hst, move_time + hst);
-    return area * es->inv_smooth_time;
+    double area = pa_range_integrate(m, move_time, hst);
+    return area * es->inv_half_smooth_time2;
 }
 
 void __visible
@@ -86,7 +121,7 @@ extruder_set_smooth_time(struct stepper_kinematics *sk, double smooth_time)
     es->sk.gen_steps_pre_active = es->sk.gen_steps_post_active = hst;
     if (! hst)
         return;
-    es->inv_smooth_time = 1. / smooth_time;
+    es->inv_half_smooth_time2 = 1. / (hst * hst);
 }
 
 struct stepper_kinematics * __visible