itersolve: Enhance "false position" method with "illinois algorithm"

This prevents some cases where the iterative solver fails to converge
in a reasonable time, causing "no next step" errors.

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

1 files changed, 8 insertions, 1 deletions

diff --git a/klippy/chelper/itersolve.c b/klippy/chelper/itersolve.c
index 3aadd5af..ae351f7d 100644
--- a/klippy/chelper/itersolve.c
+++ b/klippy/chelper/itersolve.c
@@ -22,7 +22,7 @@ struct timepos {
     double time, position;
 };
 
-// Find step using "false position" method
+// Find step using "false position" method (with "Illinois algorithm")
 static struct timepos
 itersolve_find_step(struct stepper_kinematics *sk, struct move *m
                     , struct timepos low, struct timepos high
@@ -39,6 +39,7 @@ itersolve_find_step(struct stepper_kinematics *sk, struct move *m
     if (high_sign == signbit(low.position))
         // The target is not in the low/high range - return low range
         return (struct timepos){ low.time, target };
+    int prev_choice = 0;
     for (;;) {
         double guess_time = ((low.time*high.position - high.time*low.position)
                              / (high.position - low.position));
@@ -51,9 +52,15 @@ itersolve_find_step(struct stepper_kinematics *sk, struct move *m
         if (guess_sign == high_sign) {
             high.time = guess_time;
             high.position = guess_position;
+            if (prev_choice > 0)
+                low.position *= .5;
+            prev_choice = 1;
         } else {
             low.time = guess_time;
             low.position = guess_position;
+            if (prev_choice < 0)
+                high.position *= .5;
+            prev_choice = -1;
         }
     }
     return best_guess;