From 009a6ddd39aeadc8a22a6e377100ba10e77a3b00 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Fri, 24 Mar 2017 06:28:30 -0500
Subject: [PATCH] Adjust bilinear grid in M851

---
 Marlin/Marlin_main.cpp | 14 ++++++++++++++
 1 file changed, 14 insertions(+)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 5a9d6fb784..9ac963d1bd 100755
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -7448,6 +7448,20 @@ inline void gcode_M503() {
     if (code_seen('Z')) {
       float value = code_value_axis_units(Z_AXIS);
       if (Z_PROBE_OFFSET_RANGE_MIN <= value && value <= Z_PROBE_OFFSET_RANGE_MAX) {
+
+        #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 < ABL_GRID_MAX_POINTS_X; x++)
+              for (uint8_t y = 0; y < ABL_GRID_MAX_POINTS_Y; y++)
+                bed_level_grid[x][y] += diff;
+          }
+          #if ENABLED(ABL_BILINEAR_SUBDIVISION)
+            bed_level_virt_interpolate();
+          #endif
+        #endif
+
         zprobe_zoffset = value;
         SERIAL_ECHO(zprobe_zoffset);
       }