kin_extruder: Apply pressure advance in kin_extruder.c

Implement the pressure advance calculations while performing the
definitive integral calculations.  This simplifies both the
extruder.py and kin_extruder.c code.

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

3 files changed, 37 insertions, 75 deletions

diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py
index b0b11dac..91114216 100644
--- a/klippy/chelper/__init__.py
+++ b/klippy/chelper/__init__.py
@@ -91,8 +91,8 @@ defs_kin_winch = """
 
 defs_kin_extruder = """
     struct stepper_kinematics *extruder_stepper_alloc(void);
-    void extruder_set_pressure(struct stepper_kinematics *sk
-        , double pressure_advance, double half_smooth_time);
+    void extruder_set_smooth_time(struct stepper_kinematics *sk
+        , double smooth_time);
 """
 
 defs_serialqueue = """
diff --git a/klippy/chelper/kin_extruder.c b/klippy/chelper/kin_extruder.c
index 1540c776..41c30c90 100644
--- a/klippy/chelper/kin_extruder.c
+++ b/klippy/chelper/kin_extruder.c
@@ -12,9 +12,9 @@
 #include "pyhelper.h" // errorf
 #include "trapq.h" // move_get_distance
 
-// Helper code for integrating acceleration
+// Calculate the definitive integral of the move distance
 static double
-integrate_accel(struct move *m, double start, double end)
+move_integrate_distance(struct move *m, 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);
@@ -22,64 +22,45 @@ integrate_accel(struct move *m, double start, double end)
     return ei - si;
 }
 
-// Calculate the definitive integral on part of a move
+// Calculate the definitive integral of extruder with pressure advance
 static double
-move_integrate(struct move *m, int axis, double start, double end)
+pa_move_integrate(struct move *m, double start, double end)
 {
     if (start < 0.)
         start = 0.;
     if (end > m->move_t)
         end = m->move_t;
-    double base = m->start_pos.axis[axis - 'x'] * (end - start);
-    double integral = integrate_accel(m, start, end);
-    return base + integral * m->axes_r.axis[axis - 'x'];
+    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);
 }
 
-// Calculate the definitive integral for a cartesian axis
+// Calculate the definitive integral of the extruder over a range of moves
 static double
-trapq_integrate(struct move *m, int axis, double start, double end)
+pa_range_integrate(struct move *m, double start, double end)
 {
-    double res = move_integrate(m, axis, start, end);
+    double res = pa_move_integrate(m, start, end);
     // Integrate over previous moves
     struct move *prev = m;
     while (unlikely(start < 0.)) {
         prev = list_prev_entry(prev, node);
         start += prev->move_t;
-        res += move_integrate(prev, axis, start, prev->move_t);
+        res += pa_move_integrate(prev, start, prev->move_t);
     }
     // Integrate over future moves
     while (unlikely(end > m->move_t)) {
         end -= m->move_t;
         m = list_next_entry(m, node);
-        res += move_integrate(m, axis, 0., end);
+        res += pa_move_integrate(m, 0., end);
     }
     return res;
 }
 
-// Find a move associated with a given time
-static struct move *
-trapq_find_move(struct move *m, double *ptime)
-{
-    double move_time = *ptime;
-    for (;;) {
-        if (unlikely(move_time < 0.)) {
-            // Check previous move in list
-            m = list_prev_entry(m, node);
-            move_time += m->move_t;
-        } else if (unlikely(move_time > m->move_t)) {
-            // Check next move in list
-            move_time -= m->move_t;
-            m = list_next_entry(m, node);
-        } else {
-            *ptime = move_time;
-            return m;
-        }
-    }
-}
-
 struct extruder_stepper {
     struct stepper_kinematics sk;
-    double pressure_advance_factor, half_smooth_time, inv_smooth_time;
+    double half_smooth_time, inv_smooth_time;
 };
 
 static double
@@ -91,36 +72,20 @@ extruder_calc_position(struct stepper_kinematics *sk, struct move *m
     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;
+    // 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;
 }
 
 void __visible
-extruder_set_pressure(struct stepper_kinematics *sk
-                      , double pressure_advance, double half_smooth_time)
+extruder_set_smooth_time(struct stepper_kinematics *sk, double smooth_time)
 {
     struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk);
