Add 'safe_z_lift' module for safe lifting of (unhomed) Z axis

docs/Config_Reference.md

@@ -294,11 +294,22 @@ microsteps:
 #   The default is 0.000000100 (100ns) for TMC steppers that are
 #   configured in UART or SPI mode, and the default is 0.000002 (which
 #   is 2us) for all other steppers.
-endstop_pin:
-#   Endstop switch detection pin. If this endstop pin is on a
-#   different mcu than the stepper motor then it enables "multi-mcu
-#   homing". This parameter must be provided for the X, Y, and Z
-#   steppers on cartesian style printers.
+endstop_min_pin:
+#   Endstop switch detection pin at the minimum travel limit. This
+#   parameter must be provided for the X, Y, and Z steppers on
+#   cartesian style printers that home towards the minimum limit. If
+#   the endstop pin is on a different mcu than the stepper motor then
+#   it enables "multi-mcu homing".
+endstop_max_pin:
+#   Endstop switch detection pin at the maximum travel limit. This
+#   parameter must be provided for the X, Y, and Z steppers on
+#   cartesian style printers that home towards the maximum limit. If
+#   the endstop pin is on a different mcu than the stepper motor then
+#   it enables "multi-mcu homing".
+#endstop_pin:
+#   Alias for endstop_min_pin or endstop_max_pin, depending on the
+#   homing direction. This parameter is deprecated; use
+#   endstop_min_pin or endstop_max_pin instead.
 #position_min: 0
 #   Minimum valid distance (in mm) the user may command the stepper to
 #   move to.  The default is 0mm.
@@ -1796,6 +1807,16 @@ home_xy_position:
 #  # If True, the Y axis will home first. The default is False.
 ```
 
+### [safe_move]
+
+Safe move. One may use this mechanism to prevent axis crashes during homing and
+probing moves by leveraging directional endstops configured on the axis rails.
+
+```
+[safe_move]
+# (no required parameters)
+```
+
 ### [homing_override]
 
 Homing override. One may use this mechanism to run a series of g-code

docs/G-Codes.md

@@ -1606,6 +1606,18 @@ The following additional commands are also available.
   prepended with `<prefix>`. (The `PREFIX` parameter will take
   priority over the `TYPE` parameter)
 
+### [safe_move]
+
+The following command is available:
+
+#### SAFE_MOVE
+
+`SAFE_MOVE AXIS=<X|Y|Z> DIST=<distance> SPEED=<speed>`: Performs a protected
+single-axis move in the given direction. The move stops early if the directional
+endstop triggers or the end of the axis is reached.
+
+Trying to move into a direction that does not have an endstop will raise an error.
+
 ### [save_variables]
 
 The following command is enabled if a

docs/Kalico_Additions.md

@@ -44,6 +44,7 @@
 - [`[z_tilt/quad_gantry_level] adaptive_horizontal_move_z`](./Config_Reference.md#z_tilt) adaptively decrease horizontal_move_z based on resulting error - z_tilt and QGL faster and safer!
 - [`[safe_z_home] home_y_before_x`](./Config_Reference.md#safe_z_home) let you home Y before X.
 - [`[z_tilt/quad_gantry_level/etc] use_probe_xy_offsets`](./Config_Reference.md#z_tilt) let you decide if the `[probe] XY offsets should be applied to probe positions.
