delta: Make it clear that a "virtual tower" is created

The delta code calculates a "virtual tower" along the line of
movement.  Rework the variable names and comments to make it clear
that this is occurring.

It is not necessary to pass the start_pos variable to the C code as it
is simple to update the start_pos at the start of each movement.

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

4 files changed, 52 insertions, 46 deletions

diff --git a/klippy/chelper.py b/klippy/chelper.py
index 86994830..230f6a3d 100644
--- a/klippy/chelper.py
+++ b/klippy/chelper.py
@@ -25,9 +25,8 @@ defs_stepcompress = """
     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(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);
+        , double clock_offset, double move_sd, double start_sv, double accel
+        , double height, double startxy_sd, double arm_d, double movez_r);
 
     struct steppersync *steppersync_alloc(struct serialqueue *sq
         , struct stepcompress **sc_list, int sc_num, int move_num);
diff --git a/klippy/delta.py b/klippy/delta.py
index 5913e53a..8859e17b 100644
--- a/klippy/delta.py
+++ b/klippy/delta.py
@@ -162,23 +162,24 @@ class DeltaKinematics:
             limit_xy2 = -1.
         self.limit_xy2 = min(limit_xy2, self.slow_xy2)
     def move(self, move_time, move):
+        if self.need_motor_enable:
+            self._check_motor_enable(move_time)
         axes_d = move.axes_d
         move_d = movexy_d = move.move_d
         movexy_r = 1.
         movez_r = 0.
         inv_movexy_d = 1. / movexy_d
         if not axes_d[0] and not axes_d[1]:
+            # Z only move
             movez_r = axes_d[2] * inv_movexy_d
             movexy_d = movexy_r = inv_movexy_d = 0.
         elif axes_d[2]:
+            # XY+Z move
             movexy_d = math.sqrt(axes_d[0]**2 + axes_d[1]**2)
             movexy_r = movexy_d * inv_movexy_d
             movez_r = axes_d[2] * inv_movexy_d
             inv_movexy_d = 1. / movexy_d
 
-        if self.need_motor_enable:
-            self._check_motor_enable(move_time)
-
         origx, origy, origz = move.start_pos[:3]
 
         accel = move.accel
@@ -189,15 +190,16 @@ class DeltaKinematics:
         cruise_end_d = accel_d + move.cruise_r * move_d
 
         for i in StepList:
-            # Find point on line of movement closest to tower
+            # Calculate a virtual tower along the line of movement at
+            # the point closest to this stepper's tower.
             towerx_d = self.towers[i][0] - origx
             towery_d = self.towers[i][1] - origy
-            closestxy_d = (towerx_d*axes_d[0] + towery_d*axes_d[1])*inv_movexy_d
-            tangentxy_d2 = towerx_d**2 + towery_d**2 - closestxy_d**2
-            closest_height2 = self.arm_length2 - tangentxy_d2
+            vt_startxy_d = (towerx_d*axes_d[0] + towery_d*axes_d[1])*inv_movexy_d
+            tangentxy_d2 = towerx_d**2 + towery_d**2 - vt_startxy_d**2
+            vt_arm_d = math.sqrt(self.arm_length2 - tangentxy_d2)
 
             # Calculate accel/cruise/decel portions of move
-            reversexy_d = closestxy_d + math.sqrt(closest_height2)*movez_r
+            reversexy_d = vt_startxy_d + vt_arm_d*movez_r
             accel_up_d = cruise_up_d = decel_up_d = 0.
             accel_down_d = cruise_down_d = decel_down_d = 0.
             if reversexy_d <= 0.:
@@ -229,30 +231,36 @@ class DeltaKinematics:
             mcu_time = mcu_stepper.print_to_mcu_time(move_time)
             if accel_up_d > 0.:
                 mcu_stepper.step_delta(
-                    mcu_time, 0., accel_up_d, move.start_v, accel,
-                    origz, closestxy_d, closest_height2, movez_r)
+                    mcu_time, accel_up_d, move.start_v, accel,
+                    origz, vt_startxy_d, vt_arm_d, movez_r)
             if cruise_up_d > 0.:
                 mcu_stepper.step_delta(
-                    mcu_time + accel_t, accel_d, cruise_up_d, cruise_v, 0.,
-                    origz, closestxy_d, closest_height2, movez_r)
+                    mcu_time + accel_t, cruise_up_d - accel_d, cruise_v, 0.,
+                    origz + accel_d*movez_r, vt_startxy_d - accel_d*movexy_r,
+                    vt_arm_d, movez_r)
             if decel_up_d > 0.:
                 mcu_stepper.step_delta(
-                    mcu_time + cruise_end_t, cruise_end_d, decel_up_d,
+                    mcu_time + cruise_end_t, decel_up_d - cruise_end_d,
                     cruise_v, -accel,
-                    origz, closestxy_d, closest_height2, movez_r)
+                    origz + cruise_end_d*movez_r,
+                    vt_startxy_d - cruise_end_d*movexy_r,
+                    vt_arm_d, movez_r)
             if accel_down_d > 0.:
                 mcu_stepper.step_delta(
-                    mcu_time, 0., -accel_down_d, move.start_v, accel,
-                    origz, closestxy_d, closest_height2, movez_r)
+                    mcu_time, -accel_down_d, move.start_v, accel,
+                    origz, vt_startxy_d, vt_arm_d, movez_r)
             if cruise_down_d > 0.:
                 mcu_stepper.step_delta(
-                    mcu_time + accel_t, accel_d, -cruise_down_d, cruise_v, 0.,
-                    origz, closestxy_d, closest_height2, movez_r)
+                    mcu_time + accel_t, accel_d - cruise_down_d, cruise_v, 0.,
+                    origz + accel_d*movez_r, vt_startxy_d - accel_d*movexy_r,
+                    vt_arm_d, movez_r)
             if decel_down_d > 0.:
                 mcu_stepper.step_delta(
-                    mcu_time + cruise_end_t, cruise_end_d, -decel_down_d,
+                    mcu_time + cruise_end_t, cruise_end_d - decel_down_d,
                     cruise_v, -accel,
-                    origz, closestxy_d, closest_height2, movez_r)
+                    origz + cruise_end_d*movez_r,
+                    vt_startxy_d - cruise_end_d*movexy_r,
+                    vt_arm_d, movez_r)
 
 
 ######################################################################
diff --git a/klippy/mcu.py b/klippy/mcu.py
index d28d21f1..5b12e83f 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -128,16 +128,14 @@ class MCU_stepper:
         if count == STEPCOMPRESS_ERROR_RET:
             raise error("Internal error in stepcompress")
         self._commanded_pos += count
-    def step_delta(self, mcu_time, start_pos, dist, start_v, accel
-                   , height_base, closestxy_d, closest_height2, movez_r):
+    def step_delta(self, mcu_time, dist, start_v, accel
+                   , height_base, startxy_d, arm_d, 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(
-            self._stepqueue, mcu_time * self._mcu_freq,
-            -start_pos * inv_step_dist, dist * inv_step_dist,
+            self._stepqueue, mcu_time * self._mcu_freq, dist * inv_step_dist,
             start_v * self._velocity_factor, accel * self._accel_factor,
-            height, closestxy_d * inv_step_dist,
-            closest_height2 * inv_step_dist**2, movez_r)
+            height, startxy_d * inv_step_dist, arm_d * inv_step_dist, movez_r)
         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 4f59eaa2..d9eeefc3 100644
--- a/klippy/stepcompress.c
+++ b/klippy/stepcompress.c
@@ -533,25 +533,26 @@ stepcompress_push_const(
 // Schedule steps using delta kinematics
 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)
+    struct stepcompress *sc, double clock_offset, double move_sd
+    , double start_sv, double accel
+    , double height, double startxy_sd, double arm_sd, double movez_r)
 {
     // Calculate number of steps to take
     double step_dist = 1.;
-    if (steps < 0) {
+    if (move_sd < 0) {
         step_dist = -1.;
-        steps = -steps;
+        move_sd = -move_sd;
     }
     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;
+    double arm_sd2 = arm_sd * arm_sd;
+    double endxy_sd = startxy_sd - movexy_r*move_sd;
+    double end_height = safe_sqrt(arm_sd2 - endxy_sd*endxy_sd);
+    int count = (end_height + movez_r*move_sd - height) * step_dist + .5;
     if (count <= 0 || count > 10000000) {
         if (count) {
-            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);
+            errorf("push_delta invalid count %d %d %f %f %f %f %f %f %f %f"
+                   , sc->oid, count, clock_offset, move_sd, start_sv, accel
+                   , height, startxy_sd, arm_sd, movez_r);
             return ERROR_RET;
         }
         return 0;
@@ -563,7 +564,7 @@ stepcompress_push_delta(
 
     // Calculate each step time
     clock_offset += 0.5;
-    start_pos += movexy_r*closestxy_sd;
+    double start_pos = movexy_r*startxy_sd;
     height += .5 * step_dist;
     uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
     if (!accel) {
@@ -575,7 +576,7 @@ stepcompress_push_delta(
                 int ret = check_expand(sc, &qn, &qend);
                 if (ret)
                     return ret;
-                double v = safe_sqrt(closest_height2 - height*height);
+                double v = safe_sqrt(arm_sd2 - height*height);
                 double pos = start_pos + (step_dist > 0. ? -v : v);
                 *qn++ = clock_offset + pos * inv_cruise_sv;
                 height += step_dist;
@@ -597,8 +598,8 @@ stepcompress_push_delta(
                 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 relheight = movexy_r*height - movez_r*startxy_sd;
+                double v = safe_sqrt(arm_sd2 - relheight*relheight);
                 double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v);
                 *qn++ = clock_offset + pos * inv_cruise_sv;
                 height += step_dist;
@@ -614,8 +615,8 @@ stepcompress_push_delta(
             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 relheight = movexy_r*height - movez_r*startxy_sd;
+            double v = safe_sqrt(arm_sd2 - 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);