From c7d90dc5ece06cd4e0e7788d605626679edcded9 Mon Sep 17 00:00:00 2001
From: Edward Patel <edward.patel@memention.com>
Date: Tue, 17 May 2016 23:38:08 +0200
Subject: [PATCH] Add option to configure G28 all axis resting position for MBL

---
 Marlin/Configuration.h |  2 ++
 Marlin/Marlin_main.cpp | 14 ++++++++------
 2 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index 15bfd5ef47..3dadaedc1f 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -525,6 +525,8 @@ const bool Z_MIN_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the l
   #define MESH_NUM_Y_POINTS 3
   #define MESH_HOME_SEARCH_Z 4  // Z after Home, bed somewhere below but above 0.0.
 
+  //#define MESH_G28_REST_ORIGIN // Enable to make 'G28' all axis to rest at origin [0,0,0]
+
   //#define MANUAL_BED_LEVELING  // Add display menu option for bed leveling.
 
   #if ENABLED(MANUAL_BED_LEVELING)
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 0ebfeff040..bad4b4c332 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -2918,17 +2918,19 @@ inline void gcode_G28() {
     #endif
   #endif
 
-  // For mesh leveling move back to Z=0
+  // Enable mesh leveling again
   #if ENABLED(MESH_BED_LEVELING)
     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();
-      stepper.synchronize();
+      #if ENABLED(MESH_G28_REST_ORIGIN)
+        current_position[Z_AXIS] = 0.0;
+        set_destination_to_current();
+        feedrate = homing_feedrate[Z_AXIS];
+        line_to_destination();
+        stepper.synchronize();
+      #endif
       #if ENABLED(DEBUG_LEVELING_FEATURE)
         if (DEBUGGING(LEVELING)) DEBUG_POS("mbl_was_active", current_position);
       #endif