Fix G34 "Decreasing accuracy" bug (#17013)

Co-authored-by: Scott Lahteine <thinkyhead@users.noreply.github.com>

Marlin/src/gcode/calibrate/G34_M422.cpp

     // 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;
 
       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));
             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;
 
         // 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]);
         }
         #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);
         #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;
 
     // 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
 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;