delta: Rework delta math to avoid using inv_movexy_r

Taking the inverse of the XY move distance can lead to extremely large
values when the XY distance is very small.  This can lead to
saturation of the double precision variables and incorrect results.

Rework the delta kinematic math to avoid using this inverse.  Pass the
closestxy_d value directly to the C functions so that the C code can
calculate its intermediate constants.

After this change the move_z special case is no longer necessary as
the regular delta functions now work with movexy_r=0 and movez_r=1.

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

1 files changed, 41 insertions, 92 deletions

diff --git a/klippy/delta.py b/klippy/delta.py
index 19984496..7d7b063f 100644
--- a/klippy/delta.py
+++ b/klippy/delta.py
@@ -125,72 +125,25 @@ class DeltaKinematics:
                 raise homing.EndstopMoveError(end_pos)
         if move.axes_d[2]:
             move.limit_speed(self.max_z_velocity, 9999999.9)
-    def move_z(self, move_time, move):
-        if not move.axes_d[2]:
-            return
-        if self.need_motor_enable:
-            self.check_motor_enable(move_time)
-        inv_accel = 1. / move.accel
-        inv_cruise_v = 1. / move.cruise_v
-        for i in StepList:
-            towerx_d = self.towers[i][0] - move.start_pos[0]
-            towery_d = self.towers[i][1] - move.start_pos[1]
-            tower_d2 = towerx_d**2 + towery_d**2
-            height = math.sqrt(self.arm_length2 - tower_d2) + move.start_pos[2]
-
-            mcu_stepper = self.steppers[i].mcu_stepper
-            mcu_time = mcu_stepper.print_to_mcu_time(move_time)
-            step_pos = mcu_stepper.commanded_position
-            inv_step_dist = self.steppers[i].inv_step_dist
-            step_offset = step_pos - height * inv_step_dist
-            step_dist = self.steppers[i].step_dist
-            steps = move.axes_d[2] * inv_step_dist
-
-            # Acceleration steps
-            accel_multiplier = 2.0 * step_dist * inv_accel
-            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 = mcu_stepper.step_sqrt(
-                    mcu_time - accel_time_offset, accel_steps, step_offset
-                    , accel_sqrt_offset, accel_multiplier)
-                step_offset += count - accel_steps
-                mcu_time += move.accel_t
-            # Cruising steps
-            if move.cruise_r:
-                #t = pos/cruise_v
-                cruise_multiplier = step_dist * inv_cruise_v
-                cruise_steps = move.cruise_r * steps
-                count = mcu_stepper.step_factor(
-                    mcu_time, cruise_steps, step_offset, cruise_multiplier)
-                step_offset += count - cruise_steps
-                mcu_time += move.cruise_t
-            # Deceleration steps
-            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 = mcu_stepper.step_sqrt(
-                    mcu_time + decel_time_offset, decel_steps, step_offset
-                    , decel_sqrt_offset, -accel_multiplier)
     def move(self, move_time, move):
         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]:
-            self.move_z(move_time, move)
-            return
+            if not axes_d[2]:
+                return
+            movez_r = axes_d[2] * inv_movexy_d
+            movexy_d = movexy_r = inv_movexy_d = 0.
+        elif axes_d[2]:
+            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)
-        move_d = move.move_d
-        movez_r = 0.
-        inv_movexy_d = 1. / move_d
-        inv_movexy_r = 1.
-        if axes_d[2]:
-            movez_r = axes_d[2] * inv_movexy_d
-            inv_movexy_d = 1. / math.sqrt(axes_d[0]**2 + axes_d[1]**2)
-            inv_movexy_r = move_d * inv_movexy_d
 
         origx, origy, origz = move.start_pos[:3]
 
@@ -214,80 +167,76 @@ class DeltaKinematics:
             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
-            closest_height = math.sqrt(closest_height2)
-            closest_d = closestxy_d * inv_movexy_r
-            closestz = origz + closest_d*movez_r
 
             # Calculate accel/cruise/decel portions of move
-            reverse_d = closest_d + closest_height*movez_r*inv_movexy_r
+            reversexy_d = closestxy_d + math.sqrt(closest_height2)*movez_r
             accel_up_d = cruise_up_d = decel_up_d = 0.
             accel_down_d = cruise_down_d = decel_down_d = 0.
-            if reverse_d <= 0.:
+            if reversexy_d <= 0.:
                 accel_down_d = accel_d
                 cruise_down_d = cruise_end_d
                 decel_down_d = move_d