-    if (! half_smooth_time) {
-        es->pressure_advance_factor = es->half_smooth_time = 0.;
+    double hst = smooth_time * .5;
+    es->sk.scan_past = es->sk.scan_future = es->half_smooth_time = hst;
+    if (! hst)
         return;
-    }
-    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;
+    es->inv_smooth_time = 1. / smooth_time;
 }
 
 struct stepper_kinematics * __visible
diff --git a/klippy/kinematics/extruder.py b/klippy/kinematics/extruder.py
index e5883df2..b55e6f0e 100644
--- a/klippy/kinematics/extruder.py
+++ b/klippy/kinematics/extruder.py
@@ -50,7 +50,7 @@ class PrinterExtruder:
         pressure_advance = config.getfloat('pressure_advance', 0., minval=0.)
         smooth_time = config.getfloat('pressure_advance_smooth_time',
                                       0.040, above=0., maxval=.200)
-        self.extrude_pos = self.extrude_pa_pos = 0.
+        self.extrude_pos = 0.
         # Setup iterative solver
         ffi_main, ffi_lib = chelper.get_ffi()
         self.trapq = ffi_main.gc(ffi_lib.trapq_alloc(), ffi_lib.trapq_free)
@@ -61,7 +61,7 @@ class PrinterExtruder:
         self.stepper.set_stepper_kinematics(self.sk_extruder)
         self.stepper.set_trapq(self.trapq)
         toolhead.register_step_generator(self.stepper.generate_steps)
-        self.extruder_set_pressure = ffi_lib.extruder_set_pressure
+        self.extruder_set_smooth_time = ffi_lib.extruder_set_smooth_time
         self._set_pressure_advance(pressure_advance, smooth_time)
         # Register commands
         gcode = self.printer.lookup_object('gcode')
@@ -76,17 +76,16 @@ class PrinterExtruder:
     def update_move_time(self, flush_time):
         self.trapq_free_moves(self.trapq, flush_time)
     def _set_pressure_advance(self, pressure_advance, smooth_time):
-        old_smooth_time = self.pressure_advance_smooth_time * .5
+        old_smooth_time = self.pressure_advance_smooth_time
         if not self.pressure_advance:
             old_smooth_time = 0.
-        new_smooth_time = smooth_time * .5
+        new_smooth_time = smooth_time
         if not pressure_advance:
             new_smooth_time = 0.
         toolhead = self.printer.lookup_object("toolhead")
-        toolhead.note_step_generation_scan_time(new_smooth_time,
-                                                old_delay=old_smooth_time)
-        self.extruder_set_pressure(self.sk_extruder,
-                                   pressure_advance, new_smooth_time)
+        toolhead.note_step_generation_scan_time(new_smooth_time * .5,
+                                                old_delay=old_smooth_time * .5)
+        self.extruder_set_smooth_time(self.sk_extruder, new_smooth_time)
         self.pressure_advance = pressure_advance
         self.pressure_advance_smooth_time = smooth_time
     def get_status(self, eventtime):
@@ -142,18 +141,16 @@ class PrinterExtruder:
         accel = move.accel * axis_r
         start_v = move.start_v * axis_r
         cruise_v = move.cruise_v * axis_r
-        is_pa = 0.
+        pressure_advance = 0.
         if axis_r > 0. and (move.axes_d[0] or move.axes_d[1]):
-            is_pa = 1.
-        # Queue movement (x is extruder movement, y is movement with pa)
+            pressure_advance = self.pressure_advance
+        # Queue movement (x is extruder movement, y is pressure advance)
         self.trapq_append(self.trapq, print_time,
                           move.accel_t, move.cruise_t, move.decel_t,
-                          move.start_pos[3], self.extrude_pa_pos, 0.,
-                          1., is_pa, 0.,
+                          move.start_pos[3], 0., 0.,
+                          1., pressure_advance, 0.,
                           start_v, cruise_v, accel)
         self.extrude_pos = move.end_pos[3]
-        if is_pa:
-            self.extrude_pa_pos += move.axes_d[3]
     cmd_SET_PRESSURE_ADVANCE_help = "Set pressure advance parameters"
     def cmd_default_SET_PRESSURE_ADVANCE(self, params):
         extruder = self.printer.lookup_object('toolhead').get_extruder()