From 14afe1a017610e1530185b40bd2676e45695cbb7 Mon Sep 17 00:00:00 2001
From: Edward Patel <edward.patel@memention.com>
Date: Thu, 24 Mar 2016 23:35:51 +0100
Subject: [PATCH] Move to Z=0 for G28 when using Manual Bed Leveling

copy of https://github.com/MarlinFirmware/MarlinDev/pull/199/commits/406992f9dd44655b33dab46e0cb4fd4a29be1695
---
 Marlin/Marlin_main.cpp | 17 ++++++++---------
 Marlin/ultralcd.cpp    |  1 +
 2 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 1c162666d..c88a9532a 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -2381,7 +2381,7 @@ inline void gcode_G28() {
     #endif
   #endif
 
-  // For manual bed leveling deactivate the matrix temporarily
+  // For mesh bed leveling deactivate the mesh calculations, will be turned on again when homing all axis
   #if ENABLED(MESH_BED_LEVELING)
     uint8_t mbl_was_active = mbl.active;
     mbl.active = 0;
@@ -2680,18 +2680,17 @@ inline void gcode_G28() {
     enable_endstops(false);
   #endif
 
-  // For manual leveling move back to 0,0
+  // For mesh leveling move back to Z=0
   #if ENABLED(MESH_BED_LEVELING)
-    if (mbl_was_active) {
-      current_position[X_AXIS] = mbl.get_x(0);
-      current_position[Y_AXIS] = mbl.get_y(0);
-      set_destination_to_current();
-      feedrate = homing_feedrate[X_AXIS];
-      line_to_destination();
-      st_synchronize();
+    if (mbl_was_active && home_all_axis) {
       current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
       sync_plan_position();
       mbl.active = 1;
+      current_position[Z_AXIS] = 0.0;
+      set_destination_to_current();
+      feedrate = homing_feedrate[Z_AXIS];
+      line_to_destination();
+      st_synchronize();
       #if ENABLED(DEBUG_LEVELING_FEATURE)
         if (marlin_debug_flags & DEBUG_LEVELING) {
           print_xyz("mbl_was_active > current_position", current_position);
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index 38db6ed17..56f12a17b 100644
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -2434,6 +2434,7 @@ char* ftostr52(const float& x) {
         if (_lcd_level_bed_position == (MESH_NUM_X_POINTS) * (MESH_NUM_Y_POINTS)) {
           current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
           line_to_current(Z_AXIS);
+          st_synchronize();
           mbl.active = 1;
           enqueue_and_echo_commands_P(PSTR("G28"));
           lcd_return_to_status();