From c606ed447adc4ed27962c931b44a4f72b5d6fc8f Mon Sep 17 00:00:00 2001
From: Edward Patel <edward.patel@memention.com>
Date: Thu, 24 Mar 2016 22:16:09 +0100
Subject: [PATCH 1/2] Add "G29 S4" to fine tune Z level for Mesh Bed Leveling.

Also add mbl.z_offset to the EEPROM, bumping the version to V23.
---
 Marlin/Marlin_main.cpp         | 24 ++++++++--
 Marlin/configuration_store.cpp | 87 ++++++++++++++++++----------------
 Marlin/language_en.h           |  3 ++
 Marlin/mesh_bed_leveling.cpp   |  1 +
 Marlin/mesh_bed_leveling.h     |  3 +-
 Marlin/ultralcd.cpp            |  9 ++++
 6 files changed, 81 insertions(+), 46 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index edc36914c3f..1c162666d22 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -2717,7 +2717,7 @@ inline void gcode_G28() {
 
 #if ENABLED(MESH_BED_LEVELING)
 
-  enum MeshLevelingState { MeshReport, MeshStart, MeshNext, MeshSet };
+  enum MeshLevelingState { MeshReport, MeshStart, MeshNext, MeshSet, MeshSetZOffset };
 
   /**
    * G29: Mesh-based Z probe, probes a grid and produces a
@@ -2729,21 +2729,22 @@ inline void gcode_G28() {
    *  S1              Start probing mesh points
    *  S2              Probe the next mesh point
    *  S3 Xn Yn Zn.nn  Manually modify a single point
+   *  S4 Zn.nn        Set z offset. Positive away from bed, negative closer to bed.
    *
    * The S0 report the points as below
    *
-   *  +----> X-axis
+   *  +----> X-axis  1-n
    *  |
    *  |
-   *  v Y-axis
+   *  v Y-axis  1-n
    *
    */
   inline void gcode_G29() {
 
     static int probe_point = -1;
     MeshLevelingState state = code_seen('S') ? (MeshLevelingState)code_value_short() : MeshReport;
-    if (state < 0 || state > 3) {
-      SERIAL_PROTOCOLLNPGM("S out of range (0-3).");
+    if (state < 0 || state > 4) {
+      SERIAL_PROTOCOLLNPGM("S out of range (0-4).");
       return;
     }
 
@@ -2759,6 +2760,8 @@ inline void gcode_G28() {
           SERIAL_PROTOCOL(MESH_NUM_Y_POINTS);
           SERIAL_PROTOCOLPGM("\nZ search height: ");
           SERIAL_PROTOCOL(MESH_HOME_SEARCH_Z);
+          SERIAL_PROTOCOLPGM("\nZ offset: ");
+          SERIAL_PROTOCOL_F(mbl.z_offset, 5);
           SERIAL_PROTOCOLLNPGM("\nMeasured points:");
           for (int y = 0; y < MESH_NUM_Y_POINTS; y++) {
             for (int x = 0; x < MESH_NUM_X_POINTS; x++) {
@@ -2849,6 +2852,17 @@ inline void gcode_G28() {
           return;
         }
         mbl.z_values[iy][ix] = z;
+        break;
+
+      case MeshSetZOffset:
+        if (code_seen('Z')) {
+          z = code_value();
+        } 
+        else {
+          SERIAL_PROTOCOLPGM("Z not entered.\n");
+          return;
+        }
+        mbl.z_offset = z;
 
     } // switch(state)
   }
diff --git a/Marlin/configuration_store.cpp b/Marlin/configuration_store.cpp
index d9929e10a1d..83bd8450bd9 100644
--- a/Marlin/configuration_store.cpp
+++ b/Marlin/configuration_store.cpp
@@ -36,10 +36,10 @@
  *
  */
 
-#define EEPROM_VERSION "V22"
+#define EEPROM_VERSION "V23"
 
 /**
- * V21 EEPROM Layout:
+ * V23 EEPROM Layout:
  *
  *  100  Version (char x4)
  *
@@ -59,62 +59,65 @@
  *
  * Mesh bed leveling:
  *  200  M420 S    active (bool)
- *  201            mesh_num_x (uint8 as set in firmware)
- *  202            mesh_num_y (uint8 as set in firmware)
- *  203  M421 XYZ  z_values[][] (float x9, by default)
- *  239  M851      zprobe_zoffset (float)
+ *  201            z_offset (float) (added in V23)
+ *  205            mesh_num_x (uint8 as set in firmware)
+ *  206            mesh_num_y (uint8 as set in firmware)
+ *  207  M421 XYZ  z_values[][] (float x9, by default)
+ *
+ * AUTO BED LEVELING
+ *  243  M851      zprobe_zoffset (float)
  *
  * DELTA:
- *  243  M666 XYZ  endstop_adj (float x3)
- *  255  M665 R    delta_radius (float)
- *  259  M665 L    delta_diagonal_rod (float)
- *  263  M665 S    delta_segments_per_second (float)
- *  267  M665 A    delta_diagonal_rod_trim_tower_1 (float)
- *  271  M665 B    delta_diagonal_rod_trim_tower_2 (float)
- *  275  M665 C    delta_diagonal_rod_trim_tower_3 (float)
+ *  247  M666 XYZ  endstop_adj (float x3)
+ *  259  M665 R    delta_radius (float)
+ *  263  M665 L    delta_diagonal_rod (float)
+ *  267  M665 S    delta_segments_per_second (float)
+ *  271  M665 A    delta_diagonal_rod_trim_tower_1 (float)
+ *  275  M665 B    delta_diagonal_rod_trim_tower_2 (float)
+ *  279  M665 C    delta_diagonal_rod_trim_tower_3 (float)
  *
  * Z_DUAL_ENDSTOPS:
- *  279  M666 Z    z_endstop_adj (float)
+ *  283  M666 Z    z_endstop_adj (float)
  *
  * ULTIPANEL:
- *  283  M145 S0 H plaPreheatHotendTemp (int)
- *  285  M145 S0 B plaPreheatHPBTemp (int)
- *  287  M145 S0 F plaPreheatFanSpeed (int)
- *  289  M145 S1 H absPreheatHotendTemp (int)
- *  291  M145 S1 B absPreheatHPBTemp (int)
- *  293  M145 S1 F absPreheatFanSpeed (int)
+ *  287  M145 S0 H plaPreheatHotendTemp (int)
+ *  289  M145 S0 B plaPreheatHPBTemp (int)
+ *  291  M145 S0 F plaPreheatFanSpeed (int)
+ *  293  M145 S1 H absPreheatHotendTemp (int)
+ *  295  M145 S1 B absPreheatHPBTemp (int)
+ *  297  M145 S1 F absPreheatFanSpeed (int)
  *
  * PIDTEMP:
- *  295  M301 E0 PIDC  Kp[0], Ki[0], Kd[0], Kc[0] (float x4)
- *  311  M301 E1 PIDC  Kp[1], Ki[1], Kd[1], Kc[1] (float x4)
- *  327  M301 E2 PIDC  Kp[2], Ki[2], Kd[2], Kc[2] (float x4)
- *  343  M301 E3 PIDC  Kp[3], Ki[3], Kd[3], Kc[3] (float x4)
- *  359  M301 L        lpq_len (int)
+ *  299  M301 E0 PIDC  Kp[0], Ki[0], Kd[0], Kc[0] (float x4)
+ *  315  M301 E1 PIDC  Kp[1], Ki[1], Kd[1], Kc[1] (float x4)
+ *  331  M301 E2 PIDC  Kp[2], Ki[2], Kd[2], Kc[2] (float x4)
+ *  347  M301 E3 PIDC  Kp[3], Ki[3], Kd[3], Kc[3] (float x4)
+ *  363  M301 L        lpq_len (int)
  *
  * PIDTEMPBED:
- *  361  M304 PID  bedKp, bedKi, bedKd (float x3)
+ *  365  M304 PID  bedKp, bedKi, bedKd (float x3)
  *
  * DOGLCD:
- *  373  M250 C    lcd_contrast (int)
+ *  377  M250 C    lcd_contrast (int)
  *
  * SCARA:
- *  375  M365 XYZ  axis_scaling (float x3)
+ *  379  M365 XYZ  axis_scaling (float x3)
  *
  * FWRETRACT:
- *  387  M209 S    autoretract_enabled (bool)
- *  388  M207 S    retract_length (float)
- *  392  M207 W    retract_length_swap (float)
- *  396  M207 F    retract_feedrate (float)
- *  400  M207 Z    retract_zlift (float)
- *  404  M208 S    retract_recover_length (float)
- *  408  M208 W    retract_recover_length_swap (float)
- *  412  M208 F    retract_recover_feedrate (float)
+ *  391  M209 S    autoretract_enabled (bool)
+ *  392  M207 S    retract_length (float)
+ *  396  M207 W    retract_length_swap (float)
+ *  400  M207 F    retract_feedrate (float)
+ *  404  M207 Z    retract_zlift (float)
+ *  408  M208 S    retract_recover_length (float)
+ *  412  M208 W    retract_recover_length_swap (float)
+ *  416  M208 F    retract_recover_feedrate (float)
  *
  * Volumetric Extrusion:
- *  416  M200 D    volumetric_enabled (bool)
- *  417  M200 T D  filament_size (float x4) (T0..3)
+ *  420  M200 D    volumetric_enabled (bool)
+ *  421  M200 T D  filament_size (float x4) (T0..3)
  *
- *  433  This Slot is Available!
+ *  437  This Slot is Available!
  *
  */
 #include "Marlin.h"
@@ -192,15 +195,17 @@ void Config_StoreSettings()  {
     mesh_num_x = MESH_NUM_X_POINTS;
     mesh_num_y = MESH_NUM_Y_POINTS;
     EEPROM_WRITE_VAR(i, mbl.active);
+    EEPROM_WRITE_VAR(i, mbl.z_offset);
     EEPROM_WRITE_VAR(i, mesh_num_x);
     EEPROM_WRITE_VAR(i, mesh_num_y);
     EEPROM_WRITE_VAR(i, mbl.z_values);
   #else
     uint8_t dummy_uint8 = 0;
+    dummy = 0.0f;
     EEPROM_WRITE_VAR(i, dummy_uint8);
+    EEPROM_WRITE_VAR(i, dummy);
     EEPROM_WRITE_VAR(i, mesh_num_x);
     EEPROM_WRITE_VAR(i, mesh_num_y);
-    dummy = 0.0f;
     for (uint8_t q = 0; q < mesh_num_x * mesh_num_y; q++) EEPROM_WRITE_VAR(i, dummy);
   #endif // MESH_BED_LEVELING
 
@@ -366,10 +371,12 @@ void Config_RetrieveSettings() {
 
     uint8_t dummy_uint8 = 0, mesh_num_x = 0, mesh_num_y = 0;
     EEPROM_READ_VAR(i, dummy_uint8);
+    EEPROM_READ_VAR(i, dummy);
     EEPROM_READ_VAR(i, mesh_num_x);
     EEPROM_READ_VAR(i, mesh_num_y);
     #if ENABLED(MESH_BED_LEVELING)
       mbl.active = dummy_uint8;
+      mbl.z_offset = dummy;
       if (mesh_num_x == MESH_NUM_X_POINTS && mesh_num_y == MESH_NUM_Y_POINTS) {
         EEPROM_READ_VAR(i, mbl.z_values);
       } else {
diff --git a/Marlin/language_en.h b/Marlin/language_en.h
index b4eb6d5cdf9..5cb4f95c585 100644
--- a/Marlin/language_en.h
+++ b/Marlin/language_en.h
@@ -169,6 +169,9 @@
 #ifndef MSG_SPEED
   #define MSG_SPEED                           "Speed"
 #endif
+#ifndef MSG_BED_Z
+  #define MSG_BED_Z                           "Bed Z"
+#endif
 #ifndef MSG_NOZZLE
   #define MSG_NOZZLE                          "Nozzle"
 #endif
diff --git a/Marlin/mesh_bed_leveling.cpp b/Marlin/mesh_bed_leveling.cpp
index 462c6c6866e..d35ed3e4d91 100644
--- a/Marlin/mesh_bed_leveling.cpp
+++ b/Marlin/mesh_bed_leveling.cpp
@@ -30,6 +30,7 @@
 
   void mesh_bed_leveling::reset() {
     active = 0;
+    z_offset = 0;
     for (int y = 0; y < MESH_NUM_Y_POINTS; y++)
       for (int x = 0; x < MESH_NUM_X_POINTS; x++)
         z_values[y][x] = 0;
diff --git a/Marlin/mesh_bed_leveling.h b/Marlin/mesh_bed_leveling.h
index d531ff5162e..1df1c98995d 100644
--- a/Marlin/mesh_bed_leveling.h
+++ b/Marlin/mesh_bed_leveling.h
@@ -30,6 +30,7 @@
   class mesh_bed_leveling {
   public:
     uint8_t active;
+    float z_offset;
     float z_values[MESH_NUM_Y_POINTS][MESH_NUM_X_POINTS];
 
     mesh_bed_leveling();
@@ -70,7 +71,7 @@
       float z0 = calc_z0(y0,
                          get_y(y_index), z1,
                          get_y(y_index + 1), z2);
-      return z0;
+      return z0 + z_offset;
     }
   };
 
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index 2dd247a8dd4..38db6ed17f3 100644
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -599,6 +599,11 @@ static void lcd_tune_menu() {
   //
   MENU_ITEM_EDIT(int3, MSG_SPEED, &feedrate_multiplier, 10, 999);
 
+  // Manual bed leveling, Bed Z:
+  #if ENABLED(MANUAL_BED_LEVELING)
+    MENU_ITEM_EDIT(float43, MSG_BED_Z, &mbl.z_offset, -1, 1);
+  #endif
+
   //
   // Nozzle:
   // Nozzle [1-4]:
@@ -1333,6 +1338,10 @@ static void lcd_control_motion_menu() {
   #if ENABLED(AUTO_BED_LEVELING_FEATURE)
     MENU_ITEM_EDIT(float32, MSG_ZPROBE_ZOFFSET, &zprobe_zoffset, Z_PROBE_OFFSET_RANGE_MIN, Z_PROBE_OFFSET_RANGE_MAX);
   #endif
+  // Manual bed leveling, Bed Z:
+  #if ENABLED(MANUAL_BED_LEVELING)
+    MENU_ITEM_EDIT(float43, MSG_BED_Z, &mbl.z_offset, -1, 1);
+  #endif
   MENU_ITEM_EDIT(float5, MSG_ACC, &acceleration, 10, 99000);
   MENU_ITEM_EDIT(float3, MSG_VXY_JERK, &max_xy_jerk, 1, 990);
   #if ENABLED(DELTA)

From 14afe1a017610e1530185b40bd2676e45695cbb7 Mon Sep 17 00:00:00 2001
From: Edward Patel <edward.patel@memention.com>
Date: Thu, 24 Mar 2016 23:35:51 +0100
Subject: [PATCH 2/2] Move to Z=0 for G28 when using Manual Bed Leveling

copy of https://github.com/MarlinFirmware/MarlinDev/pull/199/commits/406992f9dd44655b33dab46e0cb4fd4a29be1695
---
 Marlin/Marlin_main.cpp | 17 ++++++++---------
 Marlin/ultralcd.cpp    |  1 +
 2 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 1c162666d22..c88a9532a1d 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -2381,7 +2381,7 @@ inline void gcode_G28() {
     #endif
   #endif
 
-  // For manual bed leveling deactivate the matrix temporarily
+  // For mesh bed leveling deactivate the mesh calculations, will be turned on again when homing all axis
   #if ENABLED(MESH_BED_LEVELING)
     uint8_t mbl_was_active = mbl.active;
     mbl.active = 0;
@@ -2680,18 +2680,17 @@ inline void gcode_G28() {
     enable_endstops(false);
   #endif
 
-  // For manual leveling move back to 0,0
+  // For mesh leveling move back to Z=0
   #if ENABLED(MESH_BED_LEVELING)
-    if (mbl_was_active) {
-      current_position[X_AXIS] = mbl.get_x(0);
-      current_position[Y_AXIS] = mbl.get_y(0);
-      set_destination_to_current();
-      feedrate = homing_feedrate[X_AXIS];
-      line_to_destination();
-      st_synchronize();
+    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();
+      st_synchronize();
       #if ENABLED(DEBUG_LEVELING_FEATURE)
         if (marlin_debug_flags & DEBUG_LEVELING) {
           print_xyz("mbl_was_active > current_position", current_position);
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index 38db6ed17f3..56f12a17bec 100644
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -2434,6 +2434,7 @@ char* ftostr52(const float& x) {
         if (_lcd_level_bed_position == (MESH_NUM_X_POINTS) * (MESH_NUM_Y_POINTS)) {
           current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
           line_to_current(Z_AXIS);
+          st_synchronize();
           mbl.active = 1;
           enqueue_and_echo_commands_P(PSTR("G28"));
           lcd_return_to_status();