From 0fb507a75dca7e3717f9227cee73fac10f52e11a Mon Sep 17 00:00:00 2001
From: lrpirlet <lrpirlet@gmail.com>
Date: Sat, 23 Jan 2016 15:19:46 +0100
Subject: [PATCH] correct raise_z_for_servo

---
 Marlin/Marlin_main.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 36a3ad693..c698a9694 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -1751,7 +1751,9 @@ static void setup_for_endstop_move() {
 
     void raise_z_for_servo() {
       float zpos = current_position[Z_AXIS], z_dest = Z_RAISE_BEFORE_PROBING;
-      z_dest += axis_known_position[Z_AXIS] ? zprobe_zoffset : zpos;
+      // The zprobe_zoffset is negative any switch below the nozzle, so
+      // multiply by Z_HOME_DIR (-1) to move enough away from bed for the probe
+      z_dest += axis_known_position[Z_AXIS] ? zprobe_zoffset * Z_HOME_DIR : zpos;
       if (zpos < z_dest) do_blocking_move_to_z(z_dest); // also updates current_position
     }