diff --git a/Marlin/src/gcode/calibrate/G34_M422.cpp b/Marlin/src/gcode/calibrate/G34_M422.cpp
index fecbb4be6828ff44cd1ee826dd8a6f63172f6c36..672f40737a5b5109cecf5e5de3cefab10e30e8e0 100644
--- a/Marlin/src/gcode/calibrate/G34_M422.cpp
+++ b/Marlin/src/gcode/calibrate/G34_M422.cpp
@@ -64,6 +64,7 @@ inline void set_all_z_lock(const bool lock) {
* I<iterations>
* T<accuracy>
* A<amplification>
+ * R<recalculate> points based on current probe offsets
*/
void GcodeSuite::G34() {
if (DEBUGGING(LEVELING)) {
@@ -73,8 +74,10 @@ void GcodeSuite::G34() {
do { // break out on error
- #if NUM_Z_STEPPER_DRIVERS >= 4
- SERIAL_ECHOLNPGM("Alignment not supported for over 3 steppers");
+ #if NUM_Z_STEPPER_DRIVERS == 4
+ SERIAL_ECHOLNPGM("Alignment for 4 steppers is Experimental!");
+ #elif NUM_Z_STEPPER_DRIVERS > 4
+ SERIAL_ECHOLNPGM("Alignment not supported for over 4 steppers");
break;
#endif
@@ -101,6 +104,8 @@ void GcodeSuite::G34() {
}
#endif
+ if (parser.seen('R')) z_stepper_align.reset_to_default();
+
const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE;
// Wait for planner moves to finish!
@@ -139,13 +144,18 @@ void GcodeSuite::G34() {
// Compute a worst-case clearance height to probe from. After the first
// iteration this will be re-calculated based on the actual bed position
- float z_probe = Z_BASIC_CLEARANCE + (G34_MAX_GRADE) * 0.01f * (
+ auto magnitude2 = [&](const uint8_t i, const uint8_t j) {
+ const xy_pos_t diff = z_stepper_align.xy[i] - z_stepper_align.xy[j];
+ return HYPOT2(diff.x, diff.y);
+ };
+ float z_probe = Z_BASIC_CLEARANCE + (G34_MAX_GRADE) * 0.01f * SQRT(
#if NUM_Z_STEPPER_DRIVERS == 3
- SQRT(_MAX(HYPOT2(z_stepper_align.xy[0].x - z_stepper_align.xy[1].x, z_stepper_align.xy[0].y - z_stepper_align.xy[1].y),
- HYPOT2(z_stepper_align.xy[1].x - z_stepper_align.xy[2].x, z_stepper_align.xy[1].y - z_stepper_align.xy[2].y),
- HYPOT2(z_stepper_align.xy[2].x - z_stepper_align.xy[0].x, z_stepper_align.xy[2].y - z_stepper_align.xy[0].y)))
+ _MAX(magnitude2(0, 1), magnitude2(1, 2), magnitude2(2, 0))
+ #elif NUM_Z_STEPPER_DRIVERS == 4
+ _MAX(magnitude2(0, 1), magnitude2(1, 2), magnitude2(2, 3),
+ magnitude2(3, 0), magnitude2(0, 2), magnitude2(1, 3))
#else
- HYPOT(z_stepper_align.xy[0].x - z_stepper_align.xy[1].x, z_stepper_align.xy[0].y - z_stepper_align.xy[1].y)
+ magnitude2(0, 1)
#endif
);
@@ -159,7 +169,7 @@ void GcodeSuite::G34() {
// This hack is un-done at the end of G34 - either by re-homing, or by using the probed heights of the last iteration.
#if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
- float last_z_align_move[NUM_Z_STEPPER_DRIVERS] = ARRAY_N(NUM_Z_STEPPER_DRIVERS, 10000.0f, 10000.0f, 10000.0f);
+ float last_z_align_move[NUM_Z_STEPPER_DRIVERS] = ARRAY_N(NUM_Z_STEPPER_DRIVERS, 10000.0f, 10000.0f, 10000.0f, 10000.0f);
#else
float last_z_align_level_indicator = 10000.0f;
#endif
@@ -241,7 +251,7 @@ void GcodeSuite::G34() {
linear_fit_data lfd;
incremental_LSF_reset(&lfd);
LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) {
- SERIAL_ECHOLNPAIR("PROBEPT_", ('0' + i), ": ", z_measured[i]);
+ SERIAL_ECHOLNPAIR("PROBEPT_", int(i), ": ", z_measured[i]);
incremental_LSF(&lfd, z_stepper_align.xy[i], z_measured[i]);
}
finish_incremental_LSF(&lfd);
@@ -322,9 +332,12 @@ void GcodeSuite::G34() {
switch (zstepper) {
case 0: stepper.set_z_lock(false); break;
case 1: stepper.set_z2_lock(false); break;
- #if NUM_Z_STEPPER_DRIVERS == 3
+ #if NUM_Z_STEPPER_DRIVERS >= 3
case 2: stepper.set_z3_lock(false); break;
#endif
+ #if NUM_Z_STEPPER_DRIVERS == 4
+ case 3: stepper.set_z4_lock(false); break;
+ #endif
}
#if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
@@ -348,28 +361,13 @@ void GcodeSuite::G34() {
} // for (iteration)
- if (err_break) {
+ if (err_break)
SERIAL_ECHOLNPGM("G34 aborted.");
- set_axis_not_trusted(Z_AXIS); // The Z coordinate is messed up now
- break;
+ else {
+ SERIAL_ECHOLNPAIR("Did ", int(iteration + (iteration != z_auto_align_iterations)), " of ", int(z_auto_align_iterations));
+ SERIAL_ECHOLNPAIR_F("Accuracy: ", z_maxdiff);
}
- SERIAL_ECHOLNPAIR("Did ", int(iteration + (iteration != z_auto_align_iterations)), " iterations of ", int(z_auto_align_iterations));
- SERIAL_ECHOLNPAIR_F("Accuracy: ", z_maxdiff);
-
- // Restore the active tool after homing
- #if HOTENDS > 1
- tool_change(old_tool_index, (true
- #if ENABLED(PARKING_EXTRUDER)
- && false // Fetch the previous toolhead
- #endif
- ));
- #endif
-
- #if HAS_LEVELING && ENABLED(RESTORE_LEVELING_AFTER_G34)
- set_bed_leveling_enabled(leveling_was_active);
- #endif
-
// Stow the probe, as the last call to probe.probe_at_point(...) left
// the probe deployed if it was successful.
probe.stow();
@@ -377,7 +375,6 @@ void GcodeSuite::G34() {
#if ENABLED(HOME_AFTER_G34)
// After this operation the z position needs correction
set_axis_not_trusted(Z_AXIS);
-
// Home Z after the alignment procedure
process_subcommands_now_P(PSTR("G28Z"));
#else
@@ -388,6 +385,15 @@ void GcodeSuite::G34() {
sync_plan_position();
#endif
+ // Restore the active tool after homing
+ #if HOTENDS > 1
+ tool_change(old_tool_index, DISABLED(PARKING_EXTRUDER)); // Fetch previous tool for parking extruder
+ #endif
+
+ #if HAS_LEVELING && ENABLED(RESTORE_LEVELING_AFTER_G34)
+ set_bed_leveling_enabled(leveling_was_active);
+ #endif
+
}while(0);
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("<<< G34");
@@ -406,9 +412,16 @@ void GcodeSuite::G34() {
* S and W require an X and/or Y parameter
* X<pos> : X position to set (Unchanged if omitted)
* Y<pos> : Y position to set (Unchanged if omitted)
+ *
+ * R : Recalculate points based on current probe offsets
*/
void GcodeSuite::M422() {
+ if (parser.seen('R')) {
+ z_stepper_align.reset_to_default();
+ return;
+ }
+
if (!parser.seen_any()) {
LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS)
SERIAL_ECHOLNPAIR_P(PSTR("M422 S"), int(i + 1), SP_X_STR, z_stepper_align.xy[i].x, SP_Y_STR, z_stepper_align.xy[i].y);
diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h
index d017dbf2db5b529dc27819ed61dcd0e9edd9e43c..454183e548852616f82cfba40012833a224cc3d0 100644
--- a/Marlin/src/inc/SanityCheck.h
+++ b/Marlin/src/inc/SanityCheck.h
@@ -2509,8 +2509,8 @@ static_assert( _ARR_TEST(3,0) && _ARR_TEST(3,1) && _ARR_TEST(3,2)
#error "Z_STEPPER_AUTO_ALIGN requires NUM_Z_STEPPER_DRIVERS greater than 1."
#elif !HAS_BED_PROBE
#error "Z_STEPPER_AUTO_ALIGN requires a Z-bed probe."
- #elif ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) && NUM_Z_STEPPER_DRIVERS != 3
- #error "Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS requires NUM_Z_STEPPER_DRIVERS to be 3."
+ #elif ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) && NUM_Z_STEPPER_DRIVERS < 3
+ #error "Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS requires NUM_Z_STEPPER_DRIVERS to be 3 or 4."
#endif
#endif