From 60ac41a32c3b3c62f65ce380201c7ea0337498ff Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Tue, 11 Apr 2017 05:05:23 -0500
Subject: [PATCH] Add code to handle changes to zprobe_zoffset

---
 Marlin/Marlin.h                |  1 +
 Marlin/Marlin_main.cpp         | 65 +++++++++++++++++++---------------
 Marlin/configuration_store.cpp |  8 +++--
 Marlin/ultralcd.cpp            |  7 ++--
 4 files changed, 47 insertions(+), 34 deletions(-)

diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index d58cc6e27..e18741a38 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -317,6 +317,7 @@ float code_value_temp_diff();
 
 #if HAS_BED_PROBE
   extern float zprobe_zoffset;
+  void refresh_zprobe_zoffset(const bool no_babystep=false);
   #define DEPLOY_PROBE() set_probe_deployed(true)
   #define STOW_PROBE() set_probe_deployed(false)
 #endif
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 6f593812e..b1a94a58e 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -7968,46 +7968,53 @@ inline void gcode_M503() {
 
 #if HAS_BED_PROBE
 
-  inline void gcode_M851() {
+  void refresh_zprobe_zoffset(const bool no_babystep/*=false*/) {
+    static float last_zoffset = NAN;
 
-    SERIAL_ECHO_START;
-    SERIAL_ECHOPGM(MSG_ZPROBE_ZOFFSET);
-    SERIAL_CHAR(' ');
+    if (!isnan(last_zoffset)) {
 
-    if (code_seen('Z')) {
-      float value = code_value_axis_units(Z_AXIS);
-      if (WITHIN(value, Z_PROBE_OFFSET_RANGE_MIN, Z_PROBE_OFFSET_RANGE_MAX)) {
+      #if ENABLED(AUTO_BED_LEVELING_BILINEAR) || ENABLED(BABYSTEPPING)
+        const float diff = zprobe_zoffset - last_zoffset;
+      #endif
 
-        #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
-          // Correct bilinear grid for new probe offset
-          const float diff = value - zprobe_zoffset;
-          if (diff) {
-            for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
-              for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
-                bed_level_grid[x][y] -= diff;
-          }
-          #if ENABLED(ABL_BILINEAR_SUBDIVISION)
-            bed_level_virt_interpolate();
-          #endif
+      #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+        // Correct bilinear grid for new probe offset
+        if (diff) {
+          for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
+            for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
+              bed_level_grid[x][y] -= diff;
+        }
+        #if ENABLED(ABL_BILINEAR_SUBDIVISION)
+          bed_level_virt_interpolate();
         #endif
+      #endif
 
-        #if ENABLED(BABYSTEPPING)
-          if (planner.abl_enabled)
-            thermalManager.babystep_axis(Z_AXIS, lround(-(value - zprobe_zoffset) * planner.axis_steps_per_mm[Z_AXIS]));
-        #endif
+      #if ENABLED(BABYSTEPPING)
+        if (!no_babystep && planner.abl_enabled)
+          thermalManager.babystep_axis(Z_AXIS, -lround(diff * planner.axis_steps_per_mm[Z_AXIS]));
+      #else
+        UNUSED(no_babystep);
+      #endif
+    }
+
+    last_zoffset = zprobe_zoffset;
+  }
 
+  inline void gcode_M851() {
+    SERIAL_ECHO_START;
+    SERIAL_ECHOPGM(MSG_ZPROBE_ZOFFSET " ");
+    if (code_seen('Z')) {
+      const float value = code_value_axis_units(Z_AXIS);
+      if (WITHIN(value, Z_PROBE_OFFSET_RANGE_MIN, Z_PROBE_OFFSET_RANGE_MAX)) {
         zprobe_zoffset = value;
+        refresh_zprobe_zoffset();
         SERIAL_ECHO(zprobe_zoffset);
       }
-      else {
-        SERIAL_ECHOPAIR(MSG_Z_MIN, Z_PROBE_OFFSET_RANGE_MIN);
-        SERIAL_CHAR(' ');
-        SERIAL_ECHOPAIR(MSG_Z_MAX, Z_PROBE_OFFSET_RANGE_MAX);
-      }
+      else
+        SERIAL_ECHOPGM(MSG_Z_MIN " " STRINGIFY(Z_PROBE_OFFSET_RANGE_MIN) " " MSG_Z_MAX " " STRINGIFY(Z_PROBE_OFFSET_RANGE_MAX));
     }
-    else {
+    else
       SERIAL_ECHOPAIR(": ", zprobe_zoffset);
-    }
 
     SERIAL_EOL;
   }
diff --git a/Marlin/configuration_store.cpp b/Marlin/configuration_store.cpp
index 9ada6f536..72cf680a0 100644
--- a/Marlin/configuration_store.cpp
+++ b/Marlin/configuration_store.cpp
@@ -216,6 +216,10 @@ void MarlinSettings::postprocess() {
       //#endif
     );
   #endif
+
+  #if HAS_BED_PROBE
+    refresh_zprobe_zoffset();
+  #endif
 }
 
 #if ENABLED(EEPROM_SETTINGS)
@@ -344,7 +348,7 @@ void MarlinSettings::postprocess() {
     #endif // MESH_BED_LEVELING
 
     #if !HAS_BED_PROBE
-      float zprobe_zoffset = 0;
+      const float zprobe_zoffset = 0;
     #endif
     EEPROM_WRITE(zprobe_zoffset);
 
@@ -685,7 +689,7 @@ void MarlinSettings::postprocess() {
       #endif // MESH_BED_LEVELING
 
       #if !HAS_BED_PROBE
-        float zprobe_zoffset = 0;
+        float zprobe_zoffset;
       #endif
       EEPROM_READ(zprobe_zoffset);
 
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index 908891039..3ae8a313e 100755
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -863,11 +863,12 @@ void kill_screen(const char* lcd_msg) {
 
           const float new_zoffset = zprobe_zoffset + steps_to_mm[Z_AXIS] * babystep_increment;
           if (WITHIN(new_zoffset, Z_PROBE_OFFSET_RANGE_MIN, Z_PROBE_OFFSET_RANGE_MAX)) {
-            
+
             if (planner.abl_enabled)
               thermalManager.babystep_axis(Z_AXIS, babystep_increment);
-      
+
             zprobe_zoffset = new_zoffset;
+            refresh_zprobe_zoffset(true);
             lcdDrawUpdate = LCDVIEW_REDRAW_NOW;
           }
         }
@@ -2412,7 +2413,7 @@ void kill_screen(const char* lcd_msg) {
       #if ENABLED(BABYSTEPPING)
         MENU_ITEM(submenu, MSG_ZPROBE_ZOFFSET, lcd_babystep_zoffset);
       #else
-        MENU_ITEM_EDIT(float32, MSG_ZPROBE_ZOFFSET, &zprobe_zoffset, Z_PROBE_OFFSET_RANGE_MIN, Z_PROBE_OFFSET_RANGE_MAX);
+        MENU_ITEM_EDIT_CALLBACK(float32, MSG_ZPROBE_ZOFFSET, &zprobe_zoffset, Z_PROBE_OFFSET_RANGE_MIN, Z_PROBE_OFFSET_RANGE_MAX, refresh_zprobe_zoffset);
       #endif
     #endif
     // Manual bed leveling, Bed Z: