stepcompress: Do all step rounding in C code

Commits f0cefebf and 8f331f08 changed the way the code determined what
steps to take on fractional steps.  Unfortunately, it was possible in
some situations for the C code to round differently from the python
code which could result in warnings and lost steps.

Change the code so that all fractional step handling is done in the C
code.  Implementing the step rounding logic in one location avoids any
conflicts.

In order to efficiently handle the step rounding in the C code, the C
code has also been extended to directly send the "set_next_step_dir"
command.

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

6 files changed, 154 insertions, 131 deletions

diff --git a/klippy/cartesian.py b/klippy/cartesian.py
index 0a5f7961..85746860 100644
--- a/klippy/cartesian.py
+++ b/klippy/cartesian.py
@@ -97,45 +97,47 @@ class CartKinematics:
         inv_accel = 1. / move.accel
         inv_cruise_v = 1. / move.cruise_v
         for i in StepList:
-            inv_step_dist = self.steppers[i].inv_step_dist
-            new_step_pos = int(move.end_pos[i]*inv_step_dist + 0.5)
-            step_pos = self.stepper_pos[i]
-            if new_step_pos == step_pos:
+            if not move.axes_d[i]:
                 continue
-            self.stepper_pos[i] = new_step_pos
+            mcu_time, so = self.steppers[i].prep_move(move_time)
+            inv_step_dist = self.steppers[i].inv_step_dist
             steps = move.axes_d[i] * inv_step_dist
-            step_offset = step_pos - move.start_pos[i] * inv_step_dist + 0.5
-            sdir = 1
-            if steps < 0:
-                sdir = 0
-                steps = -steps
-                step_offset = 1. - step_offset
-            mcu_time, so = self.steppers[i].prep_move(move_time, sdir)
+            move_step_d = move.move_d / abs(steps)
 
-            move_step_d = move.move_d / steps
+            step_pos = self.stepper_pos[i]
+            step_offset = step_pos - move.start_pos[i] * inv_step_dist
 
             # Acceleration steps
-            #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
-            accel_time_offset = move.start_v * inv_accel
-            accel_sqrt_offset = accel_time_offset**2
             accel_multiplier = 2.0 * move_step_d * inv_accel
-            accel_steps = move.accel_r * steps
-            step_offset = so.step_sqrt(
-                mcu_time - accel_time_offset, accel_steps, step_offset
-                , accel_sqrt_offset, accel_multiplier)
-            mcu_time += move.accel_t
+            if move.accel_r:
+                #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
+                accel_time_offset = move.start_v * inv_accel
+                accel_sqrt_offset = accel_time_offset**2
+                accel_steps = move.accel_r * steps
+                count = so.step_sqrt(
+                    mcu_time - accel_time_offset, accel_steps, step_offset
+                    , accel_sqrt_offset, accel_multiplier)
+                step_pos += count
+                step_offset += count - accel_steps
+                mcu_time += move.accel_t
             # Cruising steps
-            #t = pos/cruise_v
-            cruise_multiplier = move_step_d * inv_cruise_v
-            cruise_steps = move.cruise_r * steps
-            step_offset = so.step_factor(
-                mcu_time, cruise_steps, step_offset, cruise_multiplier)
-            mcu_time += move.cruise_t
+            if move.cruise_r:
+                #t = pos/cruise_v
+                cruise_multiplier = move_step_d * inv_cruise_v
+                cruise_steps = move.cruise_r * steps
+                count = so.step_factor(
+                    mcu_time, cruise_steps, step_offset, cruise_multiplier)
+                step_pos += count
+                step_offset += count - cruise_steps
+                mcu_time += move.cruise_t
             # Deceleration steps
