extruder: Do extruder lookahead based on time instead of cornering

When calculating the extruder lookahead, determine how far to
lookahead by the amount of elapsed time each move takes.  This makes
the extruder lookahead code more flexible as it is no longer limited
to the next immediate cornering moves.

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

2 files changed, 37 insertions, 40 deletions

diff --git a/klippy/extruder.py b/klippy/extruder.py
index e49d4c91..fc20ccf8 100644
--- a/klippy/extruder.py
+++ b/klippy/extruder.py
@@ -38,6 +38,7 @@ class PrinterExtruder:
         self.need_motor_enable = True
     def check_move(self, move):
         move.extrude_r = move.axes_d[3] / move.move_d
+        move.extrude_max_corner_v = 0.
         if not self.heater.can_extrude:
             raise homing.EndstopMoveError(
                 move.end_pos, "Extrude below minimum temp")
@@ -60,30 +61,34 @@ class PrinterExtruder:
                 return 0.
             move.extrude_r = prev_move.extrude_r
         return move.max_cruise_v2
-    def lookahead(self, move_info, orig_flush_count, lazy):
-        if not self.pressure_advance:
-            return orig_flush_count
-        min_corner_v2 = max_corner_v2 = 0.
-        flush_count = len(move_info)
-        for i in range(flush_count-1, -1, -1):
-            move, start_v2, cruise_v2, end_v2 = move_info[i]
-            reachable_start_v2 = end_v2 + move.delta_v2
-            # Calculate min/max_corner_v2 - the speed the head will
-            # slow to due to junction cornering and the maximum speed
-            # the head will reach immediately afterwards.
-            move.extruder_min_corner_v2 = min_corner_v2
-            move.extruder_max_corner_v2 = max_corner_v2
-            if reachable_start_v2 > start_v2:
-                min_corner_v2 = start_v2
-                if (start_v2 + move.delta_v2 > end_v2
-                    or end_v2 >= move_info[i+1][2]):
-                    if lazy and max_corner_v2:
-                        flush_count = i
-                        lazy = False
-                    max_corner_v2 = cruise_v2
-        if lazy:
-            return 0
-        return min(flush_count, orig_flush_count)
+    def lookahead(self, moves, flush_count, lazy):
+        pressure_advance = self.pressure_advance
+        if not pressure_advance:
+            return flush_count
+        # Calculate max_corner_v - the speed the head will accelerate
+        # to after cornering.
+        for i in range(flush_count):
+            move = moves[i]
+            if not move.decel_t:
+                continue
+            cruise_v = move.cruise_v
+            max_corner_v = 0.
+            sum_t = pressure_advance
+            for j in range(i+1, flush_count):
+                fmove = moves[j]
+                if not fmove.max_start_v2:
+                    break
+                max_corner_v = max(max_corner_v, fmove.cruise_v)
+                if max_corner_v >= cruise_v:
+                    break
+                sum_t -= fmove.accel_t + fmove.cruise_t + fmove.decel_t
+                if sum_t <= 0.:
+                    break
+            else:
+                if lazy:
+                    return i
+            move.extrude_max_corner_v = max_corner_v
+        return flush_count
     def move(self, move_time, move):
         if self.need_motor_enable:
             self.stepper.motor_enable(move_time, 1)
@@ -108,28 +113,20 @@ class PrinterExtruder:
         if (axis_d >= 0. and (move.axes_d[0] or move.axes_d[1])
             and self.pressure_advance):
             # Increase accel_d and start_v when accelerating
-            move_extrude_r = move.extrude_r
+            pressure_advance = self.pressure_advance * move.extrude_r
             prev_pressure_d = start_pos - move.start_pos[3]
-            if accel_t:
-                npd = move.cruise_v * move_extrude_r * self.pressure_advance
+            if accel_d:
+                npd = move.cruise_v * pressure_advance
                 extra_accel_d = npd - prev_pressure_d
                 if extra_accel_d > 0.:
                     accel_d += extra_accel_d
                     start_v += extra_accel_d / accel_t
                     prev_pressure_d += extra_accel_d
             # Update decel and retract parameters when decelerating
-            if decel_t:
-                if move.extruder_min_corner_v2:
-                    min_corner_v = math.sqrt(move.extruder_min_corner_v2)
-                    max_corner_v = math.sqrt(move.extruder_max_corner_v2)
-                    npd = max_corner_v*move_extrude_r*self.pressure_advance
-                    extra_decel_d = prev_pressure_d - npd
-                    if move.end_v > min_corner_v:
-                        extra_decel_d *= ((move.cruise_v - move.end_v)
-                                          / (move.cruise_v - min_corner_v))
-                else:
-                    npd = move.end_v * move_extrude_r * self.pressure_advance
-                    extra_decel_d = prev_pressure_d - npd
+            emcv = move.extrude_max_corner_v
+            if decel_d and emcv < move.cruise_v:
+                npd = max(emcv, move.end_v) * pressure_advance
+                extra_decel_d = prev_pressure_d - npd
                 if extra_decel_d > 0.:
                     extra_decel_v = extra_decel_d / decel_t
                     decel_v -= extra_decel_v
diff --git a/klippy/toolhead.py b/klippy/toolhead.py
index 768f7233..a189a848 100644
--- a/klippy/toolhead.py
+++ b/klippy/toolhead.py
@@ -125,7 +125,7 @@ class MoveQueue:
         for move, start_v2, cruise_v2, end_v2 in move_info[:flush_count]:
             move.set_junction(start_v2, cruise_v2, end_v2)
         # Allow extruder to do its lookahead
-        flush_count = self.extruder_lookahead(move_info, flush_count, lazy)
+        flush_count = self.extruder_lookahead(queue, flush_count, lazy)
         # Generate step times for all moves ready to be flushed
         for move in queue[:flush_count]:
             move.move()