From cf61ca301b5aa15cf6807ac501f576a77c331c4f Mon Sep 17 00:00:00 2001
From: Florian Heilmann <Florian.Heilmann@gmx.net>
Date: Tue, 18 May 2021 17:31:07 +0100
Subject: [PATCH] safe_z_home: Simplify logic and fix edge case

Signed-off-by: Florian Heilmann <Florian.Heilmann@gmx.net>
---
 klippy/extras/safe_z_home.py | 32 +++++++++++++-------------------
 1 file changed, 13 insertions(+), 19 deletions(-)

diff --git a/klippy/extras/safe_z_home.py b/klippy/extras/safe_z_home.py
index 6dd2f292..0a684f26 100644
--- a/klippy/extras/safe_z_home.py
+++ b/klippy/extras/safe_z_home.py
@@ -34,22 +34,23 @@ class SafeZHoming:
 
         # Perform Z Hop if necessary
         if self.z_hop != 0.0:
-            # Check if Z axis is homed or has a known position
+            # Check if Z axis is homed and its last known position
             curtime = self.printer.get_reactor().monotonic()
             kin_status = toolhead.get_kinematics().get_status(curtime)
-            if 'z' in kin_status['homed_axes']:
-                # Check if the zhop would exceed the printer limits
-                pos = toolhead.get_position()
-                if pos[2] + self.z_hop > self.max_z:
-                    gcmd.respond_info(
-                        "No zhop performed, target Z out of bounds: " +
-                        str(pos[2] + self.z_hop))
-                elif pos[2] < self.z_hop:
-                    self._perform_z_hop()
-            else:
-                self._perform_z_hop()
+            pos = toolhead.get_position()
+
+            if 'z' not in kin_status['homed_axes']:
+                # Always perform the z_hop if the Z axis is not homed
+                pos[2] = 0
+                toolhead.set_position(pos, homing_axes=[2])
+                toolhead.manual_move([None, None, self.z_hop],
+                                     self.z_hop_speed)
                 if hasattr(toolhead.get_kinematics(), "note_z_not_homed"):
                     toolhead.get_kinematics().note_z_not_homed()
+            elif pos[2] < self.z_hop:
+                # If the Z axis is homed, and below z_hop, lift it to z_hop
+                toolhead.manual_move([None, None, self.z_hop],
+                                     self.z_hop_speed)
 
         # Determine which axes we need to home
         need_x, need_y, need_z = [gcmd.get(axis, None) is not None
@@ -88,12 +89,5 @@ class SafeZHoming:
             if self.move_to_previous:
                 toolhead.manual_move(prevpos[:2], self.speed)
 
-    def _perform_z_hop(self):
-        toolhead = self.printer.lookup_object('toolhead')
-        # Perform the Z-Hop
-        pos = toolhead.get_position()
-        toolhead.set_position(pos, homing_axes=[2])
-        toolhead.manual_move([None, None, pos[2]+self.z_hop], self.z_hop_speed)
-
 def load_config(config):
     return SafeZHoming(config)