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>

1 files changed, 36 insertions, 34 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