From 4f75ce695786ae3494c1ee1d1799f0c4566334d2 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Mon, 12 Sep 2016 03:58:40 -0500
Subject: [PATCH] Unify reset_bed_level for linear/non-linear

---
 Marlin/Marlin.h        |  1 -
 Marlin/Marlin_main.cpp | 62 +++++++++++++++++-------------------------
 2 files changed, 25 insertions(+), 38 deletions(-)

diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index 536a8cf79f..d34445ac43 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -220,7 +220,6 @@ void disable_all_steppers();
 void FlushSerialRequestResend();
 void ok_to_send();
 
-void reset_bed_level();
 void kill(const char*);
 
 void quickstop_stepper();
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index d12693ebf0..a1f18224fd 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -1955,10 +1955,6 @@ static void clean_up_after_endstop_or_probe_move() {
     // Prevent stepper_inactive_time from running out and EXTRUDER_RUNOUT_PREVENT from extruding
     refresh_cmd_timeout();
 
-    #if ENABLED(AUTO_BED_LEVELING_LINEAR)
-      planner.bed_level_matrix.set_to_identity();
-    #endif
-
     #if ENABLED(PROBE_DOUBLE_TOUCH)
 
       // Do a first probe at the fast speed
@@ -2152,22 +2148,23 @@ static void clean_up_after_endstop_or_probe_move() {
       }
     }
 
-    /**
-     * Reset calibration results to zero.
-     */
-    void reset_bed_level() {
-      #if ENABLED(DEBUG_LEVELING_FEATURE)
-        if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("reset_bed_level");
-      #endif
-      for (int y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) {
-        for (int x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) {
-          bed_level_grid[x][y] = 0.0;
-        }
-      }
-    }
-
   #endif // DELTA
 
+  /**
+   * Reset calibration results to zero.
+   */
+  void reset_bed_level() {
+    #if ENABLED(DEBUG_LEVELING_FEATURE)
+      if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("reset_bed_level");
+    #endif
+    #if ENABLED(AUTO_BED_LEVELING_LINEAR)
+      planner.bed_level_matrix.set_to_identity();
+    #elif ENABLED(AUTO_BED_LEVELING_NONLINEAR)
+      memset(bed_level_grid, 0, sizeof(bed_level_grid));
+      nonlinear_grid_spacing[X_AXIS] = nonlinear_grid_spacing[Y_AXIS] = 0;
+    #endif
+  }
+
 #endif // AUTO_BED_LEVELING_FEATURE
 
 /**
@@ -2848,11 +2845,7 @@ inline void gcode_G28() {
   stepper.synchronize();
 
   // For auto bed leveling, clear the level matrix
-  #if ENABLED(AUTO_BED_LEVELING_LINEAR)
-    planner.bed_level_matrix.set_to_identity();
-  #endif
-
-  #if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
+  #if ENABLED(AUTO_BED_LEVELING_FEATURE)
     reset_bed_level();
   #endif
 
@@ -3396,11 +3389,9 @@ inline void gcode_G28() {
 
     if (!dryrun) {
 
-      #if ENABLED(AUTO_BED_LEVELING_LINEAR)
-        // Reset the bed_level_matrix because leveling
-        // needs to be done without leveling enabled.
-        planner.bed_level_matrix.set_to_identity();
-      #endif
+      // Reset the bed_level_matrix because leveling
+      // needs to be done without leveling enabled.
+      reset_bed_level();
 
       //
       // Re-orient the current position without leveling
@@ -3408,10 +3399,6 @@ inline void gcode_G28() {
       //
       #if IS_KINEMATIC
 
-        #if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
-          reset_bed_level();
-        #endif
-
         // For DELTA/SCARA we need to apply forward kinematics.
         // This returns raw positions and we remap to the space.
         get_cartesian_from_steppers();
@@ -3757,6 +3744,10 @@ inline void gcode_G28() {
    */
   inline void gcode_G30() {
 
+    #if ENABLED(AUTO_BED_LEVELING_FEATURE)
+      reset_bed_level();
+    #endif
+
     setup_for_endstop_or_probe_move();
 
     // TODO: clear the leveling matrix or the planner will be set incorrectly
@@ -4199,12 +4190,9 @@ inline void gcode_M42() {
     if (verbose_level > 2)
       SERIAL_PROTOCOLLNPGM("Positioning the probe...");
 
-    #if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
-      // we don't do bed level correction in M48 because we want the raw data when we probe
+    // we don't do bed level correction in M48 because we want the raw data when we probe
+    #if ENABLED(AUTO_BED_LEVELING_FEATURE)
       reset_bed_level();
-    #elif ENABLED(AUTO_BED_LEVELING_LINEAR)
-      // we don't do bed level correction in M48 because we want the raw data when we probe
-      planner.bed_level_matrix.set_to_identity();
     #endif
 
     setup_for_endstop_or_probe_move();