diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index c0884cd23..b3c302d49 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -3008,7 +3008,7 @@ inline void gcode_G28() { return; } - int8_t ix, iy; + int8_t px, py; float z; switch (state) { @@ -3023,10 +3023,10 @@ inline void gcode_G28() { SERIAL_PROTOCOLPGM("\nZ offset: "); SERIAL_PROTOCOL_F(mbl.z_offset, 5); SERIAL_PROTOCOLLNPGM("\nMeasured points:"); - for (int y = 0; y < MESH_NUM_Y_POINTS; y++) { - for (int x = 0; x < MESH_NUM_X_POINTS; x++) { + for (py = 0; py < MESH_NUM_Y_POINTS; py++) { + for (px = 0; px < MESH_NUM_X_POINTS; px++) { SERIAL_PROTOCOLPGM(" "); - SERIAL_PROTOCOL_F(mbl.z_values[y][x], 5); + SERIAL_PROTOCOL_F(mbl.z_values[py][px], 5); } SERIAL_EOL; } @@ -3058,8 +3058,8 @@ inline void gcode_G28() { } // If there's another point to sample, move there with optional lift. if (probe_point < (MESH_NUM_X_POINTS) * (MESH_NUM_Y_POINTS)) { - mbl.zigzag(probe_point, ix, iy); - _mbl_goto_xy(mbl.get_x(ix), mbl.get_y(iy)); + mbl.zigzag(probe_point, px, py); + _mbl_goto_xy(mbl.get_probe_x(px), mbl.get_probe_y(py)); probe_point++; } else { @@ -3082,8 +3082,8 @@ inline void gcode_G28() { case MeshSet: if (code_seen('X')) { - ix = code_value_long() - 1; - if (ix < 0 || ix >= MESH_NUM_X_POINTS) { + px = code_value_long() - 1; + if (px < 0 || px >= MESH_NUM_X_POINTS) { SERIAL_PROTOCOLPGM("X out of range (1-" STRINGIFY(MESH_NUM_X_POINTS) ").\n"); return; } @@ -3093,8 +3093,8 @@ inline void gcode_G28() { return; } if (code_seen('Y')) { - iy = code_value_long() - 1; - if (iy < 0 || iy >= MESH_NUM_Y_POINTS) { + py = code_value_long() - 1; + if (py < 0 || py >= MESH_NUM_Y_POINTS) { SERIAL_PROTOCOLPGM("Y out of range (1-" STRINGIFY(MESH_NUM_Y_POINTS) ").\n"); return; } @@ -3110,7 +3110,7 @@ inline void gcode_G28() { SERIAL_PROTOCOLPGM("Z not entered.\n"); return; } - mbl.z_values[iy][ix] = z; + mbl.z_values[py][px] = z; break; case MeshSetZOffset: @@ -5904,39 +5904,35 @@ inline void gcode_M410() { stepper.quick_stop(); } * Use either 'M421 X Y Z' or 'M421 I J Z' */ inline void gcode_M421() { - float x = 0, y = 0, z = 0; - int8_t i = 0, j = 0; - bool err = false, hasX, hasY, hasZ, hasI, hasJ; - if ((hasX = code_seen('X'))) x = code_value(); - if ((hasY = code_seen('Y'))) y = code_value(); - if ((hasI = code_seen('I'))) i = code_value(); - if ((hasJ = code_seen('J'))) j = code_value(); + int8_t px, py; + float z = 0; + bool hasX, hasY, hasZ, hasI, hasJ; + if ((hasX = code_seen('X'))) px = mbl.probe_index_x(code_value()); + if ((hasY = code_seen('Y'))) py = mbl.probe_index_y(code_value()); + if ((hasI = code_seen('I'))) px = code_value(); + if ((hasJ = code_seen('J'))) py = code_value(); if ((hasZ = code_seen('Z'))) z = code_value(); if (hasX && hasY && hasZ) { - int8_t ix = mbl.select_x_index(x), - iy = mbl.select_y_index(y); - - if (ix >= 0 && iy >= 0) - mbl.set_z(ix, iy, z); + if (px >= 0 && py >= 0) + mbl.set_z(px, py, z); else { SERIAL_ERROR_START; SERIAL_ERRORLNPGM(MSG_ERR_MESH_XY); } } else if (hasI && hasJ && hasZ) { - if (i >= 0 && i < MESH_NUM_X_POINTS && j >= 0 && j < MESH_NUM_Y_POINTS) - mbl.set_z(i, j, z); + if (px >= 0 && px < MESH_NUM_X_POINTS && py >= 0 && py < MESH_NUM_Y_POINTS) + mbl.set_z(px, py, z); else { SERIAL_ERROR_START; SERIAL_ERRORLNPGM(MSG_ERR_MESH_XY); } } - else - { + else { SERIAL_ERROR_START; - SERIAL_ERRORLNPGM(MSG_ERR_M421_REQUIRES_XYZ); + SERIAL_ERRORLNPGM(MSG_ERR_M421_PARAMETERS); } } @@ -7303,52 +7299,52 @@ void mesh_buffer_line(float x, float y, float z, const float e, float feed_rate, set_current_to_destination(); return; } - int pix = mbl.select_x_index(current_position[X_AXIS] - home_offset[X_AXIS]); - int piy = mbl.select_y_index(current_position[Y_AXIS] - home_offset[Y_AXIS]); - int ix = mbl.select_x_index(x - home_offset[X_AXIS]); - int iy = mbl.select_y_index(y - home_offset[Y_AXIS]); - pix = min(pix, MESH_NUM_X_POINTS - 2); - piy = min(piy, MESH_NUM_Y_POINTS - 2); - ix = min(ix, MESH_NUM_X_POINTS - 2); - iy = min(iy, MESH_NUM_Y_POINTS - 2); - if (pix == ix && piy == iy) { + int pcx = mbl.cel_index_x(current_position[X_AXIS] - home_offset[X_AXIS]); + int pcy = mbl.cel_index_y(current_position[Y_AXIS] - home_offset[Y_AXIS]); + int cx = mbl.cel_index_x(x - home_offset[X_AXIS]); + int cy = mbl.cel_index_y(y - home_offset[Y_AXIS]); + NOMORE(pcx, MESH_NUM_X_POINTS - 2); + NOMORE(pcy, MESH_NUM_Y_POINTS - 2); + NOMORE(cx, MESH_NUM_X_POINTS - 2); + NOMORE(cy, MESH_NUM_Y_POINTS - 2); + if (pcx == cx && pcy == cy) { // Start and end on same mesh square planner.buffer_line(x, y, z, e, feed_rate, extruder); set_current_to_destination(); return; } float nx, ny, nz, ne, normalized_dist; - if (ix > pix && TEST(x_splits, ix)) { - nx = mbl.get_x(ix) + home_offset[X_AXIS]; + if (cx > pcx && TEST(x_splits, cx)) { + nx = mbl.get_probe_x(cx) + home_offset[X_AXIS]; normalized_dist = (nx - current_position[X_AXIS]) / (x - current_position[X_AXIS]); ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist; nz = current_position[Z_AXIS] + (z - current_position[Z_AXIS]) * normalized_dist; ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist; - CBI(x_splits, ix); + CBI(x_splits, cx); } - else if (ix < pix && TEST(x_splits, pix)) { - nx = mbl.get_x(pix) + home_offset[X_AXIS]; + else if (cx < pcx && TEST(x_splits, pcx)) { + nx = mbl.get_probe_x(pcx) + home_offset[X_AXIS]; normalized_dist = (nx - current_position[X_AXIS]) / (x - current_position[X_AXIS]); ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist; nz = current_position[Z_AXIS] + (z - current_position[Z_AXIS]) * normalized_dist; ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist; - CBI(x_splits, pix); + CBI(x_splits, pcx); } - else if (iy > piy && TEST(y_splits, iy)) { - ny = mbl.get_y(iy) + home_offset[Y_AXIS]; + else if (cy > pcy && TEST(y_splits, cy)) { + ny = mbl.get_probe_y(cy) + home_offset[Y_AXIS]; normalized_dist = (ny - current_position[Y_AXIS]) / (y - current_position[Y_AXIS]); nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist; nz = current_position[Z_AXIS] + (z - current_position[Z_AXIS]) * normalized_dist; ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist; - CBI(y_splits, iy); + CBI(y_splits, cy); } - else if (iy < piy && TEST(y_splits, piy)) { - ny = mbl.get_y(piy) + home_offset[Y_AXIS]; + else if (cy < pcy && TEST(y_splits, pcy)) { + ny = mbl.get_probe_y(pcy) + home_offset[Y_AXIS]; normalized_dist = (ny - current_position[Y_AXIS]) / (y - current_position[Y_AXIS]); nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist; nz = current_position[Z_AXIS] + (z - current_position[Z_AXIS]) * normalized_dist; ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist; - CBI(y_splits, piy); + CBI(y_splits, pcy); } else { // Already split on a border diff --git a/Marlin/configuration_store.cpp b/Marlin/configuration_store.cpp index 8fed0a93e..05a24acfe 100644 --- a/Marlin/configuration_store.cpp +++ b/Marlin/configuration_store.cpp @@ -62,7 +62,7 @@ * 201 z_offset (float) (added in V23) * 205 mesh_num_x (uint8 as set in firmware) * 206 mesh_num_y (uint8 as set in firmware) - * 207 M421 XYZ z_values[][] (float x9, by default) + * 207 G29 S3 XYZ z_values[][] (float x9, by default) * * AUTO BED LEVELING * 243 M851 zprobe_zoffset (float) @@ -733,13 +733,13 @@ void Config_PrintSettings(bool forReplay) { SERIAL_ECHOPAIR(" X", MESH_NUM_X_POINTS); SERIAL_ECHOPAIR(" Y", MESH_NUM_Y_POINTS); SERIAL_EOL; - for (uint8_t y = 0; y < MESH_NUM_Y_POINTS; y++) { - for (uint8_t x = 0; x < MESH_NUM_X_POINTS; x++) { + for (uint8_t py = 1; py <= MESH_NUM_Y_POINTS; py++) { + for (uint8_t px = 1; px <= MESH_NUM_X_POINTS; px++) { CONFIG_ECHO_START; - SERIAL_ECHOPAIR(" M421 X", mbl.get_x(x)); - SERIAL_ECHOPAIR(" Y", mbl.get_y(y)); + SERIAL_ECHOPAIR(" G29 S3 X", px); + SERIAL_ECHOPAIR(" Y", py); SERIAL_ECHOPGM(" Z"); - SERIAL_PROTOCOL_F(mbl.z_values[y][x], 5); + SERIAL_PROTOCOL_F(mbl.z_values[py-1][px-1], 5); SERIAL_EOL; } } diff --git a/Marlin/language.h b/Marlin/language.h index 6ef215415..edef06be1 100644 --- a/Marlin/language.h +++ b/Marlin/language.h @@ -147,7 +147,7 @@ #define MSG_Z2_MAX "z2_max: " #define MSG_Z_PROBE "z_probe: " #define MSG_ERR_MATERIAL_INDEX "M145 S out of range (0-1)" -#define MSG_ERR_M421_REQUIRES_XYZ "M421 requires XYZ or IJZ parameters" +#define MSG_ERR_M421_PARAMETERS "M421 requires XYZ or IJZ parameters" #define MSG_ERR_MESH_XY "Mesh XY or IJ cannot be resolved" #define MSG_ERR_M428_TOO_FAR "Too far from reference point" #define MSG_ERR_M303_DISABLED "PIDTEMP disabled" diff --git a/Marlin/mesh_bed_leveling.h b/Marlin/mesh_bed_leveling.h index f51501a22..7b2201fb9 100644 --- a/Marlin/mesh_bed_leveling.h +++ b/Marlin/mesh_bed_leveling.h @@ -37,34 +37,40 @@ void reset(); - static FORCE_INLINE float get_x(int8_t i) { return MESH_MIN_X + (MESH_X_DIST) * i; } - static FORCE_INLINE float get_y(int8_t i) { return MESH_MIN_Y + (MESH_Y_DIST) * i; } - void set_z(int8_t ix, int8_t iy, float z) { z_values[iy][ix] = z; } + static FORCE_INLINE float get_probe_x(int8_t i) { return MESH_MIN_X + (MESH_X_DIST) * i; } + static FORCE_INLINE float get_probe_y(int8_t i) { return MESH_MIN_Y + (MESH_Y_DIST) * i; } + void set_z(const int8_t px, const int8_t py, const float z) { z_values[py][px] = z; } - inline void zigzag(int8_t index, int8_t &ix, int8_t &iy) { - ix = index % (MESH_NUM_X_POINTS); - iy = index / (MESH_NUM_X_POINTS); - if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // Zig zag + inline void zigzag(int8_t index, int8_t &px, int8_t &py) { + px = index % (MESH_NUM_X_POINTS); + py = index / (MESH_NUM_X_POINTS); + if (py & 1) px = (MESH_NUM_X_POINTS - 1) - px; // Zig zag } void set_zigzag_z(int8_t index, float z) { - int8_t ix, iy; - zigzag(index, ix, iy); - set_z(ix, iy, z); + int8_t px, py; + zigzag(index, px, py); + set_z(px, py, z); } - int8_t select_x_index(float x) { - for (uint8_t i = MESH_NUM_X_POINTS; i--;) - if (fabs(x - get_x(i)) <= (MESH_X_DIST) / 2) - return i; - return -1; + int8_t cel_index_x(float x) { + int8_t cx = int(x - (MESH_MIN_X)) / (MESH_X_DIST); + return constrain(cx, 0, (MESH_NUM_X_POINTS) - 2); } - int8_t select_y_index(float y) { - for (uint8_t i = MESH_NUM_Y_POINTS; i--;) - if (fabs(y - get_y(i)) <= (MESH_Y_DIST) / 2) - return i; - return -1; + int8_t cel_index_y(float y) { + int8_t cy = int(y - (MESH_MIN_Y)) / (MESH_Y_DIST); + return constrain(cy, 0, (MESH_NUM_Y_POINTS) - 2); + } + + int8_t probe_index_x(float x) { + int8_t px = int(x - (MESH_MIN_X) + (MESH_X_DIST) / 2) / (MESH_X_DIST); + return (px >= 0 && px < (MESH_NUM_X_POINTS)) ? px : -1; + } + + int8_t probe_index_y(float y) { + int8_t py = int(y - (MESH_MIN_Y) + (MESH_Y_DIST) / 2) / (MESH_Y_DIST); + return (py >= 0 && py < (MESH_NUM_Y_POINTS)) ? py : -1; } float calc_z0(float a0, float a1, float z1, float a2, float z2) { @@ -74,18 +80,18 @@ } float get_z(float x0, float y0) { - int8_t x_index = select_x_index(x0); - int8_t y_index = select_y_index(y0); - if (x_index < 0 || y_index < 0) return z_offset; + int8_t cx = cel_index_x(x0), + cy = cel_index_y(y0); + if (cx < 0 || cy < 0) return z_offset; float z1 = calc_z0(x0, - get_x(x_index), z_values[y_index][x_index], - get_x(x_index + 1), z_values[y_index][x_index + 1]); + get_probe_x(cx), z_values[cy][cx], + get_probe_x(cx + 1), z_values[cy][cx + 1]); float z2 = calc_z0(x0, - get_x(x_index), z_values[y_index + 1][x_index], - get_x(x_index + 1), z_values[y_index + 1][x_index + 1]); + get_probe_x(cx), z_values[cy + 1][cx], + get_probe_x(cx + 1), z_values[cy + 1][cx + 1]); float z0 = calc_z0(y0, - get_y(y_index), z1, - get_y(y_index + 1), z2); + get_probe_y(cy), z1, + get_probe_y(cy + 1), z2); return z0 + z_offset; } }; diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp index dd99b2961..46ccb1964 100644 --- a/Marlin/ultralcd.cpp +++ b/Marlin/ultralcd.cpp @@ -1013,9 +1013,9 @@ void lcd_cooldown() { lcd_goto_menu(_lcd_level_bed_moving); // _mbl_goto_xy runs the menu loop until the move is done - int8_t ix, iy; - mbl.zigzag(_lcd_level_bed_position, ix, iy); - _mbl_goto_xy(mbl.get_x(ix), mbl.get_y(iy)); + int8_t px, py; + mbl.zigzag(_lcd_level_bed_position, px, py); + _mbl_goto_xy(mbl.get_probe_x(px), mbl.get_probe_y(py)); // After the blocking function returns, change menus lcd_goto_menu(_lcd_level_bed_get_z);