diff --git a/Marlin/Conditionals_post.h b/Marlin/Conditionals_post.h
index 6eab11dac3..239780f346 100644
--- a/Marlin/Conditionals_post.h
+++ b/Marlin/Conditionals_post.h
@@ -165,6 +165,11 @@
#ifndef Z_SAFE_HOMING_Y_POINT
#define Z_SAFE_HOMING_Y_POINT ((Y_MIN_POS + Y_MAX_POS) / 2)
#endif
+ #define X_TILT_FULCRUM Z_SAFE_HOMING_X_POINT
+ #define Y_TILT_FULCRUM Z_SAFE_HOMING_Y_POINT
+ #else
+ #define X_TILT_FULCRUM X_HOME_POS
+ #define Y_TILT_FULCRUM Y_HOME_POS
#endif
/**
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 7ab7af8532..4ea114504f 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -458,45 +458,51 @@ static uint8_t target_extruder;
#if ENABLED(DELTA)
- #define TOWER_1 X_AXIS
- #define TOWER_2 Y_AXIS
- #define TOWER_3 Z_AXIS
-
- float delta[ABC];
- float cartesian_position[XYZ] = { 0 };
#define SIN_60 0.8660254037844386
#define COS_60 0.5
- float endstop_adj[ABC] = { 0 };
+
+ float delta[ABC],
+ cartesian_position[XYZ] = { 0 },
+ endstop_adj[ABC] = { 0 };
+
// these are the default values, can be overriden with M665
- float delta_radius = DELTA_RADIUS;
- float delta_tower1_x = -SIN_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1); // front left tower
- float delta_tower1_y = -COS_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1);
- float delta_tower2_x = SIN_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2); // front right tower
- float delta_tower2_y = -COS_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2);
- float delta_tower3_x = 0; // back middle tower
- float delta_tower3_y = (delta_radius + DELTA_RADIUS_TRIM_TOWER_3);
- float delta_diagonal_rod = DELTA_DIAGONAL_ROD;
- float delta_diagonal_rod_trim_tower_1 = DELTA_DIAGONAL_ROD_TRIM_TOWER_1;
- float delta_diagonal_rod_trim_tower_2 = DELTA_DIAGONAL_ROD_TRIM_TOWER_2;
- float delta_diagonal_rod_trim_tower_3 = DELTA_DIAGONAL_ROD_TRIM_TOWER_3;
- float delta_diagonal_rod_2_tower_1 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_1);
- float delta_diagonal_rod_2_tower_2 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_2);
- float delta_diagonal_rod_2_tower_3 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_3);
- float delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND;
- float delta_clip_start_height = Z_MAX_POS;
+ float delta_radius = DELTA_RADIUS,
+ delta_tower1_x = -SIN_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1), // front left tower
+ delta_tower1_y = -COS_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1),
+ delta_tower2_x = SIN_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2), // front right tower
+ delta_tower2_y = -COS_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2),
+ delta_tower3_x = 0, // back middle tower
+ delta_tower3_y = (delta_radius + DELTA_RADIUS_TRIM_TOWER_3),
+ delta_diagonal_rod = DELTA_DIAGONAL_ROD,
+ delta_diagonal_rod_trim_tower_1 = DELTA_DIAGONAL_ROD_TRIM_TOWER_1,
+ delta_diagonal_rod_trim_tower_2 = DELTA_DIAGONAL_ROD_TRIM_TOWER_2,
+ delta_diagonal_rod_trim_tower_3 = DELTA_DIAGONAL_ROD_TRIM_TOWER_3,
+ delta_diagonal_rod_2_tower_1 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_1),
+ delta_diagonal_rod_2_tower_2 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_2),
+ delta_diagonal_rod_2_tower_3 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_3),
+ delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND,
+ delta_clip_start_height = Z_MAX_POS;
+
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
int delta_grid_spacing[2] = { 0, 0 };
float bed_level[AUTO_BED_LEVELING_GRID_POINTS][AUTO_BED_LEVELING_GRID_POINTS];
#endif
+
float delta_safe_distance_from_top();
+ void set_cartesian_from_steppers();
+
#else
+
static bool home_all_axis = true;
+
#endif
#if ENABLED(SCARA)
- float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND;
- float delta[ABC];
- float axis_scaling[ABC] = { 1, 1, 1 }; // Build size scaling, default to 1
+ float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND,
+ delta[ABC],
+ axis_scaling[ABC] = { 1, 1, 1 }, // Build size scaling, default to 1
+ cartesian_position[XYZ] = { 0 };
+ void set_cartesian_from_steppers() { } // to be written later
#endif
#if ENABLED(FILAMENT_WIDTH_SENSOR)
@@ -2266,79 +2272,37 @@ static void clean_up_after_endstop_or_probe_move() {
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
- #if ENABLED(AUTO_BED_LEVELING_GRID)
+ #if DISABLED(DELTA)
- #if DISABLED(DELTA)
+ /**
+ * Get the stepper positions, apply the rotation matrix
+ * using the home XY and Z0 position as the fulcrum.
+ */
+ vector_3 untilted_stepper_position() {
+ vector_3 pos = vector_3(
+ RAW_X_POSITION(stepper.get_axis_position_mm(X_AXIS)) - X_TILT_FULCRUM,
+ RAW_Y_POSITION(stepper.get_axis_position_mm(Y_AXIS)) - Y_TILT_FULCRUM,
+ RAW_Z_POSITION(stepper.get_axis_position_mm(Z_AXIS))
+ );
- static void set_bed_level_equation_lsq(double* plane_equation_coefficients) {
+ matrix_3x3 inverse = matrix_3x3::transpose(planner.bed_level_matrix);
- //planner.bed_level_matrix.debug("bed level before");
+ //pos.debug("untilted_stepper_position offset");
+ //bed_level_matrix.debug("untilted_stepper_position");
+ //inverse.debug("in untilted_stepper_position");
- #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
+ pos.apply_rotation(inverse);
- vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
- planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
+ pos.x = LOGICAL_X_POSITION(pos.x + X_TILT_FULCRUM);
+ pos.y = LOGICAL_Y_POSITION(pos.y + Y_TILT_FULCRUM);
+ pos.z = LOGICAL_Z_POSITION(pos.z);
- 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;
+ //pos.debug("after rotation and reorientation");
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) DEBUG_POS("<<< set_bed_level_equation_lsq", corrected_position);
- #endif
-
- SYNC_PLAN_POSITION_KINEMATIC();
- }
-
- #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();
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) {
- vector_3 uncorrected_position = planner.adjusted_position();
- DEBUG_POS("set_bed_level_equation_3pts", uncorrected_position);
- }
- #endif
-
- 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();
-
- 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_KINEMATIC();
+ return pos;
}
- #endif // !AUTO_BED_LEVELING_GRID
+ #endif // !DELTA
#if ENABLED(DELTA)
@@ -3626,41 +3590,41 @@ inline void gcode_G28() {
#endif // AUTO_BED_LEVELING_GRID
+ stepper.synchronize();
+
if (!dryrun) {
- #if ENABLED(DEBUG_LEVELING_FEATURE) && DISABLED(DELTA)
- if (DEBUGGING(LEVELING)) {
- vector_3 corrected_position = planner.adjusted_position();
- DEBUG_POS("BEFORE matrix.set_to_identity", corrected_position);
- DEBUG_POS("BEFORE matrix.set_to_identity", current_position);
- }
- #endif
-
- // make sure the bed_level_rotation_matrix is identity or the planner will get it wrong
+ // Reset the bed_level_matrix because leveling
+ // needs to be done without leveling enabled.
planner.bed_level_matrix.set_to_identity();
- #if ENABLED(DELTA)
- reset_bed_level();
- #else //!DELTA
+ //
+ // Re-orient the current position without leveling
+ // based on where the steppers are positioned.
+ //
+ #if ENABLED(DELTA) || ENABLED(SCARA)
- //vector_3 corrected_position = planner.adjusted_position();
- //corrected_position.debug("position before G29");
- vector_3 uncorrected_position = planner.adjusted_position();
- //uncorrected_position.debug("position during G29");
- current_position[X_AXIS] = uncorrected_position.x;
- current_position[Y_AXIS] = uncorrected_position.y;
- current_position[Z_AXIS] = uncorrected_position.z;
-
- #if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) DEBUG_POS("AFTER matrix.set_to_identity", uncorrected_position);
+ #if ENABLED(DELTA)
+ reset_bed_level();
#endif
- SYNC_PLAN_POSITION_KINEMATIC();
+ // For DELTA/SCARA we need to apply forward kinematics.
+ // This returns raw positions and we remap to the space.
+ set_cartesian_from_steppers();
+ LOOP_XYZ(i) current_position[i] = LOGICAL_POSITION(cartesian_position[i], i);
+
+ #else
+
+ // For cartesian/core the steppers are already mapped to
+ // the coordinate space by design.
+ LOOP_XYZ(i) current_position[i] = stepper.get_axis_position_mm((AxisEnum)i);
#endif // !DELTA
- }
- stepper.synchronize();
+ // Inform the planner about the new coordinates
+ // (This is probably not needed here)
+ SYNC_PLAN_POSITION_KINEMATIC();
+ }
setup_for_endstop_or_probe_move();
@@ -3766,7 +3730,20 @@ inline void gcode_G28() {
LOGICAL_Y_POSITION(ABL_PROBE_PT_3_Y),
stow_probe_after_each, verbose_level);
- if (!dryrun) set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3);
+ if (!dryrun) {
+ vector_3 pt1 = vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, z_at_pt_1),
+ pt2 = vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, z_at_pt_2),
+ 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 *= -1;
+ planeNormal.y *= -1;
+ planeNormal.z *= -1;
+ }
+ planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
+ }
#endif // !AUTO_BED_LEVELING_GRID
@@ -3810,7 +3787,12 @@ inline void gcode_G28() {
}
}
- if (!dryrun) set_bed_level_equation_lsq(plane_equation_coefficients);
+ // Create the matrix but don't correct the position yet
+ if (!dryrun) {
+ planner.bed_level_matrix = matrix_3x3::create_look_at(
+ vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1)
+ );
+ }
// Show the Topography map if enabled
if (do_topography_map) {
@@ -3851,6 +3833,7 @@ inline void gcode_G28() {
SERIAL_EOL;
} // yy
SERIAL_EOL;
+
if (verbose_level > 3) {
SERIAL_PROTOCOLLNPGM("\nCorrected Bed Height vs. Bed Topology:");
@@ -3876,47 +3859,60 @@ inline void gcode_G28() {
SERIAL_EOL;
}
} //do_topography_map
+
#endif //!DELTA
+
#endif // AUTO_BED_LEVELING_GRID
#if DISABLED(DELTA)
+
if (verbose_level > 0)
planner.bed_level_matrix.debug("\n\nBed Level Correction Matrix:");
if (!dryrun) {
- /**
- * Correct the Z height difference from Z probe position and nozzle tip position.
- * The Z height on homing is measured by Z probe, but the Z probe is quite far
- * from the nozzle. When the bed is uneven, this height must be corrected.
- */
- float x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER,
- y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER,
- z_tmp = current_position[Z_AXIS],
- stepper_z = stepper.get_axis_position_mm(Z_AXIS); //get the real Z (since planner.adjusted_position is now correcting the plane)
+ //
+ // Correct the current XYZ position based on the tilted plane.
+ //
+
+ // Get the distance from the reference point to the current position
+ // The current XY is in sync with the planner/steppers at this point
+ // but the current Z is only known to the steppers.
+ float x_dist = RAW_CURRENT_POSITION(X_AXIS) - X_TILT_FULCRUM,
+ y_dist = RAW_CURRENT_POSITION(Y_AXIS) - Y_TILT_FULCRUM,
+ z_real = RAW_Z_POSITION(stepper.get_axis_position_mm(Z_AXIS));
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) {
- SERIAL_ECHOPAIR("> BEFORE apply_rotation_xyz > stepper_z = ", stepper_z);
- SERIAL_ECHOLNPAIR(" ... z_tmp = ", z_tmp);
+ SERIAL_ECHOPAIR("BEFORE ROTATION ... x_dist:", x_dist);
+ SERIAL_ECHOPAIR("y_dist:", y_dist);
+ SERIAL_ECHOPAIR("z_real:", z_real);
}
#endif
- // Apply the correction sending the Z probe offset
- apply_rotation_xyz(planner.bed_level_matrix, x_tmp, y_tmp, z_tmp);
+ // Apply the matrix to the distance from the reference point to XY,
+ // and from the homed Z to the current Z.
+ apply_rotation_xyz(planner.bed_level_matrix, x_dist, y_dist, z_real);
#if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING))
- SERIAL_ECHOLNPAIR("> AFTER apply_rotation_xyz > z_tmp = ", z_tmp);
+ if (DEBUGGING(LEVELING)) {
+ SERIAL_ECHOPAIR("AFTER ROTATION ... x_dist:", x_dist);
+ SERIAL_ECHOPAIR("y_dist:", y_dist);
+ SERIAL_ECHOPAIR("z_real:", z_real);
+ }
#endif
- // Adjust the current Z and send it to the planner.
- current_position[Z_AXIS] += z_tmp - stepper_z;
+ // Apply the rotated distance and Z to the current position
+ current_position[X_AXIS] = LOGICAL_X_POSITION(X_TILT_FULCRUM + x_dist);
+ current_position[Y_AXIS] = LOGICAL_Y_POSITION(Y_TILT_FULCRUM + y_dist);
+ current_position[Z_AXIS] = LOGICAL_Z_POSITION(z_real);
+
SYNC_PLAN_POSITION_KINEMATIC();
#if ENABLED(DEBUG_LEVELING_FEATURE)
- if (DEBUGGING(LEVELING)) DEBUG_POS("> corrected Z in G29", current_position);
+ if (DEBUGGING(LEVELING)) DEBUG_POS("> corrected XYZ in G29", current_position);
#endif
}
+
#endif // !DELTA
#ifdef Z_PROBE_END_SCRIPT
@@ -7850,15 +7846,15 @@ void ok_to_send() {
RAW_Z_POSITION(in_cartesian[Z_AXIS])
};
- delta[TOWER_1] = sqrt(delta_diagonal_rod_2_tower_1
+ delta[A_AXIS] = sqrt(delta_diagonal_rod_2_tower_1
- sq(delta_tower1_x - cartesian[X_AXIS])
- sq(delta_tower1_y - cartesian[Y_AXIS])
) + cartesian[Z_AXIS];
- delta[TOWER_2] = sqrt(delta_diagonal_rod_2_tower_2
+ delta[B_AXIS] = sqrt(delta_diagonal_rod_2_tower_2
- sq(delta_tower2_x - cartesian[X_AXIS])
- sq(delta_tower2_y - cartesian[Y_AXIS])
) + cartesian[Z_AXIS];
- delta[TOWER_3] = sqrt(delta_diagonal_rod_2_tower_3
+ delta[C_AXIS] = sqrt(delta_diagonal_rod_2_tower_3
- sq(delta_tower3_x - cartesian[X_AXIS])
- sq(delta_tower3_y - cartesian[Y_AXIS])
) + cartesian[Z_AXIS];
@@ -7867,9 +7863,9 @@ void ok_to_send() {
SERIAL_ECHOPGM(" y="); SERIAL_ECHO(cartesian[Y_AXIS]);
SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(cartesian[Z_AXIS]);
- SERIAL_ECHOPGM("delta a="); SERIAL_ECHO(delta[TOWER_1]);
- SERIAL_ECHOPGM(" b="); SERIAL_ECHO(delta[TOWER_2]);
- SERIAL_ECHOPGM(" c="); SERIAL_ECHOLN(delta[TOWER_3]);
+ SERIAL_ECHOPGM("delta a="); SERIAL_ECHO(delta[A_AXIS]);
+ SERIAL_ECHOPGM(" b="); SERIAL_ECHO(delta[B_AXIS]);
+ SERIAL_ECHOPGM(" c="); SERIAL_ECHOLN(delta[C_AXIS]);
*/
}
@@ -7880,10 +7876,10 @@ void ok_to_send() {
LOGICAL_Z_POSITION(0)
};
inverse_kinematics(cartesian);
- float distance = delta[TOWER_3];
+ float distance = delta[A_AXIS];
cartesian[Y_AXIS] = LOGICAL_Y_POSITION(DELTA_PRINTABLE_RADIUS);
inverse_kinematics(cartesian);
- return abs(distance - delta[TOWER_3]);
+ return abs(distance - delta[A_AXIS]);
}
void forward_kinematics_DELTA(float z1, float z2, float z3) {
@@ -8014,7 +8010,7 @@ void set_current_from_steppers_for_axis(AxisEnum axis) {
set_cartesian_from_steppers();
current_position[axis] = LOGICAL_POSITION(cartesian_position[axis], axis);
#elif ENABLED(AUTO_BED_LEVELING_FEATURE)
- vector_3 pos = planner.adjusted_position();
+ vector_3 pos = untilted_stepper_position();
current_position[axis] = axis == X_AXIS ? pos.x : axis == Y_AXIS ? pos.y : pos.z;
#else
current_position[axis] = stepper.get_axis_position_mm(axis); // CORE handled transparently
diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index 066c1445b0..bcad2c9069 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -521,6 +521,38 @@ void Planner::check_axes_activity() {
#endif
}
+#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
+
+ void Planner::apply_leveling(
+ #if ENABLED(MESH_BED_LEVELING)
+ const float &x, const float &y
+ #else
+ float &x, float &y
+ #endif
+ , float &z
+ ) {
+ #if ENABLED(MESH_BED_LEVELING)
+
+ if (mbl.active())
+ z += mbl.get_z(RAW_X_POSITION(x), RAW_Y_POSITION(y));
+
+ #elif ENABLED(AUTO_BED_LEVELING_FEATURE)
+
+ float tx = RAW_X_POSITION(x) - (X_TILT_FULCRUM),
+ ty = RAW_Y_POSITION(y) - (Y_TILT_FULCRUM),
+ tz = RAW_Z_POSITION(z);
+
+ apply_rotation_xyz(bed_level_matrix, tx, ty, tz);
+
+ x = LOGICAL_X_POSITION(tx + X_TILT_FULCRUM);
+ y = LOGICAL_Y_POSITION(ty + Y_TILT_FULCRUM);
+ z = LOGICAL_Z_POSITION(tz);
+
+ #endif
+ }
+
+#endif
+
/**
* Planner::buffer_line
*
@@ -531,12 +563,14 @@ void Planner::check_axes_activity() {
* extruder - target extruder
*/
-#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
- void Planner::buffer_line(float x, float y, float z, const float& e, float fr_mm_s, const uint8_t extruder)
-#else
- void Planner::buffer_line(const float& x, const float& y, const float& z, const float& e, float fr_mm_s, const uint8_t extruder)
-#endif // AUTO_BED_LEVELING_FEATURE
-{
+void Planner::buffer_line(
+ #if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
+ float x, float y, float z
+ #else
+ const float& x, const float& y, const float& z
+ #endif
+ , const float& e, float fr_mm_s, const uint8_t extruder
+) {
// Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index(block_buffer_head);
@@ -544,11 +578,8 @@ void Planner::check_axes_activity() {
// Rest here until there is room in the buffer.
while (block_buffer_tail == next_buffer_head) idle();
- #if ENABLED(MESH_BED_LEVELING)
- if (mbl.active())
- z += mbl.get_z(x - home_offset[X_AXIS], y - home_offset[Y_AXIS]);
- #elif ENABLED(AUTO_BED_LEVELING_FEATURE)
- apply_rotation_xyz(bed_level_matrix, x, y, z);
+ #if ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_FEATURE)
+ apply_leveling(x, y, z);
#endif
// The target position of the tool in absolute steps
@@ -1116,61 +1147,33 @@ void Planner::check_axes_activity() {
} // buffer_line()
-#if ENABLED(AUTO_BED_LEVELING_FEATURE) && DISABLED(DELTA)
-
- /**
- * Get the XYZ position of the steppers as a vector_3.
- *
- * On CORE machines XYZ is derived from ABC.
- */
- vector_3 Planner::adjusted_position() {
- vector_3 pos = vector_3(stepper.get_axis_position_mm(X_AXIS), stepper.get_axis_position_mm(Y_AXIS), stepper.get_axis_position_mm(Z_AXIS));
-
- //pos.debug("in Planner::adjusted_position");
- //bed_level_matrix.debug("in Planner::adjusted_position");
-
- matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
- //inverse.debug("in Planner::inverse");
-
- pos.apply_rotation(inverse);
- //pos.debug("after rotation");
-
- return pos;
- }
-
-#endif // AUTO_BED_LEVELING_FEATURE && !DELTA
-
/**
* Directly set the planner XYZ position (hence the stepper positions).
*
* On CORE machines stepper ABC will be translated from the given XYZ.
*/
-#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
- void Planner::set_position_mm(float x, float y, float z, const float& e)
-#else
- void Planner::set_position_mm(const float& x, const float& y, const float& z, const float& e)
-#endif // AUTO_BED_LEVELING_FEATURE || MESH_BED_LEVELING
- {
- #if ENABLED(MESH_BED_LEVELING)
+void Planner::set_position_mm(
+ #if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
+ float x, float y, float z
+ #else
+ const float& x, const float& y, const float& z
+ #endif
+ , const float& e
+) {
- if (mbl.active())
- z += mbl.get_z(RAW_X_POSITION(x), RAW_Y_POSITION(y));
+ #if ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_FEATURE)
+ apply_leveling(x, y, z);
+ #endif
- #elif ENABLED(AUTO_BED_LEVELING_FEATURE)
+ long nx = position[X_AXIS] = lround(x * axis_steps_per_mm[X_AXIS]),
+ ny = position[Y_AXIS] = lround(y * axis_steps_per_mm[Y_AXIS]),
+ nz = position[Z_AXIS] = lround(z * axis_steps_per_mm[Z_AXIS]),
+ ne = position[E_AXIS] = lround(e * axis_steps_per_mm[E_AXIS]);
+ stepper.set_position(nx, ny, nz, ne);
+ previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
- apply_rotation_xyz(bed_level_matrix, x, y, z);
-
- #endif
-
- long nx = position[X_AXIS] = lround(x * axis_steps_per_mm[X_AXIS]),
- ny = position[Y_AXIS] = lround(y * axis_steps_per_mm[Y_AXIS]),
- nz = position[Z_AXIS] = lround(z * axis_steps_per_mm[Z_AXIS]),
- ne = position[E_AXIS] = lround(e * axis_steps_per_mm[E_AXIS]);
- stepper.set_position(nx, ny, nz, ne);
- previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
-
- LOOP_XYZE(i) previous_speed[i] = 0.0;
- }
+ LOOP_XYZE(i) previous_speed[i] = 0.0;
+}
/**
* Directly set the planner E position (hence the stepper E position).
diff --git a/Marlin/planner.h b/Marlin/planner.h
index e1159294a0..ecca0fd3fc 100644
--- a/Marlin/planner.h
+++ b/Marlin/planner.h
@@ -203,11 +203,10 @@ class Planner {
#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
- #if ENABLED(AUTO_BED_LEVELING_FEATURE)
- /**
- * The corrected position, applying the bed level matrix
- */
- static vector_3 adjusted_position();
+ #if ENABLED(MESH_BED_LEVELING)
+ static void apply_leveling(const float &x, const float &y, float &z);
+ #else
+ static void apply_leveling(float &x, float &y, float &z);
#endif
/**