diff --git a/Marlin/src/gcode/calibrate/G34_M422.cpp b/Marlin/src/gcode/calibrate/G34_M422.cpp index 7144038ebd..7923ef9231 100644 --- a/Marlin/src/gcode/calibrate/G34_M422.cpp +++ b/Marlin/src/gcode/calibrate/G34_M422.cpp @@ -155,8 +155,12 @@ void GcodeSuite::G34() { // Move the Z coordinate realm towards the positive - dirty trick current_position.z -= z_probe * 0.5f; - float last_z_align_move[NUM_Z_STEPPER_DRIVERS] = ARRAY_N(NUM_Z_STEPPER_DRIVERS, 10000.0f, 10000.0f, 10000.0f), - z_measured[NUM_Z_STEPPER_DRIVERS] = { 0 }, + #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); + #else + float last_z_align_level_indicator = 10000.0f; + #endif + float z_measured[NUM_Z_STEPPER_DRIVERS] = { 0 }, z_maxdiff = 0.0f, amplification = z_auto_align_amplification; @@ -167,7 +171,7 @@ void GcodeSuite::G34() { bool adjustment_reverse = false; #endif - for (iteration = 0; iteration < z_auto_align_iterations; ++iteration) { + LOOP_L_N(iteration, z_auto_align_iterations) { if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> probing all positions."); SERIAL_ECHOLNPAIR("\nITERATION: ", int(iteration + 1)); @@ -177,7 +181,7 @@ void GcodeSuite::G34() { z_measured_max = -100000.0f; // Probe all positions (one per Z-Stepper) - for (uint8_t i = 0; i < NUM_Z_STEPPER_DRIVERS; ++i) { + LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) { // iteration odd/even --> downward / upward stepper sequence const uint8_t iprobe = (iteration & 1) ? NUM_Z_STEPPER_DRIVERS - 1 - i : i; @@ -227,14 +231,14 @@ void GcodeSuite::G34() { // This allows the actual adjustment logic to be shared by both algorithms. linear_fit_data lfd; incremental_LSF_reset(&lfd); - for (uint8_t i = 0; i < NUM_Z_STEPPER_DRIVERS; ++i) { + LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) { SERIAL_ECHOLNPAIR("PROBEPT_", i + '1', ": ", z_measured[i]); incremental_LSF(&lfd, z_stepper_align.xy[i], z_measured[i]); } finish_incremental_LSF(&lfd); z_measured_min = 100000.0f; - for (uint8_t i = 0; i < NUM_Z_STEPPER_DRIVERS; ++i) { + LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) { z_measured[i] = -(lfd.A * z_stepper_align.stepper_xy[i].x + lfd.B * z_stepper_align.stepper_xy[i].y); z_measured_min = _MIN(z_measured_min, z_measured[i]); } @@ -250,12 +254,37 @@ void GcodeSuite::G34() { #endif ); + #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) + // Check if the applied corrections go in the correct direction. + // Calculate the sum of the absolute deviations from the mean of the probe measurements. + // Compare to the last iteration to ensure it's getting better. + + // Calculate mean value as a reference + float z_measured_mean = 0.0f; + LOOP_L_N(zstepper, NUM_Z_STEPPER_DRIVERS) z_measured_mean += z_measured[zstepper]; + z_measured_mean /= NUM_Z_STEPPER_DRIVERS; + + // Calculate the sum of the absolute deviations from the mean value + float z_align_level_indicator = 0.0f; + LOOP_L_N(zstepper, NUM_Z_STEPPER_DRIVERS) + z_align_level_indicator += ABS(z_measured[zstepper] - z_measured_mean); + + // If it's getting worse, stop and throw an error + if (last_z_align_level_indicator < z_align_level_indicator * 0.7f) { + SERIAL_ECHOLNPGM("Decreasing accuracy detected."); + err_break = true; + break; + } + + last_z_align_level_indicator = z_align_level_indicator; + #endif + // The following correction actions are to be enabled for select Z-steppers only stepper.set_separate_multi_axis(true); bool success_break = true; // Correct the individual stepper offsets - for (uint8_t zstepper = 0; zstepper < NUM_Z_STEPPER_DRIVERS; ++zstepper) { + LOOP_L_N(zstepper, NUM_Z_STEPPER_DRIVERS) { // Calculate current stepper move float z_align_move = z_measured[zstepper] - z_measured_min; const float z_align_abs = ABS(z_align_move); @@ -263,21 +292,16 @@ void GcodeSuite::G34() { #if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) // Optimize one iteration's correction based on the first measurements if (z_align_abs) amplification = (iteration == 1) ? _MIN(last_z_align_move[zstepper] / z_align_abs, 2.0f) : z_auto_align_amplification; - #endif - // Check for less accuracy compared to last move - if (last_z_align_move[zstepper] < z_align_abs * 0.7f) { - SERIAL_ECHOLNPGM("Decreasing accuracy detected."); - #if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) + // Check for less accuracy compared to last move + if (last_z_align_move[zstepper] < z_align_abs * 0.7f) { + SERIAL_ECHOLNPGM("Decreasing accuracy detected."); adjustment_reverse = !adjustment_reverse; - #else - err_break = true; - break; - #endif - } + } - // Remember the alignment for the next iteration - last_z_align_move[zstepper] = z_align_abs; + // Remember the alignment for the next iteration + last_z_align_move[zstepper] = z_align_abs; + #endif // Stop early if all measured points achieve accuracy target if (z_align_abs > z_auto_align_accuracy) success_break = false; @@ -322,11 +346,9 @@ void GcodeSuite::G34() { // Restore the active tool after homing #if HOTENDS > 1 - tool_change(old_tool_index, ( + tool_change(old_tool_index, (true #if ENABLED(PARKING_EXTRUDER) - false // Fetch the previous toolhead - #else - true + && false // Fetch the previous toolhead #endif )); #endif @@ -367,10 +389,10 @@ void GcodeSuite::G34() { void GcodeSuite::M422() { if (!parser.seen_any()) { - for (uint8_t i = 0; i < NUM_Z_STEPPER_DRIVERS; ++i) + LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) SERIAL_ECHOLNPAIR_P(PSTR("M422 S"), i + '1', SP_X_STR, z_stepper_align.xy[i].x, SP_Y_STR, z_stepper_align.xy[i].y); #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) - for (uint8_t i = 0; i < NUM_Z_STEPPER_DRIVERS; ++i) + LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) SERIAL_ECHOLNPAIR_P(PSTR("M422 W"), i + '1', SP_X_STR, z_stepper_align.stepper_xy[i].x, SP_Y_STR, z_stepper_align.stepper_xy[i].y); #endif return;