stepcompress: Merge stepcompress_push_accel() and stepcompress_push_const()

It's not necessary to have separate C functions for constant
acceleration and constant velocity as constant velocity can be
obtained by using a constant acceleration of zero.

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

6 files changed, 48 insertions, 82 deletions

diff --git a/klippy/cartesian.py b/klippy/cartesian.py
index c1e07a84..ab95df5d 100644
--- a/klippy/cartesian.py
+++ b/klippy/cartesian.py
@@ -112,18 +112,19 @@ class CartKinematics:
             # Acceleration steps
             if move.accel_r:
                 accel_d = move.accel_r * axis_d
-                mcu_stepper.step_accel(
+                mcu_stepper.step_const(
                     mcu_time, start_pos, accel_d, move.start_v * axis_r, accel)
                 start_pos += accel_d
                 mcu_time += move.accel_t
             # Cruising steps
             if move.cruise_r:
                 cruise_d = move.cruise_r * axis_d
-                mcu_stepper.step_const(mcu_time, start_pos, cruise_d, cruise_v)
+                mcu_stepper.step_const(
+                    mcu_time, start_pos, cruise_d, cruise_v, 0.)
                 start_pos += cruise_d
                 mcu_time += move.cruise_t
             # Deceleration steps
             if move.decel_r:
                 decel_d = move.decel_r * axis_d
-                mcu_stepper.step_accel(
+                mcu_stepper.step_const(
                     mcu_time, start_pos, decel_d, cruise_v, -accel)
diff --git a/klippy/chelper.py b/klippy/chelper.py
index bfc74127..910fa5e6 100644
--- a/klippy/chelper.py
+++ b/klippy/chelper.py
@@ -23,8 +23,6 @@ defs_stepcompress = """
     int stepcompress_push(struct stepcompress *sc, double step_clock
         , int32_t sdir);
     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
diff --git a/klippy/corexy.py b/klippy/corexy.py
index b724a4e1..dfee7bb3 100644
--- a/klippy/corexy.py
+++ b/klippy/corexy.py
@@ -126,18 +126,19 @@ class CoreXYKinematics:
             # Acceleration steps
             if move.accel_r:
                 accel_d = move.accel_r * axis_d
-                mcu_stepper.step_accel(
+                mcu_stepper.step_const(
                     mcu_time, start_pos, accel_d, move.start_v * axis_r, accel)
                 start_pos += accel_d
                 mcu_time += move.accel_t
             # Cruising steps
             if move.cruise_r:
                 cruise_d = move.cruise_r * axis_d
-                mcu_stepper.step_const(mcu_time, start_pos, cruise_d, cruise_v)
+                mcu_stepper.step_const(
+                    mcu_time, start_pos, cruise_d, cruise_v, 0.)
                 start_pos += cruise_d
                 mcu_time += move.cruise_t
             # Deceleration steps
             if move.decel_r:
                 decel_d = move.decel_r * axis_d
-                mcu_stepper.step_accel(
+                mcu_stepper.step_const(
                     mcu_time, start_pos, decel_d, cruise_v, -accel)
diff --git a/klippy/extruder.py b/klippy/extruder.py
index 4321f06b..24153e0f 100644
--- a/klippy/extruder.py
+++ b/klippy/extruder.py
@@ -169,22 +169,22 @@ class PrinterExtruder:
 
         # Acceleration steps
         if accel_d:
-            mcu_stepper.step_accel(mcu_time, start_pos, accel_d, start_v, accel)
+            mcu_stepper.step_const(mcu_time, start_pos, accel_d, start_v, accel)
             start_pos += accel_d
             mcu_time += accel_t
         # Cruising steps
         if cruise_d:
-            mcu_stepper.step_const(mcu_time, start_pos, cruise_d, cruise_v)
+            mcu_stepper.step_const(mcu_time, start_pos, cruise_d, cruise_v, 0.)
             start_pos += cruise_d
             mcu_time += cruise_t
         # Deceleration steps
         if decel_d:
-            mcu_stepper.step_accel(mcu_time, start_pos, decel_d, decel_v, -accel)
+            mcu_stepper.step_const(mcu_time, start_pos, decel_d, decel_v, -accel)
             start_pos += decel_d
             mcu_time += decel_t
         # Retraction steps
         if retract_d:
-            mcu_stepper.step_accel(
+            mcu_stepper.step_const(
                 mcu_time, start_pos, -retract_d, retract_v, accel)
             start_pos -= retract_d
         self.extrude_pos = start_pos
diff --git a/klippy/mcu.py b/klippy/mcu.py
index 9127da6c..741ac0aa 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -118,20 +118,11 @@ class MCU_stepper:
             self._commanded_pos += 1
         else:
             self._commanded_pos -= 1
-    def step_const(self, mcu_time, start_pos, dist, cruise_v):
+    def step_const(self, mcu_time, start_pos, dist, start_v, accel):
         inv_step_dist = self._inv_step_dist
         step_offset = self._commanded_pos - start_pos * inv_step_dist
         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):
-        inv_step_dist = self._inv_step_dist
-        step_offset = self._commanded_pos - start_pos * inv_step_dist
-        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:
diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c
index 6a85b3e2..8c847cb3 100644
--- a/klippy/stepcompress.c
+++ b/klippy/stepcompress.c
@@ -464,11 +464,16 @@ stepcompress_push(struct stepcompress *sc, double step_clock, int32_t sdir)
     return 0;
 }
 
-// Schedule 'steps' number of steps with a constant time between steps
-// using the formula: step_clock = clock_offset + step_num/cruise_sv
+// Schedule 'steps' number of steps at constant acceleration. If
+// acceleration is zero (ie, constant velocity) it uses the formula:
+//  step_clock = clock_offset + step_num/start_sv
+// Otherwise it uses the formula:
+//  step_clock = (clock_offset + sqrt(2*step_num/accel + (start_sv/accel)**2)
+//                - start_sv/accel)
 int32_t
-stepcompress_push_const(struct stepcompress *sc, double clock_offset
-                        , double step_offset, double steps, double cruise_sv)
+stepcompress_push_const(
+    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;
@@ -480,8 +485,9 @@ stepcompress_push_const(struct stepcompress *sc, double clock_offset
     int count = steps + .5 - step_offset;
     if (count <= 0 || count > 10000000) {
         if (count && steps) {
-            errorf("push_const invalid count %d %f %f %f %f"
-                   , sc->oid, clock_offset, step_offset, steps, cruise_sv);
+            errorf("push_const invalid count %d %f %f %f %f %f"
+                   , sc->oid, clock_offset, step_offset, steps
+                   , start_sv, accel);
             return ERROR_RET;
         }
         return 0;
@@ -493,63 +499,32 @@ stepcompress_push_const(struct stepcompress *sc, double clock_offset
 
     // 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--) {
-        int ret = check_expand(sc, &qn, &qend);
-        if (ret)
-            return ret;
-        *qn++ = clock_offset + pos*factor;
-        pos += 1.0;
-    }
-    sc->queue_next = qn;
-    return res;
-}
-
-// 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_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;
-    if (steps < 0) {
-        sdir = 0;
-        steps = -steps;
-        step_offset = -step_offset;
-    }
-    int count = steps + .5 - step_offset;
-    if (count <= 0 || count > 10000000) {
-        if (count && steps) {
-            errorf("push_accel invalid count %d %f %f %f %f %f"
-                   , sc->oid, clock_offset, step_offset, steps, start_sv, accel);
-            return ERROR_RET;
+    if (!accel) {
+        // Move at constant velocity (zero acceleration)
+        double inv_cruise_sv = 1. / start_sv;
+        while (count--) {
+            int ret = check_expand(sc, &qn, &qend);
+            if (ret)
+                return ret;
+            *qn++ = clock_offset + pos*inv_cruise_sv;
+            pos += 1.0;
+        }
+    } else {
+        // Move with constant acceleration
+        double inv_accel = 1. / accel;
+        clock_offset -= start_sv * inv_accel;
+        pos += .5 * start_sv*start_sv * inv_accel;
+        double accel_multiplier = 2. * inv_accel;
+        while (count--) {
+            int ret = check_expand(sc, &qn, &qend);
+            if (ret)
+                return ret;
+            double v = safe_sqrt(pos * accel_multiplier);
+            *qn++ = clock_offset + (accel_multiplier >= 0. ? v : -v);
+            pos += 1.0;
         }
-        return 0;
-    }
-    int ret = set_next_step_dir(sc, sdir);
-    if (ret)
-        return ret;
-    int res = sdir ? count : -count;
-
-    // Calculate each step time
-    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);
-        if (ret)
-            return ret;
-        double v = safe_sqrt(pos*factor);
-        *qn++ = clock_offset + (factor >= 0. ? v : -v);
-        pos += 1.0;
     }
     sc->queue_next = qn;
     return res;