kin_extruder: Convert pressure advance to use "weighted average"

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

4 files changed, 66 insertions, 27 deletions

diff --git a/docs/Kinematics.md b/docs/Kinematics.md
index f4b84550..ee501dff 100644
--- a/docs/Kinematics.md
+++ b/docs/Kinematics.md
@@ -284,8 +284,8 @@ rate, the more filament must be pushed in during acceleration to
 account for pressure. During head deceleration the extra filament is
 retracted (the extruder will have a negative velocity).
 
-The "smoothing" is implemented by averaging the extruder position over
-a small time period (as specified by the
+The "smoothing" is implemented using a weighted average of the
+extruder position over a small time period (as specified by the
 `pressure_advance_smooth_time` config parameter). This averaging can
 span multiple g-code moves. Note how the extruder motor will start
 moving prior to the nominal start of the first extrusion move and will
@@ -294,6 +294,7 @@ continue to move after the nominal end of the last extrusion move.
 Key formula for "smoothed pressure advance":
 ```
 smooth_pa_position(t) =
-    ( definitive_integral(pa_position, from=t-smooth_time/2, to=t+smooth_time/2)
-     / smooth_time )
+    ( 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 )
 ```
diff --git a/docs/img/pressure-velocity.png b/docs/img/pressure-velocity.png
index 89e91ab4..5268a948 100644
--- a/docs/img/pressure-velocity.png
+++ b/docs/img/pressure-velocity.png
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
diff --git a/scripts/graph_extruder.py b/scripts/graph_extruder.py
index 6223bb11..5c0fb5cf 100755
--- a/scripts/graph_extruder.py
+++ b/scripts/graph_extruder.py
@@ -67,13 +67,16 @@ def calc_pa_raw(t, positions):
     i = time_to_index(t)
     return positions[i] + pa * (positions[i+1] - positions[i])
 
-def calc_pa_smooth(t, positions):
+def calc_pa_weighted(t, positions):
+    base_index = time_to_index(t)
     start_index = time_to_index(t - PA_HALF_SMOOTH_T) + 1
     end_index = time_to_index(t + PA_HALF_SMOOTH_T)
+    diff = .5 * (end_index - start_index)
     pa = PRESSURE_ADVANCE * INV_SEG_TIME
-    pa_data = [positions[i] + pa * (positions[i+1] - positions[i])
+    pa_data = [(positions[i] + pa * (positions[i+1] - positions[i]))
+               * (diff - abs(i-base_index))
                for i in range(start_index, end_index)]
-    return sum(pa_data) / (end_index - start_index)
+    return sum(pa_data) / diff**2
 
 
 ######################################################################
@@ -92,7 +95,7 @@ def plot_motion():
     pa_positions = [calc_pa_raw(t, positions) for t in times]
     pa_velocities = gen_deriv(pa_positions)
     # Smoothed motion
-    sm_positions = [calc_pa_smooth(t, positions) for t in times]
+    sm_positions = [calc_pa_weighted(t, positions) for t in times]
     sm_velocities = gen_deriv(sm_positions)
     # Build plot
     shift_times = [t - MARGIN_TIME for t in times]