diff --git a/Marlin/src/feature/bedlevel/ubl/ubl.h b/Marlin/src/feature/bedlevel/ubl/ubl.h
index 9de087c3bf54f2eae321cab51c8d25dfeeb40e46..0d126daf9cbf918076289bdcce2cf2abd5e5610a 100644
--- a/Marlin/src/feature/bedlevel/ubl/ubl.h
+++ b/Marlin/src/feature/bedlevel/ubl/ubl.h
@@ -89,7 +89,6 @@ class unified_bed_leveling {
#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);
@@ -124,7 +123,8 @@ class unified_bed_leveling {
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
diff --git a/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp b/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp
index 7da88d7c87d8e2dfa8453855d7296069194d27e8..433e36cbfe6232a14f183e374163c117df5b797b 100644
--- a/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp
+++ b/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp
@@ -544,7 +544,7 @@
#endif
break;
- case 5: find_mean_mesh_height(); break;
+ case 5: adjust_mesh_to_mean(g29_constant); break;
case 6: shift_mesh_height(); break;
}
@@ -634,7 +634,7 @@
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++)
@@ -669,7 +669,7 @@
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() {
@@ -1081,7 +1081,7 @@
SERIAL_EOL();
#endif
- find_mean_mesh_height();
+ adjust_mesh_to_mean(g29_constant);
#if HAS_BED_PROBE
SERIAL_PROTOCOLPGM("zprobe_zoffset: ");
diff --git a/Marlin/src/gcode/bedlevel/M420.cpp b/Marlin/src/gcode/bedlevel/M420.cpp
index f877de881e1d2c2abb5bd995b79e735a6beeb74d..2df7d90285be6b21b21fea768efda5c696208fbd 100644
--- a/Marlin/src/gcode/bedlevel/M420.cpp
+++ b/Marlin/src/gcode/bedlevel/M420.cpp
@@ -32,6 +32,8 @@
#include "../../module/configuration_store.h"
#endif
+//#define M420_C_USE_MEAN
+
/**
* M420: Enable/Disable Bed Leveling and/or set the Z fade height.
*
@@ -43,8 +45,18 @@
*
* 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] };
@@ -53,6 +65,8 @@ void GcodeSuite::M420() {
// 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();
@@ -88,6 +102,65 @@ void GcodeSuite::M420() {
#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
@@ -111,21 +184,17 @@ void GcodeSuite::M420() {
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();