stepcompress: Pass constant velocity and acceleration directly to C code

Update the C code to take velocity and acceleration directly in step
distances and clock ticks.  This simplifies the mcu.py python code as
it only needs to do unit and axis conversion.

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

3 files changed, 32 insertions, 38 deletions

diff --git a/klippy/chelper.py b/klippy/chelper.py
index b35089a1..bfc74127 100644
--- a/klippy/chelper.py
+++ b/klippy/chelper.py
@@ -22,12 +22,10 @@ defs_stepcompress = """
 
     int 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);
-    int32_t stepcompress_push_sqrt(struct stepcompress *sc
-        , double steps, double step_offset
-        , double clock_offset, double sqrt_offset, double factor);
+    int32_t stepcompress_push_const(struct stepcompress *sc, double clock_offset
+        , double step_offset, double steps, double cruise_sv);
+    int32_t stepcompress_push_accel(struct stepcompress *sc, double clock_offset
+        , double step_offset, double steps, double start_sv, double accel);
     int32_t stepcompress_push_delta_const(struct stepcompress *sc
         , double clock_offset, double dist, double start_pos
         , double inv_velocity, double step_dist, double height
diff --git a/klippy/mcu.py b/klippy/mcu.py
index 94e94d63..9127da6c 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -29,7 +29,7 @@ class MCU_stepper:
         self._dir_pin, pullup, self._invert_dir = parse_pin_extras(dir_pin)
         self._commanded_pos = 0
         self._step_dist = self._inv_step_dist = 1.
-        self._velocity_factor = self._inv_accel_factor = 0.
+        self._velocity_factor = self._accel_factor = 0.
         self._mcu_position_offset = 0
         self._mcu_freq = self._min_stop_interval = 0.
         self._reset_cmd = self._get_position_cmd = None
@@ -43,7 +43,7 @@ class MCU_stepper:
     def build_config(self):
         self._mcu_freq = self._mcu.get_mcu_freq()
         self._velocity_factor = 1. / (self._mcu_freq * self._step_dist)
-        self._inv_accel_factor = self._mcu_freq**2 * self._step_dist
+        self._accel_factor = 1. / (self._mcu_freq**2 * self._step_dist)
         max_error = self._mcu.get_max_stepper_error()
         min_stop_interval = max(0., self._min_stop_interval - max_error)
         self._mcu.add_config_cmd(
@@ -119,29 +119,21 @@ class MCU_stepper:
         else:
             self._commanded_pos -= 1
     def step_const(self, mcu_time, start_pos, dist, cruise_v):
-        #t = pos/cruise_v
         inv_step_dist = self._inv_step_dist
         step_offset = self._commanded_pos - start_pos * inv_step_dist
-        steps = dist * inv_step_dist
-        count = self._ffi_lib.stepcompress_push_factor(
-            self._stepqueue, steps, step_offset,
-            mcu_time * self._mcu_freq, 1. / (cruise_v * self._velocity_factor))
+        count = self._ffi_lib.stepcompress_push_const(
+            self._stepqueue, mcu_time * self._mcu_freq, step_offset,
+            dist * inv_step_dist, cruise_v * self._velocity_factor)
         if count == STEPCOMPRESS_ERROR_RET:
             raise error("Internal error in stepcompress")
         self._commanded_pos += count
     def step_accel(self, mcu_time, start_pos, dist, start_v, accel):
-        #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
         inv_step_dist = self._inv_step_dist
-        mcu_freq = self._mcu_freq
-        inv_accel = 1. / accel
-        time_offset = start_v * inv_accel * mcu_freq
-        sqrt_offset = time_offset**2
         step_offset = self._commanded_pos - start_pos * inv_step_dist
-        steps = dist * inv_step_dist
-        clock = mcu_time * mcu_freq - time_offset
-        count = self._ffi_lib.stepcompress_push_sqrt(
-            self._stepqueue, steps, step_offset, clock,
-            sqrt_offset, 2. * inv_accel * self._inv_accel_factor)
+        count = self._ffi_lib.stepcompress_push_accel(
+            self._stepqueue, mcu_time * self._mcu_freq, step_offset,
+            dist * inv_step_dist, start_v * self._velocity_factor,
+            accel * self._accel_factor)
         if count == STEPCOMPRESS_ERROR_RET:
             raise error("Internal error in stepcompress")
         self._commanded_pos += count
diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c
index 41e7f515..6a85b3e2 100644
--- a/klippy/stepcompress.c
+++ b/klippy/stepcompress.c
@@ -465,11 +465,10 @@ stepcompress_push(struct stepcompress *sc, double step_clock, int32_t sdir)
 }
 
 // Schedule 'steps' number of steps with a constant time between steps
-// using the formula: step_clock = clock_offset + step_num*factor
+// using the formula: step_clock = clock_offset + step_num/cruise_sv
 int32_t
-stepcompress_push_factor(struct stepcompress *sc
-                         , double steps, double step_offset
-                         , double clock_offset, double factor)
+stepcompress_push_const(struct stepcompress *sc, double clock_offset
+                        , double step_offset, double steps, double cruise_sv)
 {
     // Calculate number of steps to take
     int sdir = 1;
@@ -481,8 +480,8 @@ stepcompress_push_factor(struct stepcompress *sc
     int count = steps + .5 - step_offset;
     if (count <= 0 || count > 10000000) {
         if (count && steps) {
-            errorf("push_factor invalid count %d %f %f %f %f"
-                   , sc->oid, steps, step_offset, clock_offset, factor);
+            errorf("push_const invalid count %d %f %f %f %f"
+                   , sc->oid, clock_offset, step_offset, steps, cruise_sv);
             return ERROR_RET;
         }
         return 0;
@@ -494,6 +493,7 @@ stepcompress_push_factor(struct stepcompress *sc
 
     // Calculate each step time
     clock_offset += 0.5;
+    double factor = 1. / cruise_sv;
     double pos = step_offset + .5;
     uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
     while (count--) {
@@ -507,11 +507,14 @@ stepcompress_push_factor(struct stepcompress *sc
     return res;
 }
 
-// Schedule 'steps' number of steps using the formula:
-//  step_clock = clock_offset + sqrt(step_num*factor + sqrt_offset)
+// Schedule 'steps' number of steps at constant acceleration. It uses
+// the formula:
+//  step_clock = (clock_offset + sqrt(2*step_num/accel + (start_sv/accel)**2)
+//                - start_sv/accel)
 int32_t
-stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset
-                       , double clock_offset, double sqrt_offset, double factor)
+stepcompress_push_accel(struct stepcompress *sc, double clock_offset
+                        , double step_offset, double steps
+                        , double start_sv, double accel)
 {
     // Calculate number of steps to take
     int sdir = 1;
@@ -523,9 +526,8 @@ stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset
     int count = steps + .5 - step_offset;
     if (count <= 0 || count > 10000000) {
         if (count && steps) {
-            errorf("push_sqrt invalid count %d %f %f %f %f %f"
-                   , sc->oid, steps, step_offset, clock_offset, sqrt_offset
-                   , factor);
+            errorf("push_accel invalid count %d %f %f %f %f %f"
+                   , sc->oid, clock_offset, step_offset, steps, start_sv, accel);
             return ERROR_RET;
         }
         return 0;
@@ -536,8 +538,10 @@ stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset
     int res = sdir ? count : -count;
 
     // Calculate each step time
-    clock_offset += 0.5;
-    double pos = step_offset + .5 + sqrt_offset/factor;
+    double inv_accel = 1. / accel;
+    double factor = 2. * inv_accel;
+    clock_offset += 0.5 - start_sv * inv_accel;
+    double pos = step_offset + .5 + .5 * start_sv*start_sv * inv_accel;
     uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
     while (count--) {
         int ret = check_expand(sc, &qn, &qend);