From 510ef5d14a901675282d91b72ef5c988e4927e74 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Thu, 29 Sep 2016 00:59:06 -0500
Subject: [PATCH] Standard function to turn bed leveling on/off

---
 Marlin/Marlin_main.cpp | 71 ++++++++++++++++++++++++++++++++----------
 1 file changed, 55 insertions(+), 16 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 2eec56521..14bd1d8a6 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -2132,30 +2132,69 @@ static void clean_up_after_endstop_or_probe_move() {
 
 #endif // HAS_BED_PROBE
 
-#if HAS_ABL
+#if PLANNER_LEVELING
+  /**
+   * Turn bed leveling on or off, fixing the current
+   * position as-needed.
+   *
+   * Disable: Current position = physical position
+   *  Enable: Current position = "unleveled" physical position
+   */
+  void set_bed_leveling_enabled(bool enable=true) {
+    #if ENABLED(MESH_BED_LEVELING)
+
+      if (!enable && mbl.active())
+        current_position[Z_AXIS] +=
+          mbl.get_z(RAW_CURRENT_POSITION(X_AXIS), RAW_CURRENT_POSITION(Y_AXIS)) - (MESH_HOME_SEARCH_Z);
+
+      mbl.set_active(enable && mbl.has_mesh()); // was set_has_mesh(). Is this not correct?
+
+    #elif HAS_ABL
+
+      if (enable != planner.abl_enabled) {
+        planner.abl_enabled = !planner.abl_enabled;
+        if (!planner.abl_enabled)
+          set_current_from_steppers_for_axis(
+            #if ABL_PLANAR
+              ALL_AXES
+            #else
+              Z_AXIS
+            #endif
+          );
+        else
+          planner.unapply_leveling(current_position);
+      }
+
+    #endif
+  }
+
 
   /**
    * Reset calibration results to zero.
-   *
-   * TODO: Proper functions to disable / enable
-   *       bed leveling via a flag, correcting the
-   *       current position in each case.
    */
   void reset_bed_level() {
-    planner.abl_enabled = false;
-    #if ENABLED(DEBUG_LEVELING_FEATURE)
-      if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("reset_bed_level");
-    #endif
-    #if ABL_PLANAR
-      planner.bed_level_matrix.set_to_identity();
-    #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
-      for (uint8_t x = 0; x < ABL_GRID_POINTS_X; x++)
-        for (uint8_t y = 0; y < ABL_GRID_POINTS_Y; y++)
-          bed_level_grid[x][y] = 1000.0;
+    #if ENABLED(MESH_BED_LEVELING)
+      if (mbl.has_mesh()) {
+        set_bed_leveling_enabled(false);
+        mbl.reset();
+        mbl.set_has_mesh(false);
+      }
+    #else
+      planner.abl_enabled = false;
+      #if ENABLED(DEBUG_LEVELING_FEATURE)
+        if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("reset_bed_level");
+      #endif
+      #if ABL_PLANAR
+        planner.bed_level_matrix.set_to_identity();
+      #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
+        for (uint8_t x = 0; x < ABL_GRID_POINTS_X; x++)
+          for (uint8_t y = 0; y < ABL_GRID_POINTS_Y; y++)
+            bed_level_grid[x][y] = 1000.0;
+      #endif
     #endif
   }
 
-#endif // HAS_ABL
+#endif // PLANNER_LEVELING
 
 #if ENABLED(AUTO_BED_LEVELING_BILINEAR)