From 234987ee2f6ea001fd4fe934ac24cc8057820b88 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sat, 16 Apr 2016 14:17:06 -0700
Subject: [PATCH] Fix MBL lift

---
 Marlin/Marlin_main.cpp | 10 ++++++----
 Marlin/ultralcd.cpp    | 10 ++++++----
 2 files changed, 12 insertions(+), 8 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index c0acebf8d..96eff9d3d 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -2847,10 +2847,12 @@ inline void gcode_G28() {
     saved_feedrate = feedrate;
     feedrate = homing_feedrate[X_AXIS];
 
-    #if MIN_Z_HEIGHT_FOR_HOMING > 0
-      current_position[Z_AXIS] = MESH_HOME_SEARCH_Z + MIN_Z_HEIGHT_FOR_HOMING;
-      line_to_current_position();
-    #endif
+    current_position[Z_AXIS] = MESH_HOME_SEARCH_Z
+      #if MIN_Z_HEIGHT_FOR_HOMING > 0
+        + MIN_Z_HEIGHT_FOR_HOMING
+      #endif
+    ;
+    line_to_current_position();
 
     current_position[X_AXIS] = x + home_offset[X_AXIS];
     current_position[Y_AXIS] = y + home_offset[Y_AXIS];
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index 5d5845cfb..31d5d3165 100644
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -893,10 +893,12 @@ void lcd_cooldown() {
   // Z position will be restored with the final action, a G28
   inline void _mbl_goto_xy(float x, float y) {
     mbl_wait_for_move = true;
-    #if MIN_Z_HEIGHT_FOR_HOMING > 0
-      current_position[Z_AXIS] += MIN_Z_HEIGHT_FOR_HOMING;
-      line_to_current(Z_AXIS);
-    #endif
+    current_position[Z_AXIS] = MESH_HOME_SEARCH_Z
+      #if MIN_Z_HEIGHT_FOR_HOMING > 0
+        + MIN_Z_HEIGHT_FOR_HOMING
+      #endif
+    ;
+    line_to_current(Z_AXIS);
     current_position[X_AXIS] = x + home_offset[X_AXIS];
     current_position[Y_AXIS] = y + home_offset[Y_AXIS];
     line_to_current(manual_feedrate[X_AXIS] <= manual_feedrate[Y_AXIS] ? X_AXIS : Y_AXIS);