From 0423e93c42ab5eaa636553e698894f11ad710610 Mon Sep 17 00:00:00 2001
From: Roxy-3D <Roxanne.Rae.Neufeld@gmail.com>
Date: Sun, 2 Apr 2017 16:13:15 -0500
Subject: [PATCH] Fix M421 AUTO_BED_LEVELING_BILINEAR and AUTO_BED_LEVELING_UBL

M421 was not connected up for AUTO_BED_LEVELING_BILINEAR.
M421 needed to migrate mesh data to new UBL EEPROM layout.
---
 Marlin/Marlin_main.cpp | 34 ++++++++++++++++++++++++++++++----
 Marlin/UBL_G29.cpp     |  7 ++++++-
 2 files changed, 36 insertions(+), 5 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index a2013c826..6b5ae7e0f 100755
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -178,7 +178,7 @@
  * M407 - Display measured filament diameter in millimeters. (Requires FILAMENT_WIDTH_SENSOR)
  * M410 - Quickstop. Abort all planned moves.
  * M420 - Enable/Disable Leveling (with current values) S1=enable S0=disable (Requires MESH_BED_LEVELING or ABL)
- * M421 - Set a single Z coordinate in the Mesh Leveling grid. X<units> Y<units> Z<units> (Requires MESH_BED_LEVELING)
+ * M421 - Set a single Z coordinate in the Mesh Leveling grid. X<units> Y<units> Z<units> (Requires MESH_BED_LEVELING or AUTO_BED_LEVELING_UBL)
  * M428 - Set the home_offset based on the current_position. Nearest edge applies.
  * M500 - Store parameters in EEPROM. (Requires EEPROM_SETTINGS)
  * M501 - Restore parameters from EEPROM. (Requires EEPROM_SETTINGS)
@@ -7557,8 +7557,7 @@ void quickstop_stepper() {
   }
 #endif
 
-#if ENABLED(MESH_BED_LEVELING)
-
+#if ENABLED(MESH_BED_LEVELING) 
   /**
    * M421: Set a single Mesh Bed Leveling Z coordinate
    * Use either 'M421 X<linear> Y<linear> Z<linear>' or 'M421 I<xindex> J<yindex> Z<linear>'
@@ -7628,7 +7627,34 @@ void quickstop_stepper() {
       SERIAL_ERRORLNPGM(MSG_ERR_M421_PARAMETERS);
     }
   }
+#elif ENABLED(AUTO_BED_LEVELING_UBL)
+  /**
+   * M421: Set a single Mesh Bed Leveling Z coordinate
+   *
+   *   M421 I<xindex> J<yindex> Z<linear>
+   */
+  inline void gcode_M421() {
+    int8_t px = 0, py = 0;
+    float z = 0;
+    bool hasI, hasJ, hasZ;
+    if ((hasI = code_seen('I'))) px = code_value_axis_units(X_AXIS);
+    if ((hasJ = code_seen('J'))) py = code_value_axis_units(Y_AXIS);
+    if ((hasZ = code_seen('Z'))) z = code_value_axis_units(Z_AXIS);
 
+    if (hasI && hasJ && hasZ) {
+      if (WITHIN(px, 0, UBL_MESH_NUM_Y_POINTS - 1) && WITHIN(py, 0, UBL_MESH_NUM_Y_POINTS - 1)) {
+        ubl.z_values[px][py] = z;
+      }
+      else {
+        SERIAL_ERROR_START;
+        SERIAL_ERRORLNPGM(MSG_ERR_MESH_XY);
+      }
+    }
+    else {
+      SERIAL_ERROR_START;
+      SERIAL_ERRORLNPGM(MSG_ERR_M421_PARAMETERS);
+    }
+  }
 #endif
 
 #if DISABLED(NO_WORKSPACE_OFFSETS)
@@ -9387,7 +9413,7 @@ void process_next_command() {
           break;
       #endif
 
-      #if ENABLED(MESH_BED_LEVELING)
+      #if ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_UBL) || ENABLED(AUTO_BED_LEVELING_BILINEAR)
         case 421: // M421: Set a Mesh Bed Leveling Z coordinate
           gcode_M421();
           break;
diff --git a/Marlin/UBL_G29.cpp b/Marlin/UBL_G29.cpp
index 40bd33f55..1d49eafb0 100644
--- a/Marlin/UBL_G29.cpp
+++ b/Marlin/UBL_G29.cpp
@@ -327,8 +327,13 @@
     // Invalidate Mesh Points. This command is a little bit asymetrical because
     // it directly specifies the repetition count and does not use the 'R' parameter.
     if (code_seen('I')) {
+      int cnt = 0;
       repetition_cnt = code_has_value() ? code_value_int() : 1;
       while (repetition_cnt--) {
+        if (cnt>20) {
+          cnt = 0;
+          idle();
+        }
         const mesh_index_pair location = find_closest_mesh_point_of_type(REAL, x_pos, y_pos, 0, NULL, false);  // The '0' says we want to use the nozzle's position
         if (location.x_index < 0) {
           SERIAL_PROTOCOLLNPGM("Entire Mesh invalidated.\n");
@@ -1433,4 +1438,4 @@
     SERIAL_ECHOLNPGM("Done Editing Mesh");
   }
 
-#endif // AUTO_BED_LEVELING_UBL
\ No newline at end of file
+#endif // AUTO_BED_LEVELING_UBL