diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 794aec5a85d3a0a5df65cbb859d0ebe770237f0c..fe8559dd836f26d1e3ff36c2c18d6a2a396116ad 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -1960,89 +1960,6 @@ static void setup_for_endstop_move() {
endstops.enable_z_probe(false);
}
-#endif // HAS_BED_PROBE
-
-#if ENABLED(AUTO_BED_LEVELING_FEATURE)
-
- #if ENABLED(AUTO_BED_LEVELING_GRID)
-
- #if DISABLED(DELTA)
-
- static void set_bed_level_equation_lsq(double* plane_equation_coefficients) {
-
- //planner.bed_level_matrix.debug("bed level before");
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- planner.bed_level_matrix.set_to_identity();
- if (DEBUGGING(LEVELING)) {
- vector_3 uncorrected_position = planner.adjusted_position();
- DEBUG_POS(">>> set_bed_level_equation_lsq", uncorrected_position);
- DEBUG_POS(">>> set_bed_level_equation_lsq", current_position);
- }
- #endif
-
- vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
- planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
-
- vector_3 corrected_position = planner.adjusted_position();
- current_position[X_AXIS] = corrected_position.x;
- current_position[Y_AXIS] = corrected_position.y;
- current_position[Z_AXIS] = corrected_position.z;
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) DEBUG_POS("<<< set_bed_level_equation_lsq", corrected_position);
- #endif
-
- sync_plan_position();
- }
-
- #endif // !DELTA
-
- #else // !AUTO_BED_LEVELING_GRID
-
- static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3) {
-
- planner.bed_level_matrix.set_to_identity();
-
- vector_3 pt1 = vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, z_at_pt_1);
- vector_3 pt2 = vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, z_at_pt_2);
- vector_3 pt3 = vector_3(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, z_at_pt_3);
- vector_3 planeNormal = vector_3::cross(pt1 - pt2, pt3 - pt2).get_normal();
-
- if (planeNormal.z < 0) {
- planeNormal.x = -planeNormal.x;
- planeNormal.y = -planeNormal.y;
- planeNormal.z = -planeNormal.z;
- }
-
- planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
-
- vector_3 corrected_position = planner.adjusted_position();
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) {
- vector_3 uncorrected_position = corrected_position;
- DEBUG_POS("set_bed_level_equation_3pts", uncorrected_position);
- }
- #endif
-
- current_position[X_AXIS] = corrected_position.x;
- current_position[Y_AXIS] = corrected_position.y;
- current_position[Z_AXIS] = corrected_position.z;
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) DEBUG_POS("set_bed_level_equation_3pts", corrected_position);
- #endif
-
- sync_plan_position();
- }
-
- #endif // !AUTO_BED_LEVELING_GRID
-
-#endif // AUTO_BED_LEVELING_FEATURE
-
-#if HAS_BED_PROBE
-
static void run_z_probe() {
float old_feedrate = feedrate;
@@ -2134,6 +2051,81 @@ static void setup_for_endstop_move() {
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
+ #if ENABLED(AUTO_BED_LEVELING_GRID)
+
+ #if DISABLED(DELTA)
+
+ static void set_bed_level_equation_lsq(double* plane_equation_coefficients) {
+
+ //planner.bed_level_matrix.debug("bed level before");
+
+ #if ENABLED(DEBUG_LEVELING_FEATURE)
+ planner.bed_level_matrix.set_to_identity();
+ if (DEBUGGING(LEVELING)) {
+ vector_3 uncorrected_position = planner.adjusted_position();
+ DEBUG_POS(">>> set_bed_level_equation_lsq", uncorrected_position);
+ DEBUG_POS(">>> set_bed_level_equation_lsq", current_position);
+ }
+ #endif
+
+ vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
+ planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
+
+ vector_3 corrected_position = planner.adjusted_position();
+ current_position[X_AXIS] = corrected_position.x;
+ current_position[Y_AXIS] = corrected_position.y;
+ current_position[Z_AXIS] = corrected_position.z;
+
+ #if ENABLED(DEBUG_LEVELING_FEATURE)
+ if (DEBUGGING(LEVELING)) DEBUG_POS("<<< set_bed_level_equation_lsq", corrected_position);
+ #endif
+
+ sync_plan_position();
+ }
+
+ #endif // !DELTA
+
+ #else // !AUTO_BED_LEVELING_GRID
+
+ static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3) {
+
+ planner.bed_level_matrix.set_to_identity();
+
+ vector_3 pt1 = vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, z_at_pt_1);
+ vector_3 pt2 = vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, z_at_pt_2);
+ vector_3 pt3 = vector_3(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, z_at_pt_3);
+ vector_3 planeNormal = vector_3::cross(pt1 - pt2, pt3 - pt2).get_normal();
+
+ if (planeNormal.z < 0) {
+ planeNormal.x = -planeNormal.x;
+ planeNormal.y = -planeNormal.y;
+ planeNormal.z = -planeNormal.z;
+ }
+
+ planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
+
+ vector_3 corrected_position = planner.adjusted_position();
+
+ #if ENABLED(DEBUG_LEVELING_FEATURE)
+ if (DEBUGGING(LEVELING)) {
+ vector_3 uncorrected_position = corrected_position;
+ DEBUG_POS("set_bed_level_equation_3pts", uncorrected_position);
+ }
+ #endif
+
+ current_position[X_AXIS] = corrected_position.x;
+ current_position[Y_AXIS] = corrected_position.y;
+ current_position[Z_AXIS] = corrected_position.z;
+
+ #if ENABLED(DEBUG_LEVELING_FEATURE)
+ if (DEBUGGING(LEVELING)) DEBUG_POS("set_bed_level_equation_3pts", corrected_position);
+ #endif
+
+ sync_plan_position();
+ }
+
+ #endif // !AUTO_BED_LEVELING_GRID
+
inline void do_blocking_move_to_xy(float x, float y) {
do_blocking_move_to(x, y, current_position[Z_AXIS]);
}