-            elif reverse_d >= move_d:
+            elif reversexy_d >= movexy_d:
                 accel_up_d = accel_d
                 cruise_up_d = cruise_end_d
                 decel_up_d = move_d
-            elif reverse_d < accel_d:
-                accel_up_d = reverse_d
+            elif reversexy_d < accel_d * movexy_r:
+                accel_up_d = reversexy_d * move_d * inv_movexy_d
                 accel_down_d = accel_d
                 cruise_down_d = cruise_end_d
                 decel_down_d = move_d
-            elif reverse_d < cruise_end_d:
+            elif reversexy_d < cruise_end_d * movexy_r:
                 accel_up_d = accel_d
-                cruise_up_d = reverse_d
+                cruise_up_d = reversexy_d * move_d * inv_movexy_d
                 cruise_down_d = cruise_end_d
                 decel_down_d = move_d
             else:
                 accel_up_d = accel_d
                 cruise_up_d = cruise_end_d
-                decel_up_d = reverse_d
+                decel_up_d = reversexy_d * move_d * inv_movexy_d
                 decel_down_d = move_d
 
             # Generate steps
             mcu_stepper = self.steppers[i].mcu_stepper
             mcu_time = mcu_stepper.print_to_mcu_time(move_time)
             step_pos = mcu_stepper.commanded_position
-            inv_step_dist = self.steppers[i].inv_step_dist
             step_dist = self.steppers[i].step_dist
-            height = step_pos*step_dist - closestz
+            height = step_pos*step_dist - origz
             if accel_up_d > 0.:
                 count = mcu_stepper.step_delta_accel(
-                    mcu_time - accel_time_offset, closest_d - accel_up_d,
-                    step_dist, closest_d + accel_offset,
-                    closest_height2, height, movez_r, accel_multiplier)
+                    mcu_time - accel_time_offset, accel_up_d,
+                    accel_offset, accel_multiplier, step_dist,
+                    height, closestxy_d, closest_height2, movez_r)
                 height += count * step_dist
             if cruise_up_d > 0.:
                 count = mcu_stepper.step_delta_const(
-                    mcu_time + accel_t, closest_d - cruise_up_d,
-                    step_dist, closest_d - accel_d,
-                    closest_height2, height, movez_r, inv_cruise_v)
+                    mcu_time + accel_t, cruise_up_d,
+                    -accel_d, inv_cruise_v, step_dist,
+                    height, closestxy_d, closest_height2, movez_r)
                 height += count * step_dist
             if decel_up_d > 0.:
                 count = mcu_stepper.step_delta_accel(
-                    mcu_time + decel_time_offset, closest_d - decel_up_d,
-                    step_dist, closest_d - decel_offset,
-                    closest_height2, height, movez_r, -accel_multiplier)
+                    mcu_time + decel_time_offset, decel_up_d,
+                    -decel_offset, -accel_multiplier, step_dist,
+                    height, closestxy_d, closest_height2, movez_r)
                 height += count * step_dist
             if accel_down_d > 0.:
                 count = mcu_stepper.step_delta_accel(
-                    mcu_time - accel_time_offset, closest_d - accel_down_d,
-                    -step_dist, closest_d + accel_offset,
-                    closest_height2, height, movez_r, accel_multiplier)
+                    mcu_time - accel_time_offset, accel_down_d,
+                    accel_offset, accel_multiplier, -step_dist,
+                    height, closestxy_d, closest_height2, movez_r)
                 height += count * step_dist
             if cruise_down_d > 0.:
                 count = mcu_stepper.step_delta_const(
-                    mcu_time + accel_t, closest_d - cruise_down_d,
-                    -step_dist, closest_d - accel_d,
-                    closest_height2, height, movez_r, inv_cruise_v)
+                    mcu_time + accel_t, cruise_down_d,
+                    -accel_d, inv_cruise_v, -step_dist,
+                    height, closestxy_d, closest_height2, movez_r)
                 height += count * step_dist
             if decel_down_d > 0.:
                 count = mcu_stepper.step_delta_accel(
-                    mcu_time + decel_time_offset, closest_d - decel_down_d,
-                    -step_dist, closest_d - decel_offset,
-                    closest_height2, height, movez_r, -accel_multiplier)
+                    mcu_time + decel_time_offset, decel_down_d,
+                    -decel_offset, -accel_multiplier, -step_dist,
+                    height, closestxy_d, closest_height2, movez_r)
 
 
 ######################################################################