Fix MBL lift

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];

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);