safe_z_home: Hop only if necessary and add option to move xy back

Once a hop is performed, it will only be re-issued if the z-axis has been
moved in the meantime. Usually it is only moved by a z-homing so doing so
will cause safe_z_home to do the hop on the next homing action.

When z-axis is homed, x and y positions are known. When setting this
boolean option, these are set back to their last positions.

Whenever a hop is specified it is re-issued after the Z axis has been
homed. This is especially necessary when a pressure-based probe is used.
Also, the module decides if a hop is necessary, based on either a known
Z position or a flag that is set whenever the motors are disabled.

Signed-off-by: Nils Friedchen <Nils.Friedchen@googlemail.com>

3 files changed, 47 insertions, 16 deletions

diff --git a/config/example-extras.cfg b/config/example-extras.cfg
index 17af0acc..d18d777c 100644
--- a/config/example-extras.cfg
+++ b/config/example-extras.cfg
@@ -313,10 +313,17 @@
 #z_hop: 0.0
 #   Lift the Z axis prior to homing. This is applied to any homing command,
 #   even if it doesn't home the Z axis. If the Z axis is already homed and
-#   the zhop would exceed the printer limits, the zhop is ignored.
+#   the zhop would exceed the printer limits, the zhop is ignored. If a lift
+#   has already been performed or the Z axis is known to be equally or higher
+#   than this distance, the zhop is ignored. After homing Z completed, the
+#   printhead is lifted to zhop, respecting the probe's z_offset.
 #   The default is 0.0mm.
 #z_hop_speed: 20.0
 #   Speed at which the Z axis is lifted prior to homing. The default is 20mm/s.
+#move_to_previous: True
+#   When set to True, xy are reset to their previous positions after z homing.
+#   The default is True.
+
 
 # Homing override. One may use this mechanism to run a series of
 # g-code commands in place of a G28 found in the normal g-code input.
diff --git a/docs/Config_Changes.md b/docs/Config_Changes.md
index 28f20635..d5d3dca3 100644
--- a/docs/Config_Changes.md
+++ b/docs/Config_Changes.md
@@ -6,6 +6,10 @@ All dates in this document are approximate.
 
 # Changes
 
+20190918: The zhop option in [safe_z_homing] is always re-applied
+after Z axis homing completed. This might need users to update custom
+scripts based on this module.
+
 20190806: The SET_NEOPIXEL command has been renamed to SET_LED.
 
 20190726: The mcp4728 digital-to-analog code has changed. The default
diff --git a/klippy/extras/safe_z_home.py b/klippy/extras/safe_z_home.py
index dc3fbf5d..6f52b3ca 100644
--- a/klippy/extras/safe_z_home.py
+++ b/klippy/extras/safe_z_home.py
@@ -9,7 +9,7 @@ class SafeZHoming:
         self.printer = config.get_printer()
         try:
             x_pos, y_pos = config.get("home_xy_position",
-                                            default=",").split(',')
+                                      default=",").split(',')
             self.home_x_pos, self.home_y_pos = float(x_pos), float(y_pos)
         except:
             raise config.error("Unable to parse home_xy_position in %s" % (
@@ -19,36 +19,35 @@ class SafeZHoming:
         self.z_hop_speed = config.getfloat('z_hop_speed', 15., above=0.)
         self.max_z = config.getsection('stepper_z').getfloat('position_max')
         self.speed = config.getfloat('speed', 50.0, above=0.)
+        self.move_to_previous = config.getboolean('move_to_previous', True)
         self.gcode = self.printer.lookup_object('gcode')
         self.gcode.register_command("G28", None)
         self.gcode.register_command("G28", self.cmd_G28)
 
         if config.has_section("homing_override"):
             raise config.error("homing_override and safe_z_homing cannot"
-                                    +" be used simultaneously")
+                               +" be used simultaneously")
 
     def cmd_G28(self, params):
-
         toolhead = self.printer.lookup_object('toolhead')
         kinematics = toolhead.get_kinematics()
 
         # Perform Z Hop if necessary
         if self.z_hop != 0.0:
-            # Check if the zhop would exceed the printer limits
             pos = toolhead.get_position()
             kin_status = kinematics.get_status()
-            if ('Z' in kin_status['homed_axes'] and
-                pos[2] + self.z_hop > self.max_z):
-                self.gcode.respond_info(
-                    "No zhop performed, target Z out of bounds: " +
-                    str(pos[2] + self.z_hop)
-                )
+            # Check if Z axis is homed or has a known position
+            if 'Z' in kin_status['homed_axes']:
+                # Check if the zhop would exceed the printer limits
+                if pos[2] + self.z_hop > self.max_z:
+                    self.gcode.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(pos)
             else:
-                # Perform the Z-Hop
-                toolhead.set_position(pos, homing_axes=[2])
-                pos[2] = pos[2] + self.z_hop
-                toolhead.move(pos, self.z_hop_speed)
-                self.gcode.reset_last_position()
+                self._perform_z_hop(pos)
 
         # Determine which axes we need to home
         if not any([axis in params.keys() for axis in ['X', 'Y', 'Z']]):
@@ -69,6 +68,8 @@ class SafeZHoming:
         if need_z:
             # Move to safe XY homing position
             pos = toolhead.get_position()
+            prev_x = pos[0]
+            prev_y = pos[1]
             if self.home_x_pos:
                 pos[0] = self.home_x_pos
             if self.home_y_pos:
@@ -77,6 +78,25 @@ class SafeZHoming:
             self.gcode.reset_last_position()
             # Home Z
             self.gcode.cmd_G28({'Z': '0'})
+            # Perform Z Hop again for pressure-based probes
+            pos = toolhead.get_position()
+            if self.z_hop:
+                pos[2] = self.z_hop
+                toolhead.move(pos, self.z_hop_speed)
+            # Move XY back to previous positions
+            if self.move_to_previous:
+                pos[0] = prev_x
+                pos[1] = prev_y
+                toolhead.move(pos, self.speed)
+            self.gcode.reset_last_position()
+
+    def _perform_z_hop(self, pos):
+        toolhead = self.printer.lookup_object('toolhead')
+        # Perform the Z-Hop
+        toolhead.set_position(pos, homing_axes=[2])
+        pos[2] = pos[2] + self.z_hop
+        toolhead.move(pos, self.z_hop_speed)
+        self.gcode.reset_last_position()
 
 def load_config(config):
     return SafeZHoming(config)