diff --git a/Marlin/src/feature/bedlevel/ubl/ubl.h b/Marlin/src/feature/bedlevel/ubl/ubl.h index 9de087c3bf..0d126daf9c 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 7da88d7c87..433e36cbfe 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 f877de881e..2df7d90285 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();