-            #t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel)
-            decel_time_offset = move.cruise_v * inv_accel
-            decel_sqrt_offset = decel_time_offset**2
-            decel_steps = move.decel_r * steps
-            so.step_sqrt(
-                mcu_time + decel_time_offset, decel_steps, step_offset
-                , decel_sqrt_offset, -accel_multiplier)
+            if move.decel_r:
+                #t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel)
+                decel_time_offset = move.cruise_v * inv_accel
+                decel_sqrt_offset = decel_time_offset**2
+                decel_steps = move.decel_r * steps
+                count = so.step_sqrt(
+                    mcu_time + decel_time_offset, decel_steps, step_offset
+                    , decel_sqrt_offset, -accel_multiplier)
+                step_pos += count
+            self.stepper_pos[i] = step_pos
diff --git a/klippy/chelper.py b/klippy/chelper.py
index 5b7837ee..a3ce4a95 100644
--- a/klippy/chelper.py
+++ b/klippy/chelper.py
@@ -13,12 +13,14 @@ OTHER_FILES = ['list.h', 'serialqueue.h']
 
 defs_stepcompress = """
     struct stepcompress *stepcompress_alloc(uint32_t max_error
-        , uint32_t queue_step_msgid, uint32_t oid);
-    void stepcompress_push(struct stepcompress *sc, double step_clock);
-    double stepcompress_push_factor(struct stepcompress *sc
+        , uint32_t queue_step_msgid, uint32_t set_next_step_dir_msgid
+        , uint32_t invert_sdir, uint32_t oid);
+    void stepcompress_push(struct stepcompress *sc, double step_clock
+        , int32_t sdir);
+    int32_t stepcompress_push_factor(struct stepcompress *sc
         , double steps, double step_offset
         , double clock_offset, double factor);
-    double stepcompress_push_sqrt(struct stepcompress *sc
+    int32_t stepcompress_push_sqrt(struct stepcompress *sc
         , double steps, double step_offset
         , double clock_offset, double sqrt_offset, double factor);
     void stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock);
diff --git a/klippy/extruder.py b/klippy/extruder.py
index e7f1b40c..fb726fca 100644
--- a/klippy/extruder.py
+++ b/klippy/extruder.py
@@ -88,69 +88,58 @@ class PrinterExtruder:
                         # There is still only a decel phase (no retraction)
                         decel_d -= extra_decel_d
 
-        # Determine regular steps
-        forward_d = accel_d + cruise_d + decel_d
-        end_pos = start_pos + forward_d
+        # Prepare for steps
+        stepper_pos = self.stepper_pos
         inv_step_dist = self.stepper.inv_step_dist
-        new_step_pos = int(end_pos*inv_step_dist + 0.5)
-        if new_step_pos != self.stepper_pos:
-            steps = forward_d * inv_step_dist
-            step_offset = self.stepper_pos - start_pos * inv_step_dist + 0.5
-            self.stepper_pos = new_step_pos
-            sdir = 1
-            if steps < 0:
-                sdir = 0
-                steps = -steps
-                step_offset = 1. - step_offset
-            mcu_time, so = self.stepper.prep_move(move_time, sdir)
+        step_dist = self.stepper.step_dist
+        mcu_time, so = self.stepper.prep_move(move_time)
+        step_offset = stepper_pos - start_pos * inv_step_dist
 
-            move_step_d = forward_d / steps
-            inv_move_step_d = 1. / move_step_d
-
-            # Acceleration steps
+        # Acceleration steps
+        accel_multiplier = 2.0 * step_dist * inv_accel
+        if accel_d:
             #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
             accel_time_offset = start_v * inv_accel
             accel_sqrt_offset = accel_time_offset**2
-            accel_multiplier = 2.0 * move_step_d * inv_accel
-            accel_steps = accel_d * inv_move_step_d
-            step_offset = so.step_sqrt(
+            accel_steps = accel_d * inv_step_dist
+            count = so.step_sqrt(
                 mcu_time - accel_time_offset, accel_steps, step_offset
                 , accel_sqrt_offset, accel_multiplier)
+            stepper_pos += count
+            step_offset += count - accel_steps
             mcu_time += accel_t
-            # Cruising steps
+        # Cruising steps
+        if cruise_d:
             #t = pos/cruise_v
-            cruise_multiplier = move_step_d / cruise_v
-            cruise_steps = cruise_d * inv_move_step_d
-            step_offset = so.step_factor(
+            cruise_multiplier = step_dist / cruise_v
+            cruise_steps = cruise_d * inv_step_dist
+            count = so.step_factor(
                 mcu_time, cruise_steps, step_offset, cruise_multiplier)
+            stepper_pos += count
+            step_offset += count - cruise_steps
             mcu_time += cruise_t
-            # Deceleration steps
+        # Deceleration steps
+        if decel_d:
             #t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel)
             decel_time_offset = decel_v * inv_accel
             decel_sqrt_offset = decel_time_offset**2
-            decel_steps = decel_d * inv_move_step_d
-            so.step_sqrt(
+            decel_steps = decel_d * inv_step_dist
+            count = so.step_sqrt(
                 mcu_time + decel_time_offset, decel_steps, step_offset
                 , decel_sqrt_offset, -accel_multiplier)
-
-        # Determine retract steps
-        start_pos = end_pos
-        end_pos -= retract_d
-        new_step_pos = int(end_pos*inv_step_dist + 0.5)
-        if new_step_pos != self.stepper_pos:
-            steps = retract_d * inv_step_dist
-            step_offset = start_pos * inv_step_dist - self.stepper_pos + 0.5
-            self.stepper_pos = new_step_pos
-            mcu_time, so = self.stepper.prep_move(
-                move_time+accel_t+cruise_t+decel_t, 0)
-
-            move_step_d = retract_d / steps
-
-            # Acceleration steps
+            stepper_pos += count
+            step_offset += count - decel_steps
+            mcu_time += decel_t
+        # Retraction steps
+        if retract_d:
             #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
             accel_time_offset = retract_v * inv_accel
             accel_sqrt_offset = accel_time_offset**2
-            accel_multiplier = 2.0 * move_step_d * inv_accel
-            so.step_sqrt(mcu_time - accel_time_offset, steps, step_offset
-                         , accel_sqrt_offset, accel_multiplier)
-        self.extrude_pos = end_pos
+            accel_steps = -retract_d * inv_step_dist
+            count = so.step_sqrt(
+                mcu_time - accel_time_offset, accel_steps, step_offset
+                , accel_sqrt_offset, accel_multiplier)
+            stepper_pos += count
+
+        self.stepper_pos = stepper_pos
+        self.extrude_pos = start_pos + accel_d + cruise_d + decel_d - retract_d
diff --git a/klippy/mcu.py b/klippy/mcu.py
index 7e172f63..79537ed6 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -22,7 +22,7 @@ class MCU_stepper:
         self._oid = mcu.create_oid()
         step_pin, pullup, invert_step = parse_pin_extras(step_pin)
         dir_pin, pullup, self._invert_dir = parse_pin_extras(dir_pin)
-        self._sdir = -1
+        self._need_reset = True
         self._mcu_freq = mcu.get_mcu_freq()
         min_stop_interval = int(min_stop_interval * self._mcu_freq)
         max_error = int(max_error * self._mcu_freq)
@@ -39,30 +39,26 @@ class MCU_stepper:
             "reset_step_clock oid=%c clock=%u")
         ffi_main, self.ffi_lib = chelper.get_ffi()
         self._stepqueue = self.ffi_lib.stepcompress_alloc(
-            max_error, self._step_cmd.msgid, self._oid)
+            max_error, self._step_cmd.msgid
+            , self._dir_cmd.msgid, self._invert_dir, self._oid)
         self.print_to_mcu_time = mcu.print_to_mcu_time
     def get_oid(self):
         return self._oid
     def get_invert_dir(self):
         return self._invert_dir
     def note_stepper_stop(self):
-        self._sdir = -1
-    def _reset_step_clock(self, clock):
+        self._need_reset = True
+    def check_reset(self, mcu_time):
+        if not self._need_reset:
+            return
+        self._need_reset = False
+        clock = int(mcu_time * self._mcu_freq)
         self.ffi_lib.stepcompress_reset(self._stepqueue, clock)
         data = (self._reset_cmd.msgid, self._oid, clock & 0xffffffff)
         self.ffi_lib.stepcompress_queue_msg(self._stepqueue, data, len(data))
-    def set_next_step_dir(self, mcu_time, sdir):
-        if self._sdir == sdir:
-            return
-        if self._sdir == -1:
-            clock = int(mcu_time * self._mcu_freq)
-            self._reset_step_clock(clock)
-        self._sdir = sdir
-        data = (self._dir_cmd.msgid, self._oid, sdir ^ self._invert_dir)
-        self.ffi_lib.stepcompress_queue_msg(self._stepqueue, data, len(data))
-    def step(self, mcu_time):
+    def step(self, mcu_time, sdir):
         clock = mcu_time * self._mcu_freq
-        self.ffi_lib.stepcompress_push(self._stepqueue, clock)
+        self.ffi_lib.stepcompress_push(self._stepqueue, clock, sdir)
     def step_sqrt(self, mcu_time, steps, step_offset, sqrt_offset, factor):
         clock = mcu_time * self._mcu_freq
         mcu_freq2 = self._mcu_freq**2
@@ -477,7 +473,7 @@ class Dummy_MCU_stepper:
             self._stepid, dirstr, countstr, addstr, interval))
     def set_next_step_dir(self, dir):
         self._sdir = dir
-    def _reset_step_clock(self, clock):
+    def check_reset(self, clock):
         self._mcu.outfile.write("G6S%dT%d\n" % (self._stepid, clock))
     def print_to_mcu_time(self, print_time):
         return self._mcu.print_to_mcu_time(print_time)
diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c
index ee8f32bc..c3f06761 100644
--- a/klippy/stepcompress.c
+++ b/klippy/stepcompress.c
@@ -35,7 +35,8 @@ struct stepcompress {
     // Message generation
     uint64_t last_step_clock;
     struct list_head msg_queue;
-    uint32_t queue_step_msgid, oid;
+    uint32_t queue_step_msgid, set_next_step_dir_msgid, oid;
+    int sdir, invert_sdir;
 };
 
 
@@ -268,14 +269,19 @@ safe_sqrt(double v)
 
 // Allocate a new 'stepcompress' object
 struct stepcompress *
-stepcompress_alloc(uint32_t max_error, uint32_t queue_step_msgid, uint32_t oid)
+stepcompress_alloc(uint32_t max_error, uint32_t queue_step_msgid
+                   , uint32_t set_next_step_dir_msgid, uint32_t invert_sdir
+                   , uint32_t oid)
 {
     struct stepcompress *sc = malloc(sizeof(*sc));
     memset(sc, 0, sizeof(*sc));
     sc->max_error = max_error;
     list_init(&sc->msg_queue);
     sc->queue_step_msgid = queue_step_msgid;
+    sc->set_next_step_dir_msgid = set_next_step_dir_msgid;
     sc->oid = oid;
+    sc->sdir = -1;
+    sc->invert_sdir = !!invert_sdir;
     return sc;
 }
 
@@ -312,75 +318,102 @@ stepcompress_flush(struct stepcompress *sc, uint64_t move_clock)
     }
 }
 
+// Send the set_next_step_dir command
+static void
+set_next_step_dir(struct stepcompress *sc, int sdir)
+{
+    sc->sdir = sdir;
+    stepcompress_flush(sc, UINT64_MAX);
+    uint32_t msg[3] = {
+        sc->set_next_step_dir_msgid, sc->oid, sdir ^ sc->invert_sdir
+    };
+    struct queue_message *qm = message_alloc_and_encode(msg, 3);
+    qm->req_clock = sc->last_step_clock;
+    list_add_tail(&qm->node, &sc->msg_queue);
+}
+
 // Check if the internal queue needs to be expanded, and expand if so
 static inline void
-check_expand(struct stepcompress *sc, int count)
+check_expand(struct stepcompress *sc, int sdir, int count)
 {
+    if (sdir != sc->sdir)
+        set_next_step_dir(sc, sdir);
     if (sc->queue_next + count > sc->queue_end)
         expand_queue(sc, count);
 }
 
 // Schedule a step event at the specified step_clock time
 void
-stepcompress_push(struct stepcompress *sc, double step_clock)
+stepcompress_push(struct stepcompress *sc, double step_clock, int32_t sdir)
 {
-    check_expand(sc, 1);
+    sdir = !!sdir;
+    check_expand(sc, sdir, 1);
     step_clock += 0.5;
     *sc->queue_next++ = step_clock;
 }
 
 // Schedule 'steps' number of steps with a constant time between steps
 // using the formula: step_clock = clock_offset + step_num*factor
-double
+int32_t
 stepcompress_push_factor(struct stepcompress *sc
                          , double steps, double step_offset
                          , double clock_offset, double factor)
 {
     // Calculate number of steps to take
-    double ceil_steps = ceil(steps - step_offset);
-    double next_step_offset = ceil_steps - (steps - step_offset);
-    int count = ceil_steps;
-    if (count < 0 || count > 1000000) {
-        fprintf(stderr, "ERROR: push_factor invalid count %d %f %f %f %f\n"
-                , sc->oid, steps, step_offset, clock_offset, factor);
-        return next_step_offset;
+    int sdir = 1;
+    if (steps < 0) {
+        sdir = 0;
+        steps = -steps;
+        step_offset = -step_offset;
     }
-    check_expand(sc, count);
+    int count = steps + .5 - step_offset;
+    if (count <= 0 || count > 1000000) {
+        if (count && steps)
+            fprintf(stderr, "ERROR: push_factor invalid count %d %f %f %f %f\n"
+                    , sc->oid, steps, step_offset, clock_offset, factor);
+        return 0;
+    }
+    check_expand(sc, sdir, count);
 
     // Calculate each step time
     uint64_t *qn = sc->queue_next, *end = &qn[count];
     clock_offset += 0.5;
-    double pos = step_offset;
+    double pos = step_offset + .5;
     while (qn < end) {
         *qn++ = clock_offset + pos*factor;
         pos += 1.0;
     }
     sc->queue_next = qn;
-    return next_step_offset;
+    return sdir ? count : -count;
 }
 
 // Schedule 'steps' number of steps using the formula:
 //  step_clock = clock_offset + sqrt(step_num*factor + sqrt_offset)
-double
+int32_t
 stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset
                        , double clock_offset, double sqrt_offset, double factor)
 {
     // Calculate number of steps to take
-    double ceil_steps = ceil(steps - step_offset);
-    double next_step_offset = ceil_steps - (steps - step_offset);
-    int count = ceil_steps;
-    if (count < 0 || count > 1000000) {
-        fprintf(stderr, "ERROR: push_sqrt invalid count %d %f %f %f %f %f\n"
-                , sc->oid, steps, step_offset, clock_offset, sqrt_offset
-                , factor);
-        return next_step_offset;
+    int sdir = 1;
+    if (steps < 0) {
+        sdir = 0;
+        steps = -steps;
+        step_offset = -step_offset;
+    }
+    int count = steps + .5 - step_offset;
+    if (count <= 0 || count > 1000000) {
+        if (count && steps)
+            fprintf(stderr, "ERROR: push_sqrt invalid count %d %f %f %f %f %f\n"
+                    , sc->oid, steps, step_offset, clock_offset, sqrt_offset
+                    , factor);
+        return 0;
     }
-    check_expand(sc, count);
+    check_expand(sc, sdir, count);
 
     // Calculate each step time
     uint64_t *qn = sc->queue_next, *end = &qn[count];
     clock_offset += 0.5;
-    double pos = step_offset + sqrt_offset/factor;
+    double pos = step_offset + .5 + sqrt_offset/factor;
     if (factor >= 0.0)
         while (qn < end) {
             *qn++ = clock_offset + safe_sqrt(pos*factor);
@@ -392,7 +425,7 @@ stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset
             pos += 1.0;
         }
     sc->queue_next = qn;
-    return next_step_offset;
+    return sdir ? count : -count;
 }
 
 // Reset the internal state of the stepcompress object
@@ -401,6 +434,7 @@ stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock)
 {
     stepcompress_flush(sc, UINT64_MAX);
     sc->last_step_clock = last_step_clock;
+    sc->sdir = -1;
 }
 
 // Queue an mcu command to go out in order with stepper commands
diff --git a/klippy/stepper.py b/klippy/stepper.py
index 8881c99f..57a5e9ae 100644
--- a/klippy/stepper.py
+++ b/klippy/stepper.py
@@ -69,9 +69,9 @@ class PrinterStepper:
             mcu_time = self.mcu_enable.print_to_mcu_time(move_time)
             self.mcu_enable.set_digital(mcu_time, enable)
         self.need_motor_enable = True
-    def prep_move(self, move_time, sdir):
+    def prep_move(self, move_time):
         mcu_time = self.mcu_stepper.print_to_mcu_time(move_time)
-        self.mcu_stepper.set_next_step_dir(mcu_time, sdir)
+        self.mcu_stepper.check_reset(mcu_time)
         if self.need_motor_enable:
             self.motor_enable(move_time, 1)
             self.need_motor_enable = False