From f0b7f2702919f8d84ea93bf2b9ffa6e95937cab3 Mon Sep 17 00:00:00 2001 From: Chris Pepper Date: Tue, 22 Dec 2020 11:59:25 +0000 Subject: [PATCH] Fix UBL mesh inset Z position (#20538) --- Marlin/src/feature/bedlevel/ubl/ubl.h | 33 ++++++++----- .../src/feature/bedlevel/ubl/ubl_motion.cpp | 49 ++++++++----------- 2 files changed, 42 insertions(+), 40 deletions(-) diff --git a/Marlin/src/feature/bedlevel/ubl/ubl.h b/Marlin/src/feature/bedlevel/ubl/ubl.h index c90b1f7ac1..762becfb69 100644 --- a/Marlin/src/feature/bedlevel/ubl/ubl.h +++ b/Marlin/src/feature/bedlevel/ubl/ubl.h @@ -122,20 +122,29 @@ class unified_bed_leveling { FORCE_INLINE static void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; } + static int8_t cell_index_x_raw(const float &x) { + return FLOOR((x - (MESH_MIN_X)) * RECIPROCAL(MESH_X_DIST)); + } + + static int8_t cell_index_y_raw(const float &y) { + return FLOOR((y - (MESH_MIN_Y)) * RECIPROCAL(MESH_Y_DIST)); + } + + static int8_t cell_index_x_valid(const float &x) { + return WITHIN(cell_index_x_raw(x), 0, (GRID_MAX_POINTS_X - 2)); + } + + static int8_t cell_index_y_valid(const float &y) { + return WITHIN(cell_index_y_raw(y), 0, (GRID_MAX_POINTS_Y - 2)); + } + static int8_t cell_index_x(const float &x) { - const int8_t cx = (x - (MESH_MIN_X)) * RECIPROCAL(MESH_X_DIST); - return constrain(cx, 0, (GRID_MAX_POINTS_X) - 1); // -1 is appropriate if we want all movement to the X_MAX - } // position. But with this defined this way, it is possible - // to extrapolate off of this point even further out. Probably - // that is OK because something else should be keeping that from - // happening and should not be worried about at this level. + return constrain(cell_index_x_raw(x), 0, (GRID_MAX_POINTS_X) - 2); + } + static int8_t cell_index_y(const float &y) { - const int8_t cy = (y - (MESH_MIN_Y)) * RECIPROCAL(MESH_Y_DIST); - return constrain(cy, 0, (GRID_MAX_POINTS_Y) - 1); // -1 is appropriate if we want all movement to the Y_MAX - } // position. But with this defined this way, it is possible - // to extrapolate off of this point even further out. Probably - // that is OK because something else should be keeping that from - // happening and should not be worried about at this level. + return constrain(cell_index_y_raw(y), 0, (GRID_MAX_POINTS_Y) - 2); + } static inline xy_int8_t cell_indexes(const float &x, const float &y) { return { cell_index_x(x), cell_index_y(y) }; diff --git a/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp b/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp index 010b5951be..8b7cd15a3c 100644 --- a/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp +++ b/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp @@ -56,39 +56,32 @@ // A move within the same cell needs no splitting if (istart == iend) { - // For a move off the bed, use a constant Z raise - if (!WITHIN(iend.x, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(iend.y, 0, GRID_MAX_POINTS_Y - 1)) { - - // Note: There is no Z Correction in this case. We are off the grid and don't know what - // a reasonable correction would be. If the user has specified a UBL_Z_RAISE_WHEN_OFF_MESH - // value, that will be used instead of a calculated (Bi-Linear interpolation) correction. - - #ifdef UBL_Z_RAISE_WHEN_OFF_MESH - end.z += UBL_Z_RAISE_WHEN_OFF_MESH; - #endif - planner.buffer_segment(end, scaled_fr_mm_s, extruder); - current_position = destination; - return; - } - FINAL_MOVE: - // The distance is always MESH_X_DIST so multiply by the constant reciprocal. - const float xratio = (end.x - mesh_index_to_xpos(iend.x)) * RECIPROCAL(MESH_X_DIST); + // When UBL_Z_RAISE_WHEN_OFF_MESH is disabled Z correction is extrapolated from the edge of the mesh + #ifdef UBL_Z_RAISE_WHEN_OFF_MESH + // For a move off the UBL mesh, use a constant Z raise + if (!cell_index_x_valid(end.x) || !cell_index_y_valid(end.y)) { - float z1, z2; - if (iend.x >= GRID_MAX_POINTS_X - 1) - z1 = z2 = 0.0; - else { - z1 = z_values[iend.x ][iend.y ] + xratio * - (z_values[iend.x + 1][iend.y ] - z_values[iend.x][iend.y ]), - z2 = z_values[iend.x ][iend.y + 1] + xratio * - (z_values[iend.x + 1][iend.y + 1] - z_values[iend.x][iend.y + 1]); - } + // Note: There is no Z Correction in this case. We are off the mesh and don't know what + // a reasonable correction would be, UBL_Z_RAISE_WHEN_OFF_MESH will be used instead of + // a calculated (Bi-Linear interpolation) correction. + + end.z += UBL_Z_RAISE_WHEN_OFF_MESH; + planner.buffer_segment(end, scaled_fr_mm_s, extruder); + current_position = destination; + return; + } + #endif + + // The distance is always MESH_X_DIST so multiply by the constant reciprocal. + const float xratio = (end.x - mesh_index_to_xpos(iend.x)) * RECIPROCAL(MESH_X_DIST), + yratio = (end.y - mesh_index_to_ypos(iend.y)) * RECIPROCAL(MESH_Y_DIST), + z1 = z_values[iend.x][iend.y ] + xratio * (z_values[iend.x + 1][iend.y ] - z_values[iend.x][iend.y ]), + z2 = z_values[iend.x][iend.y + 1] + xratio * (z_values[iend.x + 1][iend.y + 1] - z_values[iend.x][iend.y + 1]); // X cell-fraction done. Interpolate the two Z offsets with the Y fraction for the final Z offset. - const float yratio = (end.y - mesh_index_to_ypos(iend.y)) * RECIPROCAL(MESH_Y_DIST), - z0 = iend.y < GRID_MAX_POINTS_Y - 1 ? (z1 + (z2 - z1) * yratio) * planner.fade_scaling_factor_for_z(end.z) : 0.0; + const float z0 = (z1 + (z2 - z1) * yratio) * planner.fade_scaling_factor_for_z(end.z); // Undefined parts of the Mesh in z_values[][] are NAN. // Replace NAN corrections with 0.0 to prevent NAN propagation.