itersolve: Implement a step+dir+step filter

Some stepper motor drivers do not respond well to rapid "step +
direction change + step" events.  In particular, it is believed this
can cause "over current" events on the tmc2208 drivers when they are
in "stealthchop" mode.  Detect these events and remove them from the
generated step times.

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

klippy/chelper/itersolve.c

@@ -13,6 +13,65 @@
 #include "stepcompress.h" // queue_append_start
 #include "trapq.h" // struct move
 
+
+/****************************************************************
+ * Filter rapid "step + direction change + step" sequences
+ ****************************************************************/
+
+#define SDS_CHECK_TIME .001
+#define SDS_FILTER_TIME .000750
+
+static int
+sds_commit(struct stepper_kinematics *sk)
+{
+    double mtime = sk->next_move_print_time, stime = sk->next_step_time;
+    sk->next_move_print_time = 0.;
+    return stepcompress_append(sk->sc, sk->next_step_dir, mtime, stime);
+}
+
+static int
+sds_append(struct stepper_kinematics *sk, int sdir
+           , double move_print_time, double step_time)
+{
+    if (sk->next_move_print_time) {
+        if (sdir != sk->next_step_dir) {
+            double mt_diff = move_print_time - sk->next_move_print_time;
+            double st_diff = step_time - sk->next_step_time;
+            if (mt_diff + st_diff < SDS_FILTER_TIME) {
+                // Rollback last step
+                sk->next_move_print_time = 0.;
+                sk->next_step_dir = sdir;
+                return 0;
+            }
+        }
+        int ret = sds_commit(sk);
+        if (ret)
+            return ret;
+    }
+    sk->next_move_print_time = move_print_time;
+    sk->next_step_time = step_time;
+    sk->next_step_dir = sdir;
+    return 0;
+}
+
+static int
+sds_flush(struct stepper_kinematics *sk
+          , double move_print_time, double step_time)
+{
+    if (sk->next_move_print_time) {
+        double mt_diff = move_print_time - sk->next_move_print_time;
+        double st_diff = step_time - sk->next_step_time;
+        if (mt_diff + st_diff >= SDS_FILTER_TIME)
+            return sds_commit(sk);
+    }
+    return 0;
+}
+
+
+/****************************************************************
+ * Main iterative solver
+ ****************************************************************/
+
 struct timepos {
     double time, position;
 };
@@ -66,7 +125,7 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m
     double start = move_start - m->print_time, end = move_end - m->print_time;
     struct timepos last = { start, sk->commanded_pos }, low = last, high = last;
     double seek_time_delta = SEEK_TIME_RESET;
-    int sdir = !!stepcompress_get_step_dir(sk->sc), is_dir_change = 0;
+    int sdir = sk->next_step_dir, is_dir_change = 0;
     for (;;) {
         double diff = high.position - last.position, dist = sdir ? diff : -diff;
         if (dist >= half_step) {
@@ -74,8 +133,7 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m
             double target = last.position + (sdir ? half_step : -half_step);
             struct timepos next = itersolve_find_step(sk, m, low, high, target);
             // Add step at given time
-            int ret = stepcompress_append(sk->sc, sdir
-                                          , m->print_time, next.time);
+            int ret = sds_append(sk, sdir, m->print_time, next.time);
             if (ret)
                 return ret;
             seek_time_delta = next.time - last.time;
@@ -90,6 +148,10 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m
             if (low.time < high.time)
                 // The existing search range is still valid
                 continue;
+        } else if (dist > 0.) {
+            // Avoid rollback if stepper fully reaches target position
+            if (sk->next_move_print_time)
+                sds_commit(sk);
         } else if (unlikely(dist < -(half_step + .000000001))) {
             // Found direction change
             is_dir_change = 1;
@@ -121,12 +183,20 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m
             high.time = end;
         high.position = calc_position_cb(sk, m, high.time);
     }
+    int ret = sds_flush(sk, m->print_time, end);
+    if (ret)
+        return ret;
     sk->commanded_pos = last.position;
     if (sk->post_cb)
         sk->post_cb(sk);
     return 0;
 }
 
+
+/****************************************************************
+ * Interface functions
+ ****************************************************************/
+
 // Check if a move is likely to cause movement on a stepper
 static inline int
 check_active(struct stepper_kinematics *sk, struct move *m)
@@ -149,7 +219,10 @@ itersolve_generate_steps(struct stepper_kinematics *sk, double flush_time)
     struct move *m = list_first_entry(&sk->tq->moves, struct move, node);
     while (last_flush_time >= m->print_time + m->move_t)
         m = list_next_entry(m, node);
-    double force_steps_time = sk->last_move_time + sk->gen_steps_post_active;
+    double gen_steps_post_active = sk->gen_steps_post_active;
+    if (gen_steps_post_active < SDS_CHECK_TIME)
+        gen_steps_post_active = SDS_CHECK_TIME;
+    double force_steps_time = sk->last_move_time + gen_steps_post_active;
     for (;;) {
         if (last_flush_time >= flush_time)
             return 0;
@@ -174,7 +247,7 @@ itersolve_generate_steps(struct stepper_kinematics *sk, double flush_time)
             if (ret)
                 return ret;
             sk->last_move_time = last_flush_time = end;
-            force_steps_time = end + sk->gen_steps_post_active;
+            force_steps_time = end + gen_steps_post_active;
         } else if (start < force_steps_time) {
             // Must generates steps just past stepper activity
             if (end > force_steps_time)

klippy/chelper/itersolve.h

@@ -16,6 +16,9 @@ struct stepper_kinematics {
     double step_dist, commanded_pos;
     struct stepcompress *sc;
 
+    double next_move_print_time, next_step_time;
+    int next_step_dir;
+
     double last_flush_time, last_move_time;
     struct trapq *tq;
     int active_flags;

klippy/toolhead.py

@@ -1,6 +1,6 @@
 # Code for coordinating events on the printer toolhead
 #
-# Copyright (C) 2016-2019  Kevin O'Connor <kevin@koconnor.net>
+# Copyright (C) 2016-2020  Kevin O'Connor <kevin@koconnor.net>
 #
 # This file may be distributed under the terms of the GNU GPLv3 license.
 import math, logging, importlib
@@ -183,6 +183,7 @@ class MoveQueue:
 
 MIN_KIN_TIME = 0.100
 MOVE_BATCH_TIME = 0.500
+SDS_CHECK_TIME = 0.001 # step+dir+step filter in itersolve.c
 
 DRIP_SEGMENT_TIME = 0.050
 DRIP_TIME = 0.100
@@ -236,7 +237,7 @@ class ToolHead:
         self.print_stall = 0
         self.drip_completion = None
         # Kinematic step generation scan window time tracking
-        self.kin_flush_delay = 0.
+        self.kin_flush_delay = SDS_CHECK_TIME
         self.kin_flush_times = []
         self.last_kin_flush_time = self.last_kin_move_time = 0.
         # Setup iterative solver
@@ -512,7 +513,7 @@ class ToolHead:
             self.kin_flush_times.pop(self.kin_flush_times.index(old_delay))
         if delay:
             self.kin_flush_times.append(delay)
-        new_delay = max(self.kin_flush_times + [0.])
+        new_delay = max(self.kin_flush_times + [SDS_CHECK_TIME])
         self.kin_flush_delay = new_delay
     def register_lookahead_callback(self, callback):
         last_move = self.move_queue.get_last()