diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 0eedde5f..27a4a8d6 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -1389,13 +1389,13 @@ inline void set_current_to_destination() { memcpy(current_position, destination,
inline void set_destination_to_current() { memcpy(destination, current_position, sizeof(destination)); }
-static void setup_for_endstop_move() {
+static void setup_for_endstop_move(bool enable_endstops_now = true) {
saved_feedrate = feedrate;
saved_feedmultiply = feedmultiply;
feedmultiply = 100;
previous_millis_cmd = millis();
- enable_endstops(true);
+ enable_endstops(enable_endstops_now);
}
static void clean_up_after_endstop_move() {
@@ -2408,7 +2408,7 @@ void process_commands()
int Z_PROBE_FEEDRATE = homing_feedrate[Z_AXIS]/60;
int Z_LIFT_FEEDRATE = homing_feedrate[Z_AXIS]/40;
bool has_z = is_bed_z_jitter_data_valid();
- setup_for_endstop_move();
+ setup_for_endstop_move(false);
const char *kill_message = NULL;
while (mesh_point != MESH_MEAS_NUM_X_POINTS * MESH_MEAS_NUM_Y_POINTS) {
// Get coords of a measuring point.
@@ -2428,16 +2428,15 @@ void process_commands()
#endif
}
- // Move Z to proper distance
+ // Move Z up to MESH_HOME_Z_SEARCH.
current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], Z_LIFT_FEEDRATE, active_extruder);
st_synchronize();
+ // Move to XY position of the sensor point.
current_position[X_AXIS] = pgm_read_float(bed_ref_points+2*mesh_point);
current_position[Y_AXIS] = pgm_read_float(bed_ref_points+2*mesh_point+1);
world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
-// mbl.get_meas_xy(ix, iy, current_position[X_AXIS], current_position[Y_AXIS], false);
- enable_endstops(false);
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], XY_AXIS_FEEDRATE, active_extruder);
st_synchronize();
@@ -2464,8 +2463,8 @@ void process_commands()
}
current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS],current_position[Z_AXIS] , current_position[E_AXIS], Z_LIFT_FEEDRATE, active_extruder);
+ st_synchronize();
if (mesh_point != MESH_MEAS_NUM_X_POINTS * MESH_MEAS_NUM_Y_POINTS) {
- st_synchronize();
kill(kill_message);
}
clean_up_after_endstop_move();
@@ -2525,12 +2524,7 @@ void process_commands()
mbl.upsample_3x3();
mbl.active = 1;
- current_position[X_AXIS] = X_MIN_POS+0.2;
- current_position[Y_AXIS] = Y_MIN_POS+0.2;
- current_position[Z_AXIS] = Z_MIN_POS;
- world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
- plan_buffer_line(current_position[X_AXIS], current_position[X_AXIS], current_position[Z_AXIS], current_position[E_AXIS], XY_AXIS_FEEDRATE, active_extruder);
- st_synchronize();
+ go_home_with_z_lift();
// Restore custom message state
custom_message = custom_message_old;
diff --git a/Firmware/mesh_bed_calibration.cpp b/Firmware/mesh_bed_calibration.cpp
index 4177dc93..a4bc9fc7 100644
--- a/Firmware/mesh_bed_calibration.cpp
+++ b/Firmware/mesh_bed_calibration.cpp
@@ -1932,6 +1932,25 @@ canceled:
return result;
}
+void go_home_with_z_lift()
+{
+ // Don't let the manage_inactivity() function remove power from the motors.
+ refresh_cmd_timeout();
+ // Go home.
+ // First move up to a safe height.
+ current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
+ go_to_current(homing_feedrate[Z_AXIS]/60);
+ // Second move to XY [0, 0].
+ current_position[X_AXIS] = X_MIN_POS+0.2;
+ current_position[Y_AXIS] = Y_MIN_POS+0.2;
+ // Clamp to the physical coordinates.
+ world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
+ go_to_current(homing_feedrate[X_AXIS]/60);
+ // Third move up to a safe height.
+ current_position[Z_AXIS] = Z_MIN_POS;
+ go_to_current(homing_feedrate[Z_AXIS]/60);
+}
+
// Sample the 9 points of the bed and store them into the EEPROM as a reference.
// When calling this function, the X, Y, Z axes should be already homed,
// and the world2machine correction matrix should be active.
@@ -2025,17 +2044,7 @@ bool sample_mesh_and_store_reference()
mbl.upsample_3x3();
mbl.active = true;
- // Don't let the manage_inactivity() function remove power from the motors.
- refresh_cmd_timeout();
-
- // Go home.
- current_position[Z_AXIS] = Z_MIN_POS;
- go_to_current(homing_feedrate[Z_AXIS]/60);
- current_position[X_AXIS] = X_MIN_POS+0.2;
- current_position[Y_AXIS] = Y_MIN_POS+0.2;
- // Clamp to the physical coordinates.
- world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
- go_to_current(homing_feedrate[X_AXIS]/60);
+ go_home_with_z_lift();
enable_endstops(endstops_enabled);
enable_z_endstop(endstop_z_enabled);
diff --git a/Firmware/mesh_bed_calibration.h b/Firmware/mesh_bed_calibration.h
index f1f8ff7a..102fa48d 100644
--- a/Firmware/mesh_bed_calibration.h
+++ b/Firmware/mesh_bed_calibration.h
@@ -142,6 +142,7 @@ inline bool world2machine_clamp(float &x, float &y)
extern bool find_bed_induction_sensor_point_z(float minimum_z = -10.f, uint8_t n_iter = 3);
extern bool find_bed_induction_sensor_point_xy();
+extern void go_home_with_z_lift();
// Positive or zero: ok
// Negative: failed
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index 2629073c..8c6375dd 100644
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -10,6 +10,7 @@
#include <string.h>
#include "util.h"
+#include "mesh_bed_leveling.h"
//#include "Configuration.h"
@@ -2665,8 +2666,14 @@ void lcd_sdcard_stop()
if ((int32_t)encoderPosition == 2)
{
cancel_heatup = true;
+ #ifdef MESH_BED_LEVELING
+ mbl.active = false;
+ #endif
// Stop the stoppers, update the position from the stoppers.
planner_abort_hard();
+ // Because the planner_abort_hard() initialized current_position[Z] from the stepper,
+ // Z baystep is no more applied. Reset it.
+ babystep_reset();
// Clean the input command queue.
cmdqueue_reset();
lcd_setstatuspgm(MSG_PRINT_ABORTED);