Add M420 C to adjust the whole mesh (#10522)

Marlin/src/feature/bedlevel/ubl/ubl.h

     #endif
 
     static bool g29_parameter_parsing() _O0;
-    static void find_mean_mesh_height();
     static void shift_mesh_height();
     static void probe_entire_mesh(const float &rx, const float &ry, const bool do_ubl_mesh_map, const bool stow_probe, const bool do_furthest) _O0;
     static void tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3);
     static mesh_index_pair find_furthest_invalid_mesh_point() _O0;
     static void reset();
     static void invalidate();
-    static void set_all_mesh_points_to_value(const float);
+    static void set_all_mesh_points_to_value(const float value);
+    static void adjust_mesh_to_mean(const float value);
     static bool sanity_check();
 
     static void G29() _O0;                          // O0 for no optimization

Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp

           #endif
           break;
 
-        case 5: find_mean_mesh_height(); break;
+        case 5: adjust_mesh_to_mean(g29_constant); break;
 
         case 6: shift_mesh_height(); break;
       }
     return;
   }
 
-  void unified_bed_leveling::find_mean_mesh_height() {
+  void unified_bed_leveling::adjust_mesh_to_mean(const float value) {
     float sum = 0.0;
     int n = 0;
     for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
       for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
         for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
           if (!isnan(z_values[x][y]))
-            z_values[x][y] -= mean + g29_constant;
+            z_values[x][y] -= mean + value;
   }
 
   void unified_bed_leveling::shift_mesh_height() {
       SERIAL_EOL();
     #endif
 
-    find_mean_mesh_height();
+    adjust_mesh_to_mean(g29_constant);
 
     #if HAS_BED_PROBE
       SERIAL_PROTOCOLPGM("zprobe_zoffset: ");

Marlin/src/gcode/bedlevel/M420.cpp

   #include "../../module/configuration_store.h"
 #endif
 
+//#define M420_C_USE_MEAN
+
 /**
  * M420: Enable/Disable Bed Leveling and/or set the Z fade height.
  *
  *
  *   L[index]  Load UBL mesh from index (0 is default)
  *   T[map]    0:Human-readable 1:CSV 2:"LCD" 4:Compact
+ *
+ * With mesh-based leveling only:
+ *
+ *   C         Center mesh on the mean of the lowest and highest
  */
 void GcodeSuite::M420() {
+  const bool seen_S = parser.seen('S');
+  bool to_enable = seen_S ? parser.value_bool() : planner.leveling_active;
+
+  // If disabling leveling do it right away
+  // (Don't disable for just M420 or M420 V)
+  if (seen_S && !to_enable) set_bed_leveling_enabled(false);
 
   const float oldpos[] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] };
 
     // L to load a mesh from the EEPROM
     if (parser.seen('L')) {
 
+      set_bed_leveling_enabled(false);
+
       #if ENABLED(EEPROM_SETTINGS)
         const int8_t storage_slot = parser.has_value() ? parser.value_int() : ubl.storage_slot;
         const int16_t a = settings.calc_num_meshes();
 
   #endif // AUTO_BED_LEVELING_UBL
 
+  #if HAS_MESH
+
+    #if ENABLED(MESH_BED_LEVELING)
+      #define Z_VALUES(X,Y) mbl.z_values[X][Y]
+    #else
+      #define Z_VALUES(X,Y) z_values[X][Y]
+    #endif
+
+    // Subtract the given value or the mean from all mesh values
+    if (leveling_is_valid() && parser.seen('C')) {
+      const float cval = parser.value_float();
+      #if ENABLED(AUTO_BED_LEVELING_UBL)
+
+        set_bed_leveling_enabled(false);
+        ubl.adjust_mesh_to_mean(cval);
+
+      #else
+
+        #if ENABLED(M420_C_USE_MEAN)
+
+          // Get the sum and average of all mesh values
+          float mesh_sum = 0;
+          for (uint8_t x = GRID_MAX_POINTS_X; x--;)
+            for (uint8_t y = GRID_MAX_POINTS_Y; y--;)
+              mesh_sum += Z_VALUES(x, y);
+          const float zmean = mesh_sum / float(GRID_MAX_POINTS);
+
+        #else
+
+          // Find the low and high mesh values
+          float lo_val = 100, hi_val = -100;
+          for (uint8_t x = GRID_MAX_POINTS_X; x--;)
+            for (uint8_t y = GRID_MAX_POINTS_Y; y--;) {
+              const float z = Z_VALUES(x, y);
+              NOMORE(lo_val, z);
+              NOLESS(hi_val, z);
+            }
+          // Take the mean of the lowest and highest
+          const float zmean = (lo_val + hi_val) / 2.0 + cval;
+
+        #endif
+
+        // If not very close to 0, adjust the mesh
+        if (!NEAR_ZERO(zmean)) {
+          set_bed_leveling_enabled(false);
+          // Subtract the mean from all values
+          for (uint8_t x = GRID_MAX_POINTS_X; x--;)
+            for (uint8_t y = GRID_MAX_POINTS_Y; y--;)
+              Z_VALUES(x, y) -= zmean;
+          #if ENABLED(ABL_BILINEAR_SUBDIVISION)
+            bed_level_virt_interpolate();
+          #endif
+        }
+
+      #endif
+    }
+
+  #endif // HAS_MESH
+
   // V to print the matrix or mesh
   if (parser.seen('V')) {
     #if ABL_PLANAR
     if (parser.seen('Z')) set_z_fade_height(parser.value_linear_units(), false);
   #endif
 
-  bool to_enable = false;
-  if (parser.seen('S')) {
-    to_enable = parser.value_bool();
-    set_bed_leveling_enabled(to_enable);
-  }
-
-  const bool new_status = planner.leveling_active;
+  // Enable leveling if specified, or if previously active
+  set_bed_leveling_enabled(to_enable);
 
-  if (to_enable && !new_status) {
+  // Error if leveling failed to enable or reenable
+  if (to_enable && !planner.leveling_active) {
     SERIAL_ERROR_START();
     SERIAL_ERRORLNPGM(MSG_ERR_M420_FAILED);
   }
 
   SERIAL_ECHO_START();
-  SERIAL_ECHOLNPAIR("Bed Leveling ", new_status ? MSG_ON : MSG_OFF);
+  SERIAL_ECHOLNPAIR("Bed Leveling ", planner.leveling_active ? MSG_ON : MSG_OFF);
 
   #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
     SERIAL_ECHO_START();