From 0b6f8f82442a55b60fa1d62116be83a1169b0d8f Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Thu, 7 Jul 2016 12:14:54 -0700
Subject: [PATCH] Apply MIN_Z_HEIGHT_FOR_HOMING raise literally

---
 Marlin/Marlin_main.cpp | 33 +++++++++++++++++++--------------
 1 file changed, 19 insertions(+), 14 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 967f9b3f4c..dbac3f1112 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -2915,22 +2915,27 @@ inline void gcode_G28() {
     #elif defined(MIN_Z_HEIGHT_FOR_HOMING) && MIN_Z_HEIGHT_FOR_HOMING > 0
 
       // Raise Z before homing, if specified
-      destination[Z_AXIS] = (current_position[Z_AXIS] += MIN_Z_HEIGHT_FOR_HOMING);
-      #if ENABLED(DEBUG_LEVELING_FEATURE)
-        if (DEBUGGING(LEVELING)) {
-          SERIAL_ECHOPAIR("Raise Z (before homing) to ", destination[Z_AXIS]);
-          SERIAL_EOL;
-        }
-      #endif
+      float z_dest = home_offset[Z_AXIS] + MIN_Z_HEIGHT_FOR_HOMING;
+      if (z_dest > current_position[Z_AXIS]) {
 
-      feedrate = homing_feedrate[Z_AXIS];
+        #if ENABLED(DEBUG_LEVELING_FEATURE)
+          if (DEBUGGING(LEVELING)) {
+            SERIAL_ECHOPAIR("Raise Z (before homing) to ", z_dest);
+            SERIAL_EOL;
+          }
+        #endif
 
-      #if HAS_BED_PROBE
-        do_blocking_move_to_z(destination[Z_AXIS]);
-      #else
-        line_to_z(destination[Z_AXIS]);
-        stepper.synchronize();
-      #endif
+        feedrate = homing_feedrate[Z_AXIS];
+
+        #if HAS_BED_PROBE
+          do_blocking_move_to_z(z_dest);
+        #else
+          line_to_z(z_dest);
+          stepper.synchronize();
+        #endif
+
+        destination[Z_AXIS] = current_position[Z_AXIS] = z_dest;
+      }
 
     #endif // MIN_Z_HEIGHT_FOR_HOMING