+- [`SAFE_MOVE`](./G-Codes.md#safe_move) allows protected axis moves using directional endstops.
 
 ## Heaters, Fans, and PID changes
 

klippy/extras/dockable_probe.py

@@ -271,7 +271,6 @@ def __init__(self, config):
         self.finish_home_complete = self.wait_trigger_complete = None
 
         # State
-        self.last_z = -9999
         self.multi = MULTI_OFF
         self._last_homed = None
 
@@ -383,6 +382,7 @@ def _handle_config(self):
 
     def _handle_connect(self):
         self.toolhead = self.printer.lookup_object("toolhead")
+        self.safe_move = self.printer.lookup_object("safe_move")
         rails = self.toolhead.get_kinematics().rails
         endstops = [es for rail in rails for es, name in rail.get_endstops()]
         positions = [
@@ -846,18 +846,13 @@ def _align_z_required(self):
 
     # Hop z and return to un-homed state
     def _force_z_hop(self):
-        this_z = self.toolhead.get_position()[2]
-        if self.last_z == this_z:
-            return
-
-        tpos = self.toolhead.get_position()
-        self.toolhead.set_position(
-            [tpos[0], tpos[1], 0.0, tpos[3]], homing_axes=[2]
+        self.safe_move.move(
+            self.toolhead,
+            "z",
+            self.z_hop,
+            self.lift_speed,
+            allow_unsafe=True,
         )
-        self.toolhead.manual_move([None, None, self.z_hop], self.lift_speed)
-        kin = self.toolhead.get_kinematics()
-        kin.clear_homing_state([2])
-        self.last_z = self.toolhead.get_position()[2]
 
     #######################################################################
     # Probe Wrappers

klippy/extras/homing.py

@@ -5,6 +5,7 @@
 # This file may be distributed under the terms of the GNU GPLv3 license.
 import logging
 import math
+from enum import Enum
 
 from .danger_options import get_danger_options
 
@@ -13,6 +14,17 @@
 ENDSTOP_SAMPLE_COUNT = 4
 
 
+class MoveResult(str, Enum):
+    # The move has covered the full requested distance without triggering an endstop.
+    FULL_MOVE = "full_move"
+
+    # The endstop was hit before covering the full distance.
+    HIT_ENDSTOP = "hit_endstop"
+
+    # There was no move because the endstop was already triggered.
+    ALREADY_AT_ENDSTOP = "already_at_endstop"
+
+
 # Return a completion that completes when all completions in a list complete
 def multi_complete(printer, completions):
     if len(completions) == 1:
@@ -28,6 +40,32 @@ def multi_complete(printer, completions):
     return cp
 
 
+# Return a completion that completes when the first completion in a list complete
+def any_complete(printer, completions):
+    if len(completions) == 1:
+        return completions[0]
+    # Build completion that completes on the first completion.
+    reactor = printer.get_reactor()
+    cp = reactor.completion()
+
+    def _wait_one(eventtime, c):
+        res = c.wait()
+        if cp.test():
+            # Another callback already completed cp (and unblocked the other waits) while we were blocked in c.wait().
+            return 0
+        # Complete the main completion and abort any remaining waits so that
+        # callers are not blocked waiting on completions that will never fire.
+        cp.complete(res)
+        for oc in completions:
+            if oc is not c and not oc.test():
+                oc.complete(res)
+        return 0
+
+    for c in completions:
+        reactor.register_callback(lambda e, c=c: _wait_one(e, c))
+    return cp
+
+
 # Tracking of stepper positions during a homing/probing move
 class StepperPosition:
     def __init__(self, stepper, endstop_name):
@@ -104,6 +142,7 @@ def homing_move(
         probe_pos=False,
         triggered=True,
         check_triggered=True,
+        complete=multi_complete,
     ):
         # Notify start of homing/probing move
         self.printer.send_event("homing:homing_move_begin", self)
@@ -131,7 +170,7 @@ def homing_move(
                 triggered=triggered,
             )
             endstop_triggers.append(wait)
-        all_endstop_trigger = multi_complete(self.printer, endstop_triggers)
+        all_endstop_trigger = complete(self.printer, endstop_triggers)
 
         self.toolhead.dwell(HOMING_START_DELAY)
         # Issue move
@@ -176,15 +215,19 @@ def homing_move(
                 sp.verify_no_probe_skew(haltpos)
         else:
             haltpos = trigpos = movepos
-            over_steps = {
-                sp.stepper_name: sp.halt_pos - sp.trig_pos
-                for sp in self.stepper_positions
-            }
-            steps_moved = {
-                sp.stepper_name: (sp.halt_pos - sp.start_pos)
-                * sp.stepper.get_step_dist()
-                for sp in self.stepper_positions
-            }
+            over_steps = {}
+            steps_moved = {}
+            for sp in self.stepper_positions:
+                halt_pos = sp.halt_pos
+                trig_pos = sp.trig_pos
+                if halt_pos is None or trig_pos is None:
+                    raise self.printer.command_error(
+                        "Internal error: missing endstop position data"
+                    )
+                over_steps[sp.stepper_name] = halt_pos - trig_pos
+                steps_moved[sp.stepper_name] = (
+                    halt_pos - sp.start_pos
+                ) * sp.stepper.get_step_dist()
             filled_steps_moved = {
                 sname: steps_moved.get(sname, 0)
                 for sname in [s.get_name() for s in kin.get_steppers()]
@@ -454,6 +497,30 @@ def probing_move(self, mcu_probe, pos, speed):
             )
         return epos
 
+    def endstop_move(self, endstops, pos, speed, *, complete=multi_complete):
+        hmove = HomingMove(self.printer, endstops)
+        try:
+            epos = hmove.homing_move(
+                pos,
+                speed,
+                probe_pos=True,
+                check_triggered=False,
+                complete=complete,
+            )
+        except self.printer.command_error:
+            if self.printer.is_shutdown():
+                raise self.printer.command_error(
+                    "Endstop move failed due to printer shutdown"
+                )
+            raise
+
+        if hmove.check_no_movement():
+            return epos, MoveResult.ALREADY_AT_ENDSTOP
+        elif all(math.isclose(p, e) for p, e in zip(pos, epos)):
+            return epos, MoveResult.FULL_MOVE
+        else:
+            return epos, MoveResult.HIT_ENDSTOP
+
     def cmd_G28(self, gcmd):
         # Move to origin
         axes = []

klippy/extras/safe_move.py

@@ -0,0 +1,144 @@
+# Safe move
+#
+# Allows safe moves of an axis even when unhomed, as long as an endstop is available in the respective direction.
+# This is used by modules like safe_z_home and dockable_probe during their Z hops, and be used via a Gcode
+# command as well.
+from klippy.extras.homing import MoveResult, any_complete
+
+
+class SafeMove:
+    """Execute safe single-axis moves using directional endstops."""
+
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.homing = self.printer.load_object(config, "homing")
+        gcode = self.printer.lookup_object("gcode")
+        gcode.register_command(
+            "SAFE_MOVE", self.cmd_SAFE_MOVE, desc=self.cmd_SAFE_MOVE_help
+        )
+        self.last_axis = None
+        self.last_dist = None
+        self.last_result = None
+
+    def move(self, toolhead, axis, dist, speed, allow_unsafe=False):
+        """Move on one axis, stopping early if protected endstops trigger."""
+        axis_lower = axis.lower()
+        if axis_lower not in "xyz":
+            raise self.printer.command_error(
+                "SAFE_MOVE: AXIS must be X, Y, or Z"
+            )
+        if dist == 0.0:
+            return
+
+        axis_idx = "xyz".index(axis_lower)
+        positive = dist > 0.0
+
+        kin = toolhead.get_kinematics()
+        endstops = self._get_endstops(kin, axis_idx, positive, allow_unsafe)
+
+        reactor = self.printer.get_reactor()
+        curtime = reactor.monotonic()
+        kin_status = kin.get_status(curtime)
+
+        axis_min = kin_status["axis_minimum"][axis_idx]
+        axis_max = kin_status["axis_maximum"][axis_idx]
+
+        was_unhomed = False
+        position = toolhead.get_position()
+        if axis_lower not in kin_status["homed_axes"]:
+            was_unhomed = True
+            # Assume a safe coordinate so the requested move stays in range.
+            position[axis_idx] = axis_max if dist < 0.0 else axis_min
+            toolhead.set_position(position, homing_axes=[axis_idx])
+            position = toolhead.get_position()
+
+        target_pos = list(position)
+        target_pos[axis_idx] = position[axis_idx] + dist
+        # Clamp moves for homed axes to avoid out-of-range errors.
+        target_pos[axis_idx] = max(
+            axis_min, min(axis_max, target_pos[axis_idx])
+        )
+        if target_pos[axis_idx] == position[axis_idx]:
+            return
+
+        try:
+            if endstops:
+                epos, res = self.homing.endstop_move(
+                    endstops,
+                    target_pos,
+                    speed,
+                    complete=any_complete,
+                )
+                self.last_dist = epos[axis_idx] - position[axis_idx]
+                self.last_result = res
+
+                if res == MoveResult.ALREADY_AT_ENDSTOP:
+                    raise self.printer.command_error(
+                        "Toolhead is already at endstop - unsafe to continue."
+                    )
+            elif allow_unsafe:
+                move_cmd = [None, None, None, None]
+                move_cmd[axis_idx] = target_pos[axis_idx]
+                toolhead.manual_move(move_cmd, speed)
+
+                self.last_dist = dist
+                self.last_result = MoveResult.FULL_MOVE
+            else:
+                raise self.printer.command_error(
+                    "SAFE_MOVE: No endstop protects axis %s in the %s direction"
+                    % (
+                        axis.upper(),
+                        "positive" if positive else "negative",
+                    )
+                )
+
+            self.last_axis = axis_lower
+        finally:
+            if was_unhomed:
+                kin.clear_homing_state([axis_idx])
+
+    def _get_endstops(self, kin, axis_idx, positive, allow_unsafe):
+        endstops = kin.get_endstops_for_safe_move(axis_idx, positive)
+        if endstops is None:
+            if allow_unsafe:
+                return []
+            raise self.printer.command_error(
+                f"SAFE_MOVE: kinematics do not support axis {'XYZ'[axis_idx]} in the {'positive' if positive else 'negative'} direction"
+            )
+        if len(endstops) == 0 and not allow_unsafe:
+            raise self.printer.command_error(
+                f"SAFE_MOVE: No endstops configured for axis {'XYZ'[axis_idx]} in the {'positive' if positive else 'negative'} direction"
+            )
+        return endstops
+
+    cmd_SAFE_MOVE_help = "Perform a safe axis move"
+
+    def cmd_SAFE_MOVE(self, gcmd):
+        axis = gcmd.get("AXIS", None)
+        if axis is None:
+            raise gcmd.error("AXIS must be specified")
+
+        dist = gcmd.get_float("DIST")
+        if dist == 0.0:
+            return
+        speed = gcmd.get_float("SPEED", above=0.0)
+        allow_unsafe = gcmd.get_int("ALLOW_UNSAFE", 0)
+
+        toolhead = self.printer.lookup_object("toolhead")
+        try:
+            self.move(
+                toolhead, axis, dist, speed, allow_unsafe=bool(allow_unsafe)
+            )
+        except self.printer.command_error as err:
+            raise gcmd.error(str(err))
+
+    def get_status(self, eventtime):
+        return {
+            "last_axis": self.last_axis,
+            "last_dist": self.last_dist,
+            "last_result": self.last_result,
+        }
+
+
+def load_config(config):
+    return SafeMove(config)