Quad Z leveling, G34 (R)ecalculate (#17122)

Marlin/src/gcode/calibrate/G34_M422.cpp

  *   I<iterations>
  *   T<accuracy>
  *   A<amplification>
+ *   R<recalculate> points based on current probe offsets
  */
 void GcodeSuite::G34() {
   if (DEBUGGING(LEVELING)) {
 
   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
 
         }
       #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!
 
     // 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
     );
 
     // 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
         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);
         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)
 
     } // 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();
     #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
       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");
  * 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);

Marlin/src/inc/SanityCheck.h

     #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