stepcompress: Merge stepcompress_delta_const and stepcompress_delta_accel

It's not necessary to have separate C delta kinematic 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>

4 files changed, 62 insertions, 114 deletions

diff --git a/klippy/chelper.py b/klippy/chelper.py
index afc5b1d9..86994830 100644
--- a/klippy/chelper.py
+++ b/klippy/chelper.py
@@ -24,11 +24,7 @@ defs_stepcompress = """
         , int32_t sdir);
     int32_t stepcompress_push_const(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 start_pos, double steps, double cruise_sv
-        , double height, double closestxy_sd
-        , double closest_height2, double movez_r);
-    int32_t stepcompress_push_delta_accel(struct stepcompress *sc
+    int32_t stepcompress_push_delta(struct stepcompress *sc
         , double clock_offset, double start_pos, double steps, double start_sv
         , double accel, double height, double closestxy_sd
         , double closest_height2, double movez_r);
diff --git a/klippy/delta.py b/klippy/delta.py
index 9ae72810..5913e53a 100644
--- a/klippy/delta.py
+++ b/klippy/delta.py
@@ -228,28 +228,28 @@ class DeltaKinematics:
             mcu_stepper = self.steppers[i].mcu_stepper
             mcu_time = mcu_stepper.print_to_mcu_time(move_time)
             if accel_up_d > 0.:
-                mcu_stepper.step_delta_accel(
+                mcu_stepper.step_delta(
                     mcu_time, 0., accel_up_d, move.start_v, accel,
                     origz, closestxy_d, closest_height2, movez_r)
             if cruise_up_d > 0.:
-                mcu_stepper.step_delta_const(
-                    mcu_time + accel_t, accel_d, cruise_up_d, cruise_v,
+                mcu_stepper.step_delta(
+                    mcu_time + accel_t, accel_d, cruise_up_d, cruise_v, 0.,
                     origz, closestxy_d, closest_height2, movez_r)
             if decel_up_d > 0.:
-                mcu_stepper.step_delta_accel(
+                mcu_stepper.step_delta(
                     mcu_time + cruise_end_t, cruise_end_d, decel_up_d,
                     cruise_v, -accel,
                     origz, closestxy_d, closest_height2, movez_r)
             if accel_down_d > 0.:
-                mcu_stepper.step_delta_accel(
+                mcu_stepper.step_delta(
                     mcu_time, 0., -accel_down_d, move.start_v, accel,
                     origz, closestxy_d, closest_height2, movez_r)
             if cruise_down_d > 0.:
-                mcu_stepper.step_delta_const(
-                    mcu_time + accel_t, accel_d, -cruise_down_d, cruise_v,
+                mcu_stepper.step_delta(
+                    mcu_time + accel_t, accel_d, -cruise_down_d, cruise_v, 0.,
                     origz, closestxy_d, closest_height2, movez_r)
             if decel_down_d > 0.:
-                mcu_stepper.step_delta_accel(
+                mcu_stepper.step_delta(
                     mcu_time + cruise_end_t, cruise_end_d, -decel_down_d,
                     cruise_v, -accel,
                     origz, closestxy_d, closest_height2, movez_r)
diff --git a/klippy/mcu.py b/klippy/mcu.py
index 26e98d39..d28d21f1 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -128,24 +128,11 @@ class MCU_stepper:
         if count == STEPCOMPRESS_ERROR_RET:
             raise error("Internal error in stepcompress")
         self._commanded_pos += count
-    def step_delta_const(self, mcu_time, start_pos, dist, cruise_v
-                         , height_base, closestxy_d, closest_height2, movez_r):
+    def step_delta(self, mcu_time, start_pos, dist, start_v, accel
+                   , height_base, closestxy_d, closest_height2, movez_r):
         inv_step_dist = self._inv_step_dist
         height = self._commanded_pos - height_base * inv_step_dist
-        count = self._ffi_lib.stepcompress_push_delta_const(
-            self._stepqueue, mcu_time * self._mcu_freq,
-            -start_pos * inv_step_dist, dist * inv_step_dist,
-            cruise_v * self._velocity_factor,
-            height, closestxy_d * inv_step_dist,
-            closest_height2 * inv_step_dist**2, movez_r)
-        if count == STEPCOMPRESS_ERROR_RET:
-            raise error("Internal error in stepcompress")
-        self._commanded_pos += count
-    def step_delta_accel(self, mcu_time, start_pos, dist, start_v, accel
-                         , height_base, closestxy_d, closest_height2, movez_r):
-        inv_step_dist = self._inv_step_dist
-        height = self._commanded_pos - height_base * inv_step_dist
-        count = self._ffi_lib.stepcompress_push_delta_accel(
+        count = self._ffi_lib.stepcompress_push_delta(
             self._stepqueue, mcu_time * self._mcu_freq,
             -start_pos * inv_step_dist, dist * inv_step_dist,
             start_v * self._velocity_factor, accel * self._accel_factor,
diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c
index 18750e9f..4f59eaa2 100644
--- a/klippy/stepcompress.c
+++ b/klippy/stepcompress.c
@@ -530,11 +530,11 @@ stepcompress_push_const(
     return res;
 }
 
-// Schedule 'count' number of steps using the delta kinematic const speed
+// Schedule steps using delta kinematics
 int32_t
-stepcompress_push_delta_const(
+stepcompress_push_delta(
     struct stepcompress *sc, double clock_offset, double start_pos
-    , double steps, double cruise_sv
+    , double steps, double start_sv, double accel
     , double height, double closestxy_sd, double closest_height2, double movez_r)
 {
     // Calculate number of steps to take
@@ -549,9 +549,9 @@ stepcompress_push_delta_const(
     int count = (end_height - height + movez_r*steps) * step_dist + .5;
     if (count <= 0 || count > 10000000) {
         if (count) {
-            errorf("push_delta_const invalid count %d %d %f %f %f %f %f %f %f %f"
-                   , sc->oid, count, clock_offset, start_pos, steps, cruise_sv
-                   , height, closestxy_sd, closest_height2, movez_r);
+            errorf("push_delta invalid count %d %d %f %f %f %f %f %f %f %f %f"
+                   , sc->oid, count, clock_offset, start_pos, steps, start_sv
+                   , accel, height, closestxy_sd, closest_height2, movez_r);
             return ERROR_RET;
         }
         return 0;
@@ -562,35 +562,54 @@ stepcompress_push_delta_const(
     int res = step_dist > 0. ? count : -count;
 
     // Calculate each step time
-    double inv_cruise_sv = 1. / cruise_sv;
     clock_offset += 0.5;
     start_pos += movexy_r*closestxy_sd;
     height += .5 * step_dist;
     uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
-    if (!movez_r) {
-        // Optmized case for common XY only moves (no Z movement)
-        while (count--) {
-            int ret = check_expand(sc, &qn, &qend);
-            if (ret)
-                return ret;
-            double v = safe_sqrt(closest_height2 - height*height);
-            double pos = start_pos + (step_dist > 0. ? -v : v);
-            *qn++ = clock_offset + pos * inv_cruise_sv;
-            height += step_dist;
-        }
-    } else if (!movexy_r) {
-        // Optmized case for Z only moves
-        double v = (step_dist > 0. ? -end_height : end_height);
-        while (count--) {
-            int ret = check_expand(sc, &qn, &qend);
-            if (ret)
-                return ret;
-            double pos = start_pos + movez_r*height + v;
-            *qn++ = clock_offset + pos * inv_cruise_sv;
-            height += step_dist;
+    if (!accel) {
+        // Move at constant velocity (zero acceleration)
+        double inv_cruise_sv = 1. / start_sv;
+        if (!movez_r) {
+            // Optmized case for common XY only moves (no Z movement)
+            while (count--) {
+                int ret = check_expand(sc, &qn, &qend);
+                if (ret)
+                    return ret;
+                double v = safe_sqrt(closest_height2 - height*height);
+                double pos = start_pos + (step_dist > 0. ? -v : v);
+                *qn++ = clock_offset + pos * inv_cruise_sv;
+                height += step_dist;
+            }
+        } else if (!movexy_r) {
+            // Optmized case for Z only moves
+            double v = (step_dist > 0. ? -end_height : end_height);
+            while (count--) {
+                int ret = check_expand(sc, &qn, &qend);
+                if (ret)
+                    return ret;
+                double pos = start_pos + movez_r*height + v;
+                *qn++ = clock_offset + pos * inv_cruise_sv;
+                height += step_dist;
+            }
+        } else {
+            // General case (handles XY+Z moves)
+            while (count--) {
+                int ret = check_expand(sc, &qn, &qend);
+                if (ret)
+                    return ret;
+                double relheight = movexy_r*height - movez_r*closestxy_sd;
+                double v = safe_sqrt(closest_height2 - relheight*relheight);
+                double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v);
+                *qn++ = clock_offset + pos * inv_cruise_sv;
+                height += step_dist;
+            }
         }
     } else {
-        // General case (handles XY+Z moves)
+        // Move with constant acceleration
+        double inv_accel = 1. / accel;
+        clock_offset -= start_sv * inv_accel;
+        start_pos += 0.5 * start_sv*start_sv * inv_accel;
+        double accel_multiplier = 2. * inv_accel;
         while (count--) {
             int ret = check_expand(sc, &qn, &qend);
             if (ret)
@@ -598,7 +617,8 @@ stepcompress_push_delta_const(
             double relheight = movexy_r*height - movez_r*closestxy_sd;
             double v = safe_sqrt(closest_height2 - relheight*relheight);
             double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v);
-            *qn++ = clock_offset + pos * inv_cruise_sv;
+            v = safe_sqrt(pos * accel_multiplier);
+            *qn++ = clock_offset + (accel_multiplier >= 0. ? v : -v);
             height += step_dist;
         }
     }
@@ -606,61 +626,6 @@ stepcompress_push_delta_const(
     return res;
 }
 
-// Schedule 'count' number of steps using delta kinematic acceleration
-int32_t
-stepcompress_push_delta_accel(
-    struct stepcompress *sc, double clock_offset, double start_pos
-    , double steps, double start_sv, double accel
-    , double height, double closestxy_sd, double closest_height2, double movez_r)
-{
-    // Calculate number of steps to take
-    double step_dist = 1.;
-    if (steps < 0) {
-        step_dist = -1.;
-        steps = -steps;
-    }
-    double movexy_r = movez_r ? sqrt(1. - movez_r*movez_r) : 1.;
-    double reldist = closestxy_sd - movexy_r*steps;
-    double end_height = safe_sqrt(closest_height2 - reldist*reldist);
-    int count = (end_height - height + movez_r*steps) * step_dist + .5;
-    if (count <= 0 || count > 10000000) {
-        if (count) {
-            errorf("push_delta_accel invalid count %d %d %f %f"
-                   " %f %f %f %f %f %f %f"
-                   , sc->oid, count, clock_offset, start_pos
-                   , steps, start_sv, accel
-                   , height, closestxy_sd, closest_height2, movez_r);
-            return ERROR_RET;
-        }
-        return 0;
-    }
-    int ret = set_next_step_dir(sc, step_dist > 0.);
-    if (ret)
-        return ret;
-    int res = step_dist > 0. ? count : -count;
-
-    // Calculate each step time
-    double inv_accel = 1. / accel;
-    double accel_multiplier = 2. * inv_accel;
-    clock_offset += 0.5 - start_sv * inv_accel;
-    start_pos += movexy_r*closestxy_sd + 0.5 * start_sv*start_sv * inv_accel;
-    height += .5 * step_dist;
-    uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
-    while (count--) {
-        int ret = check_expand(sc, &qn, &qend);
-        if (ret)
-            return ret;
-        double relheight = movexy_r*height - movez_r*closestxy_sd;
-        double v = safe_sqrt(closest_height2 - relheight*relheight);
-        double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v);
-        v = safe_sqrt(pos * accel_multiplier);
-        *qn++ = clock_offset + (accel_multiplier >= 0. ? v : -v);
-        height += step_dist;
-    }
-    sc->queue_next = qn;
-    return res;
-}
-
 
 /****************************************************************
  * Step compress synchronization