From 1b6b7fc58c5037256f9d0935c94916a51bdfd6b3 Mon Sep 17 00:00:00 2001
From: Kevin O'Connor <kevin@koconnor.net>
Date: Fri, 23 Oct 2020 22:59:20 -0400
Subject: [PATCH] kin_extruder: Fix numerical stability when using pressure
 advance

Avoid using the absolute E position when calculating pressure advance
as that position can grow arbitrarily large, which can result in
"numerical stability" problems.  That instability could eventually
lead to internal errors during step compression.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
---
 klippy/chelper/kin_extruder.c | 18 +++++++++++-------
 1 file changed, 11 insertions(+), 7 deletions(-)

diff --git a/klippy/chelper/kin_extruder.c b/klippy/chelper/kin_extruder.c
index 2fce006b..63dca909 100644
--- a/klippy/chelper/kin_extruder.c
+++ b/klippy/chelper/kin_extruder.c
@@ -52,7 +52,8 @@ extruder_integrate_time(double base, double start_v, double half_accel
 
 // Calculate the definitive integral of extruder for a given move
 static double
-pa_move_integrate(struct move *m, double start, double end, double time_offset)
+pa_move_integrate(struct move *m, double base, double start, double end,
+                  double time_offset)
 {
     if (start < 0.)
         start = 0.;
@@ -60,7 +61,7 @@ pa_move_integrate(struct move *m, double start, double end, double time_offset)
         end = m->move_t;
     // Calculate base position and velocity with pressure advance
     double pressure_advance = m->axes_r.y;
-    double base = m->start_pos.x + pressure_advance * m->start_v;
+    base += 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;
@@ -75,20 +76,23 @@ pa_range_integrate(struct move *m, double move_time, double hst)
 {
     // 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);
+    double start_base = m->start_pos.x;
+    res += pa_move_integrate(m, 0., start, move_time, start);
+    res -= pa_move_integrate(m, 0., 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, start);
+        double base = prev->start_pos.x - start_base;
+        res += pa_move_integrate(prev, base, 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, end);
+        double base = m->start_pos.x - start_base;
+        res -= pa_move_integrate(m, base, 0., end, end);
     }
     return res;
 }
@@ -109,7 +113,7 @@ extruder_calc_position(struct stepper_kinematics *sk, struct move *m
         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);
-    return area * es->inv_half_smooth_time2;
+    return m->start_pos.x + area * es->inv_half_smooth_time2;
 }
 
 void __visible