diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 4e01b3780b898c1a5c628a9057843dd1bd6e1b58..3e92612b7017d02cfa646e5119906b2c0e45b767 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -2799,6 +2799,28 @@ inline void gcode_G28() {
enum MeshLevelingState { MeshReport, MeshStart, MeshNext, MeshSet, MeshSetZOffset };
+ inline void _mbl_goto_xy(float x, float y) {
+ saved_feedrate = feedrate;
+ feedrate = homing_feedrate[X_AXIS];
+
+ #if MIN_Z_HEIGHT_FOR_HOMING > 0
+ current_position[Z_AXIS] = MESH_HOME_SEARCH_Z + MIN_Z_HEIGHT_FOR_HOMING;
+ line_to_current_position();
+ #endif
+
+ current_position[X_AXIS] = x + home_offset[X_AXIS];
+ current_position[Y_AXIS] = y + home_offset[Y_AXIS];
+ line_to_current_position();
+
+ #if MIN_Z_HEIGHT_FOR_HOMING > 0
+ current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
+ line_to_current_position();
+ #endif
+
+ feedrate = saved_feedrate;
+ st_synchronize();
+ }
+
/**
* G29: Mesh-based Z probe, probes a grid and produces a
* mesh to compensate for variable bed height
@@ -2866,33 +2888,28 @@ inline void gcode_G28() {
SERIAL_PROTOCOLLNPGM("Start mesh probing with \"G29 S1\" first.");
return;
}
+ // For each G29 S2...
if (probe_point == 0) {
- // Set Z to a positive value before recording the first Z.
- current_position[Z_AXIS] = MESH_HOME_SEARCH_Z + home_offset[Z_AXIS];
+ // For the intial G29 S2 make Z a positive value (e.g., 4.0)
+ current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
sync_plan_position();
}
else {
- // For others, save the Z of the previous point, then raise Z again.
- ix = (probe_point - 1) % (MESH_NUM_X_POINTS);
- iy = (probe_point - 1) / (MESH_NUM_X_POINTS);
- if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // zig-zag
- mbl.set_z(ix, iy, current_position[Z_AXIS]);
- current_position[Z_AXIS] = MESH_HOME_SEARCH_Z + home_offset[Z_AXIS];
- plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS] / 60, active_extruder);
- st_synchronize();
+ // For G29 S2 after adjusting Z.
+ mbl.set_zigzag_z(probe_point - 1, current_position[Z_AXIS]);
}
- // Is there another point to sample? Move there.
+ // If there's another point to sample, move there with optional lift.
if (probe_point < (MESH_NUM_X_POINTS) * (MESH_NUM_Y_POINTS)) {
- ix = probe_point % (MESH_NUM_X_POINTS);
- iy = probe_point / (MESH_NUM_X_POINTS);
- if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // zig-zag
- current_position[X_AXIS] = mbl.get_x(ix) + home_offset[X_AXIS];
- current_position[Y_AXIS] = mbl.get_y(iy) + home_offset[Y_AXIS];
- plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS] / 60, active_extruder);
- st_synchronize();
+ mbl.zigzag(probe_point, ix, iy);
+ _mbl_goto_xy(mbl.get_x(ix), mbl.get_y(iy));
probe_point++;
}
else {
+ // One last "return to the bed" (as originally coded) at completion
+ current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
+ line_to_current_position();
+ st_synchronize();
+
// After recording the last point, activate the mbl and home
SERIAL_PROTOCOLLNPGM("Mesh probing done.");
probe_point = -1;
diff --git a/Marlin/mesh_bed_leveling.h b/Marlin/mesh_bed_leveling.h
index 7c1c0225ed2a084afffbd58d44b870316327724c..ce20247a05f6f4b94de42f4f37103fb1e4b9c231 100644
--- a/Marlin/mesh_bed_leveling.h
+++ b/Marlin/mesh_bed_leveling.h
@@ -41,6 +41,18 @@
float get_y(int i) { return MESH_MIN_Y + (MESH_Y_DIST) * i; }
void set_z(int ix, int iy, float z) { z_values[iy][ix] = z; }
+ inline void zigzag(int index, int &ix, int &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
+ }
+
+ void set_zigzag_z(int index, float z) {
+ int ix, iy;
+ zigzag(index, ix, iy);
+ set_z(ix, iy, z);
+ }
+
int select_x_index(float x) {
int i = 1;
while (x > get_x(i) && i < MESH_NUM_X_POINTS - 1) i++;
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index 516292c5bbcf991e8d1af954be0a25e8c4ae1664..e34d21b2464ff49eaf8088d6a3747ee76759ab59 100644
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -887,6 +887,28 @@ void lcd_cooldown() {
*/
static int _lcd_level_bed_position;
+ static bool mbl_wait_for_move = false;
+
+ // Utility to go to the next mesh point
+ // A raise is added between points if MIN_Z_HEIGHT_FOR_HOMING is in use
+ // Note: During Manual Bed Leveling the homed Z position is MESH_HOME_SEARCH_Z
+ // Z position will be restored with the final action, a G28
+ inline void _mbl_goto_xy(float x, float y) {
+ mbl_wait_for_move = true;
+ #if MIN_Z_HEIGHT_FOR_HOMING > 0
+ current_position[Z_AXIS] += MIN_Z_HEIGHT_FOR_HOMING;
+ line_to_current(Z_AXIS);
+ #endif
+ current_position[X_AXIS] = x + home_offset[X_AXIS];
+ current_position[Y_AXIS] = y + home_offset[Y_AXIS];
+ line_to_current(manual_feedrate[X_AXIS] <= manual_feedrate[Y_AXIS] ? X_AXIS : Y_AXIS);
+ #if MIN_Z_HEIGHT_FOR_HOMING > 0
+ current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
+ line_to_current(Z_AXIS);
+ #endif
+ st_synchronize();
+ mbl_wait_for_move = false;
+ }
/**
* 5. MBL Wait for controller movement and clicks:
@@ -894,7 +916,6 @@ void lcd_cooldown() {
* - Click saves the Z, goes to the next mesh point
*/
static void _lcd_level_bed_procedure() {
- static bool mbl_wait_for_move = false;
// Menu handlers may be called in a re-entrant fashion
// if they call st_synchronize or plan_buffer_line. So
// while waiting for a move we just ignore new input.
@@ -931,11 +952,7 @@ void lcd_cooldown() {
if (LCD_CLICKED) {
if (!debounce_click) {
debounce_click = true; // ignore multiple "clicks" in a row
- int ix = _lcd_level_bed_position % (MESH_NUM_X_POINTS),
- iy = _lcd_level_bed_position / (MESH_NUM_X_POINTS);
- if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // Zig zag
- mbl.set_z(ix, iy, current_position[Z_AXIS]);
- _lcd_level_bed_position++;
+ mbl.set_zigzag_z(_lcd_level_bed_position++, current_position[Z_AXIS]);
if (_lcd_level_bed_position == (MESH_NUM_X_POINTS) * (MESH_NUM_Y_POINTS)) {
lcd_return_to_status();
LCD_ALERTMESSAGEPGM(MSG_LEVEL_BED_DONE);
@@ -953,17 +970,9 @@ void lcd_cooldown() {
#if ENABLED(NEWPANEL)
lcd_quick_feedback();
#endif
- mbl_wait_for_move = true;
- current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
- line_to_current(Z_AXIS);
- ix = _lcd_level_bed_position % (MESH_NUM_X_POINTS);
- iy = _lcd_level_bed_position / (MESH_NUM_X_POINTS);
- if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // Zig zag
- current_position[X_AXIS] = mbl.get_x(ix);
- current_position[Y_AXIS] = mbl.get_y(iy);
- line_to_current(manual_feedrate[X_AXIS] <= manual_feedrate[Y_AXIS] ? X_AXIS : Y_AXIS);
- st_synchronize();
- mbl_wait_for_move = false;
+ int ix, iy;
+ mbl.zigzag(_lcd_level_bed_position, ix, iy);
+ _mbl_goto_xy(mbl.get_x(ix), mbl.get_y(iy));
encoderPosition = 0;
}
}
@@ -980,12 +989,11 @@ void lcd_cooldown() {
static void _lcd_level_bed_homing_done() {
if (lcdDrawUpdate) lcd_implementation_drawedit(PSTR(MSG_LEVEL_BED_WAITING), NULL);
lcdDrawUpdate = LCDVIEW_CALL_NO_REDRAW;
+ if (mbl_wait_for_move) return;
if (LCD_CLICKED) {
current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
- current_position[X_AXIS] = MESH_MIN_X;
- current_position[Y_AXIS] = MESH_MIN_Y;
- line_to_current(manual_feedrate[X_AXIS] <= manual_feedrate[Y_AXIS] ? X_AXIS : Y_AXIS);
+ _mbl_goto_xy(MESH_MIN_X, MESH_MIN_Y);
_lcd_level_bed_position = 0;
lcd_goto_menu(_lcd_level_bed_procedure, true);
}