Initial commit

13 files changed, 3950 insertions, 0 deletions

diff --git a/Marlin/src/gcode/calibrate/G28.cpp b/Marlin/src/gcode/calibrate/G28.cpp
new file mode 100644
index 0000000..49dee87
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/G28.cpp
@@ -0,0 +1,493 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#include "../gcode.h"
+
+#include "../../module/stepper.h"
+#include "../../module/endstops.h"
+
+#if HAS_MULTI_HOTEND
+  #include "../../module/tool_change.h"
+#endif
+
+#if HAS_LEVELING
+  #include "../../feature/bedlevel/bedlevel.h"
+#endif
+
+#if ENABLED(SENSORLESS_HOMING)
+  #include "../../feature/tmc_util.h"
+#endif
+
+#include "../../module/probe.h"
+
+#if ENABLED(BLTOUCH)
+  #include "../../feature/bltouch.h"
+#endif
+
+#include "../../lcd/marlinui.h"
+#if ENABLED(DWIN_CREALITY_LCD)
+  #include "../../lcd/dwin/e3v2/dwin.h"
+#endif
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../../lcd/extui/ui_api.h"
+#endif
+
+#if HAS_L64XX                         // set L6470 absolute position registers to counts
+  #include "../../libs/L64XX/L64XX_Marlin.h"
+#endif
+
+#if ENABLED(LASER_MOVE_G28_OFF)
+  #include "../../feature/spindle_laser.h"
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../../core/debug_out.h"
+
+#if ENABLED(QUICK_HOME)
+
+  static void quick_home_xy() {
+
+    // Pretend the current position is 0,0
+    current_position.set(0.0, 0.0);
+    sync_plan_position();
+
+    const int x_axis_home_dir = x_home_dir(active_extruder);
+
+    const float mlx = max_length(X_AXIS),
+                mly = max_length(Y_AXIS),
+                mlratio = mlx > mly ? mly / mlx : mlx / mly,
+                fr_mm_s = _MIN(homing_feedrate(X_AXIS), homing_feedrate(Y_AXIS)) * SQRT(sq(mlratio) + 1.0);
+
+    #if ENABLED(SENSORLESS_HOMING)
+      sensorless_t stealth_states {
+          tmc_enable_stallguard(stepperX)
+        , tmc_enable_stallguard(stepperY)
+        , false
+        , false
+          #if AXIS_HAS_STALLGUARD(X2)
+            || tmc_enable_stallguard(stepperX2)
+          #endif
+        , false
+          #if AXIS_HAS_STALLGUARD(Y2)
+            || tmc_enable_stallguard(stepperY2)
+          #endif
+      };
+    #endif
+
+    do_blocking_move_to_xy(1.5 * mlx * x_axis_home_dir, 1.5 * mly * home_dir(Y_AXIS), fr_mm_s);
+
+    endstops.validate_homing_move();
+
+    current_position.set(0.0, 0.0);
+
+    #if ENABLED(SENSORLESS_HOMING)
+      tmc_disable_stallguard(stepperX, stealth_states.x);
+      tmc_disable_stallguard(stepperY, stealth_states.y);
+      #if AXIS_HAS_STALLGUARD(X2)
+        tmc_disable_stallguard(stepperX2, stealth_states.x2);
+      #endif
+      #if AXIS_HAS_STALLGUARD(Y2)
+        tmc_disable_stallguard(stepperY2, stealth_states.y2);
+      #endif
+    #endif
+  }
+
+#endif // QUICK_HOME
+
+#if ENABLED(Z_SAFE_HOMING)
+
+  inline void home_z_safely() {
+    DEBUG_SECTION(log_G28, "home_z_safely", DEBUGGING(LEVELING));
+
+    // Disallow Z homing if X or Y homing is needed
+    if (homing_needed_error(_BV(X_AXIS) | _BV(Y_AXIS))) return;
+
+    sync_plan_position();
+
+    /**
+     * Move the Z probe (or just the nozzle) to the safe homing point
+     * (Z is already at the right height)
+     */
+    constexpr xy_float_t safe_homing_xy = { Z_SAFE_HOMING_X_POINT, Z_SAFE_HOMING_Y_POINT };
+    #if HAS_HOME_OFFSET
+      xy_float_t okay_homing_xy = safe_homing_xy;
+      okay_homing_xy -= home_offset;
+    #else
+      constexpr xy_float_t okay_homing_xy = safe_homing_xy;
+    #endif
+
+    destination.set(okay_homing_xy, current_position.z);
+
+    TERN_(HOMING_Z_WITH_PROBE, destination -= probe.offset_xy);
+
+    if (position_is_reachable(destination)) {
+
+      if (DEBUGGING(LEVELING)) DEBUG_POS("home_z_safely", destination);
+
+      // Free the active extruder for movement
+      TERN_(DUAL_X_CARRIAGE, idex_set_parked(false));
+
+      TERN_(SENSORLESS_HOMING, safe_delay(500)); // Short delay needed to settle
+
+      do_blocking_move_to_xy(destination);
+      homeaxis(Z_AXIS);
+    }
+    else {
+      LCD_MESSAGEPGM(MSG_ZPROBE_OUT);
+      SERIAL_ECHO_MSG(STR_ZPROBE_OUT_SER);
+    }
+  }
+
+#endif // Z_SAFE_HOMING
+
+#if ENABLED(IMPROVE_HOMING_RELIABILITY)
+
+  slow_homing_t begin_slow_homing() {
+    slow_homing_t slow_homing{0};
+    slow_homing.acceleration.set(planner.settings.max_acceleration_mm_per_s2[X_AXIS],
+                                 planner.settings.max_acceleration_mm_per_s2[Y_AXIS]);
+    planner.settings.max_acceleration_mm_per_s2[X_AXIS] = 100;
+    planner.settings.max_acceleration_mm_per_s2[Y_AXIS] = 100;
+    #if HAS_CLASSIC_JERK
+      slow_homing.jerk_xy = planner.max_jerk;
+      planner.max_jerk.set(0, 0);
+    #endif
+    planner.reset_acceleration_rates();
+    return slow_homing;
+  }
+
+  void end_slow_homing(const slow_homing_t &slow_homing) {
+    planner.settings.max_acceleration_mm_per_s2[X_AXIS] = slow_homing.acceleration.x;
+    planner.settings.max_acceleration_mm_per_s2[Y_AXIS] = slow_homing.acceleration.y;
+    TERN_(HAS_CLASSIC_JERK, planner.max_jerk = slow_homing.jerk_xy);
+    planner.reset_acceleration_rates();
+  }
+
+#endif // IMPROVE_HOMING_RELIABILITY
+
+/**
+ * G28: Home all axes according to settings
+ *
+ * Parameters
+ *
+ *  None  Home to all axes with no parameters.
+ *        With QUICK_HOME enabled XY will home together, then Z.
+ *
+ *  O   Home only if position is unknown
+ *
+ *  Rn  Raise by n mm/inches before homing
+ *
+ * Cartesian/SCARA parameters
+ *
+ *  X   Home to the X endstop
+ *  Y   Home to the Y endstop
+ *  Z   Home to the Z endstop
+ */
+void GcodeSuite::G28() {
+  DEBUG_SECTION(log_G28, "G28", DEBUGGING(LEVELING));
+  if (DEBUGGING(LEVELING)) log_machine_info();
+
+  TERN_(LASER_MOVE_G28_OFF, cutter.set_inline_enabled(false));  // turn off laser
+
+  #if ENABLED(DUAL_X_CARRIAGE)
+    bool IDEX_saved_duplication_state = extruder_duplication_enabled;
+    DualXMode IDEX_saved_mode = dual_x_carriage_mode;
+  #endif
+
+  #if ENABLED(MARLIN_DEV_MODE)
+    if (parser.seen('S')) {
+      LOOP_XYZ(a) set_axis_is_at_home((AxisEnum)a);
+      sync_plan_position();
+      SERIAL_ECHOLNPGM("Simulated Homing");
+      report_current_position();
+      return;
+    }
+  #endif
+
+  // Home (O)nly if position is unknown
+  if (!axes_should_home() && parser.boolval('O')) {
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> homing not needed, skip");
+    return;
+  }
+
+  TERN_(DWIN_CREALITY_LCD, DWIN_StartHoming());
+  TERN_(EXTENSIBLE_UI, ExtUI::onHomingStart());
+
+  planner.synchronize();          // Wait for planner moves to finish!
+
+  SET_SOFT_ENDSTOP_LOOSE(false);  // Reset a leftover 'loose' motion state
+
+  // Disable the leveling matrix before homing
+  #if HAS_LEVELING
+    const bool leveling_restore_state = parser.boolval('L', TERN(RESTORE_LEVELING_AFTER_G28, planner.leveling_active, ENABLED(ENABLE_LEVELING_AFTER_G28)));
+    IF_ENABLED(PROBE_MANUALLY, g29_in_progress = false); // Cancel the active G29 session
+    set_bed_leveling_enabled(false);
+  #endif
+
+  // Reset to the XY plane
+  TERN_(CNC_WORKSPACE_PLANES, workspace_plane = PLANE_XY);
+
+  // Count this command as movement / activity
+  reset_stepper_timeout();
+
+  #define HAS_CURRENT_HOME(N) (defined(N##_CURRENT_HOME) && N##_CURRENT_HOME != N##_CURRENT)
+  #if HAS_CURRENT_HOME(X) || HAS_CURRENT_HOME(X2) || HAS_CURRENT_HOME(Y) || HAS_CURRENT_HOME(Y2)
+    #define HAS_HOMING_CURRENT 1
+  #endif
+
+  #if HAS_HOMING_CURRENT
+    auto debug_current = [](PGM_P const s, const int16_t a, const int16_t b){
+      serialprintPGM(s); DEBUG_ECHOLNPAIR(" current: ", a, " -> ", b);
+    };
+    #if HAS_CURRENT_HOME(X)
+      const int16_t tmc_save_current_X = stepperX.getMilliamps();
+      stepperX.rms_current(X_CURRENT_HOME);
+      if (DEBUGGING(LEVELING)) debug_current(PSTR("X"), tmc_save_current_X, X_CURRENT_HOME);
+    #endif
+    #if HAS_CURRENT_HOME(X2)
+      const int16_t tmc_save_current_X2 = stepperX2.getMilliamps();
+      stepperX2.rms_current(X2_CURRENT_HOME);
+      if (DEBUGGING(LEVELING)) debug_current(PSTR("X2"), tmc_save_current_X2, X2_CURRENT_HOME);
+    #endif
+    #if HAS_CURRENT_HOME(Y)
+      const int16_t tmc_save_current_Y = stepperY.getMilliamps();
+      stepperY.rms_current(Y_CURRENT_HOME);
+      if (DEBUGGING(LEVELING)) debug_current(PSTR("Y"), tmc_save_current_Y, Y_CURRENT_HOME);
+    #endif
+    #if HAS_CURRENT_HOME(Y2)
+      const int16_t tmc_save_current_Y2 = stepperY2.getMilliamps();
+      stepperY2.rms_current(Y2_CURRENT_HOME);
+      if (DEBUGGING(LEVELING)) debug_current(PSTR("Y2"), tmc_save_current_Y2, Y2_CURRENT_HOME);
+    #endif
+  #endif
+
+  TERN_(IMPROVE_HOMING_RELIABILITY, slow_homing_t slow_homing = begin_slow_homing());
+
+  // Always home with tool 0 active
+  #if HAS_MULTI_HOTEND
+    #if DISABLED(DELTA) || ENABLED(DELTA_HOME_TO_SAFE_ZONE)
+      const uint8_t old_tool_index = active_extruder;
+    #endif
+    // PARKING_EXTRUDER homing requires different handling of movement / solenoid activation, depending on the side of homing
+    #if ENABLED(PARKING_EXTRUDER)
+      const bool pe_final_change_must_unpark = parking_extruder_unpark_after_homing(old_tool_index, X_HOME_DIR + 1 == old_tool_index * 2);
+    #endif
+    tool_change(0, true);
+  #endif
+
+  TERN_(HAS_DUPLICATION_MODE, set_duplication_enabled(false));
+
+  remember_feedrate_scaling_off();
+
+  endstops.enable(true); // Enable endstops for next homing move
+
+  #if ENABLED(DELTA)
+
+    constexpr bool doZ = true; // for NANODLP_Z_SYNC if your DLP is on a DELTA
+
+    home_delta();
+
+    TERN_(IMPROVE_HOMING_RELIABILITY, end_slow_homing(slow_homing));
+
+  #else // NOT DELTA
+
+    const bool homeZ = parser.seen('Z'),
+               needX = homeZ && TERN0(Z_SAFE_HOMING, axes_should_home(_BV(X_AXIS))),
+               needY = homeZ && TERN0(Z_SAFE_HOMING, axes_should_home(_BV(Y_AXIS))),
+               homeX = needX || parser.seen('X'), homeY = needY || parser.seen('Y'),
+               home_all = homeX == homeY && homeX == homeZ, // All or None
+               doX = home_all || homeX, doY = home_all || homeY, doZ = home_all || homeZ;
+
+    #if ENABLED(HOME_Z_FIRST)
+
+      if (doZ) homeaxis(Z_AXIS);
+
+    #endif
+
+    const float z_homing_height = TERN1(UNKNOWN_Z_NO_RAISE, axis_is_trusted(Z_AXIS))
+                                  ? (parser.seenval('R') ? parser.value_linear_units() : Z_HOMING_HEIGHT)
+                                  : 0;
+
+    if (z_homing_height && (doX || doY || TERN0(Z_SAFE_HOMING, doZ))) {
+      // Raise Z before homing any other axes and z is not already high enough (never lower z)
+      if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Raise Z (before homing) by ", z_homing_height);
+      do_z_clearance(z_homing_height, axis_is_trusted(Z_AXIS), DISABLED(UNKNOWN_Z_NO_RAISE));
+    }
+
+    #if ENABLED(QUICK_HOME)
+
+      if (doX && doY) quick_home_xy();
+
+    #endif
+
+    // Home Y (before X)
+    if (ENABLED(HOME_Y_BEFORE_X) && (doY || TERN0(CODEPENDENT_XY_HOMING, doX)))
+      homeaxis(Y_AXIS);
+
+    // Home X
+    if (doX || (doY && ENABLED(CODEPENDENT_XY_HOMING) && DISABLED(HOME_Y_BEFORE_X))) {
+
+      #if ENABLED(DUAL_X_CARRIAGE)
+
+        // Always home the 2nd (right) extruder first
+        active_extruder = 1;
+        homeaxis(X_AXIS);
+
+        // Remember this extruder's position for later tool change
+        inactive_extruder_x = current_position.x;
+
+        // Home the 1st (left) extruder
+        active_extruder = 0;
+        homeaxis(X_AXIS);
+
+        // Consider the active extruder to be in its "parked" position
+        idex_set_parked();
+
+      #else
+
+        homeaxis(X_AXIS);
+
+      #endif
+    }
+
+    // Home Y (after X)
+    if (DISABLED(HOME_Y_BEFORE_X) && doY)
+      homeaxis(Y_AXIS);
+
+    TERN_(IMPROVE_HOMING_RELIABILITY, end_slow_homing(slow_homing));
+
+    // Home Z last if homing towards the bed
+    #if DISABLED(HOME_Z_FIRST)
+      if (doZ) {
+        #if EITHER(Z_MULTI_ENDSTOPS, Z_STEPPER_AUTO_ALIGN)
+          stepper.set_all_z_lock(false);
+          stepper.set_separate_multi_axis(false);
+        #endif
+
+        TERN_(BLTOUCH, bltouch.init());
+        TERN(Z_SAFE_HOMING, home_z_safely(), homeaxis(Z_AXIS));
+        probe.move_z_after_homing();
+      }
+    #endif
+
+    sync_plan_position();
+
+  #endif // !DELTA (G28)
+
+  /**
+   * Preserve DXC mode across a G28 for IDEX printers in DXC_DUPLICATION_MODE.
+   * This is important because it lets a user use the LCD Panel to set an IDEX Duplication mode, and
+   * then print a standard GCode file that contains a single print that does a G28 and has no other
+   * IDEX specific commands in it.
+   */
+  #if ENABLED(DUAL_X_CARRIAGE)
+
+    if (idex_is_duplicating()) {
+
+      TERN_(IMPROVE_HOMING_RELIABILITY, slow_homing = begin_slow_homing());
+
+      // Always home the 2nd (right) extruder first
+      active_extruder = 1;
+      homeaxis(X_AXIS);
+
+      // Remember this extruder's position for later tool change
+      inactive_extruder_x = current_position.x;
+
+      // Home the 1st (left) extruder
+      active_extruder = 0;
+      homeaxis(X_AXIS);
+
+      // Consider the active extruder to be parked
+      idex_set_parked();
+
+      dual_x_carriage_mode = IDEX_saved_mode;
+      set_duplication_enabled(IDEX_saved_duplication_state);
+
+      TERN_(IMPROVE_HOMING_RELIABILITY, end_slow_homing(slow_homing));
+    }
+
+  #endif // DUAL_X_CARRIAGE
+
+  endstops.not_homing();
+
+  // Clear endstop state for polled stallGuard endstops
+  TERN_(SPI_ENDSTOPS, endstops.clear_endstop_state());
+
+  #if BOTH(DELTA, DELTA_HOME_TO_SAFE_ZONE)
+    // move to a height where we can use the full xy-area
+    do_blocking_move_to_z(delta_clip_start_height);
+  #endif
+
+  TERN_(HAS_LEVELING, set_bed_leveling_enabled(leveling_restore_state));
+
+  restore_feedrate_and_scaling();
+
+  // Restore the active tool after homing
+  #if HAS_MULTI_HOTEND && (DISABLED(DELTA) || ENABLED(DELTA_HOME_TO_SAFE_ZONE))
+    tool_change(old_tool_index, TERN(PARKING_EXTRUDER, !pe_final_change_must_unpark, DISABLED(DUAL_X_CARRIAGE)));   // Do move if one of these
+  #endif
+
+  #if HAS_HOMING_CURRENT
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Restore driver current...");
+    #if HAS_CURRENT_HOME(X)
+      stepperX.rms_current(tmc_save_current_X);
+    #endif
+    #if HAS_CURRENT_HOME(X2)
+      stepperX2.rms_current(tmc_save_current_X2);
+    #endif
+    #if HAS_CURRENT_HOME(Y)
+      stepperY.rms_current(tmc_save_current_Y);
+    #endif
+    #if HAS_CURRENT_HOME(Y2)
+      stepperY2.rms_current(tmc_save_current_Y2);
+    #endif
+  #endif
+
+  ui.refresh();
+
+  TERN_(DWIN_CREALITY_LCD, DWIN_CompletedHoming());
+  TERN_(EXTENSIBLE_UI, ExtUI::onHomingComplete());
+
+  report_current_position();
+
+  if (ENABLED(NANODLP_Z_SYNC) && (doZ || ENABLED(NANODLP_ALL_AXIS)))
+    SERIAL_ECHOLNPGM(STR_Z_MOVE_COMP);
+
+  #if HAS_L64XX
+    // Set L6470 absolute position registers to counts
+    // constexpr *might* move this to PROGMEM.
+    // If not, this will need a PROGMEM directive and an accessor.
+    static constexpr AxisEnum L64XX_axis_xref[MAX_L64XX] = {
+      X_AXIS, Y_AXIS, Z_AXIS,
+      X_AXIS, Y_AXIS, Z_AXIS, Z_AXIS,
+      E_AXIS, E_AXIS, E_AXIS, E_AXIS, E_AXIS, E_AXIS
+    };
+    for (uint8_t j = 1; j <= L64XX::chain[0]; j++) {
+      const uint8_t cv = L64XX::chain[j];
+      L64xxManager.set_param((L64XX_axis_t)cv, L6470_ABS_POS, stepper.position(L64XX_axis_xref[cv]));
+    }
+  #endif
+  TERN_(HAS_LEVELING, set_bed_leveling_enabled(true));
+}
diff --git a/Marlin/src/gcode/calibrate/G33.cpp b/Marlin/src/gcode/calibrate/G33.cpp
new file mode 100644
index 0000000..77cc457
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/G33.cpp
@@ -0,0 +1,648 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(DELTA_AUTO_CALIBRATION)
+
+#include "../gcode.h"
+#include "../../module/delta.h"
+#include "../../module/motion.h"
+#include "../../module/stepper.h"
+#include "../../module/endstops.h"
+#include "../../lcd/marlinui.h"
+
+#if HAS_BED_PROBE
+  #include "../../module/probe.h"
+#endif
+
+#if HAS_MULTI_HOTEND
+  #include "../../module/tool_change.h"
+#endif
+
+#if HAS_LEVELING
+  #include "../../feature/bedlevel/bedlevel.h"
+#endif
+
+constexpr uint8_t _7P_STEP = 1,              // 7-point step - to change number of calibration points
+                  _4P_STEP = _7P_STEP * 2,   // 4-point step
+                  NPP      = _7P_STEP * 6;   // number of calibration points on the radius
+enum CalEnum : char {                        // the 7 main calibration points - add definitions if needed
+  CEN      = 0,
+  __A      = 1,
+  _AB      = __A + _7P_STEP,
+  __B      = _AB + _7P_STEP,
+  _BC      = __B + _7P_STEP,
+  __C      = _BC + _7P_STEP,
+  _CA      = __C + _7P_STEP,
+};
+
+#define LOOP_CAL_PT(VAR, S, N) for (uint8_t VAR=S; VAR<=NPP; VAR+=N)
+#define F_LOOP_CAL_PT(VAR, S, N) for (float VAR=S; VAR<NPP+0.9999; VAR+=N)
+#define I_LOOP_CAL_PT(VAR, S, N) for (float VAR=S; VAR>CEN+0.9999; VAR-=N)
+#define LOOP_CAL_ALL(VAR) LOOP_CAL_PT(VAR, CEN, 1)
+#define LOOP_CAL_RAD(VAR) LOOP_CAL_PT(VAR, __A, _7P_STEP)
+#define LOOP_CAL_ACT(VAR, _4P, _OP) LOOP_CAL_PT(VAR, _OP ? _AB : __A, _4P ? _4P_STEP : _7P_STEP)
+
+TERN_(HAS_MULTI_HOTEND, const uint8_t old_tool_index = active_extruder);
+
+float lcd_probe_pt(const xy_pos_t &xy);
+
+void ac_home() {
+  endstops.enable(true);
+  home_delta();
+  endstops.not_homing();
+}
+
+void ac_setup(const bool reset_bed) {
+  TERN_(HAS_MULTI_HOTEND, tool_change(0, true));
+
+  planner.synchronize();
+  remember_feedrate_scaling_off();
+
+  #if HAS_LEVELING
+    if (reset_bed) reset_bed_level(); // After full calibration bed-level data is no longer valid
+  #endif
+}
+
+void ac_cleanup(TERN_(HAS_MULTI_HOTEND, const uint8_t old_tool_index)) {
+  TERN_(DELTA_HOME_TO_SAFE_ZONE, do_blocking_move_to_z(delta_clip_start_height));
+  TERN_(HAS_BED_PROBE, probe.stow());
+  restore_feedrate_and_scaling();
+  TERN_(HAS_MULTI_HOTEND, tool_change(old_tool_index, true));
+}
+
+void print_signed_float(PGM_P const prefix, const float &f) {
+  SERIAL_ECHOPGM("  ");
+  serialprintPGM(prefix);
+  SERIAL_CHAR(':');
+  if (f >= 0) SERIAL_CHAR('+');
+  SERIAL_ECHO_F(f, 2);
+}
+
+/**
+ *  - Print the delta settings
+ */
+static void print_calibration_settings(const bool end_stops, const bool tower_angles) {
+  SERIAL_ECHOPAIR(".Height:", delta_height);
+  if (end_stops) {
+    print_signed_float(PSTR("Ex"), delta_endstop_adj.a);
+    print_signed_float(PSTR("Ey"), delta_endstop_adj.b);
+    print_signed_float(PSTR("Ez"), delta_endstop_adj.c);
+  }
+  if (end_stops && tower_angles) {
+    SERIAL_ECHOPAIR("  Radius:", delta_radius);
+    SERIAL_EOL();
+    SERIAL_CHAR('.');
+    SERIAL_ECHO_SP(13);
+  }
+  if (tower_angles) {
+    print_signed_float(PSTR("Tx"), delta_tower_angle_trim.a);
+    print_signed_float(PSTR("Ty"), delta_tower_angle_trim.b);
+    print_signed_float(PSTR("Tz"), delta_tower_angle_trim.c);
+  }
+  if ((!end_stops && tower_angles) || (end_stops && !tower_angles)) { // XOR
+    SERIAL_ECHOPAIR("  Radius:", delta_radius);
+  }
+  SERIAL_EOL();
+}
+
+/**
+ *  - Print the probe results
+ */
+static void print_calibration_results(const float z_pt[NPP + 1], const bool tower_points, const bool opposite_points) {
+  SERIAL_ECHOPGM(".    ");
+  print_signed_float(PSTR("c"), z_pt[CEN]);
+  if (tower_points) {
+    print_signed_float(PSTR(" x"), z_pt[__A]);
+    print_signed_float(PSTR(" y"), z_pt[__B]);
+    print_signed_float(PSTR(" z"), z_pt[__C]);
+  }
+  if (tower_points && opposite_points) {
+    SERIAL_EOL();
+    SERIAL_CHAR('.');
+    SERIAL_ECHO_SP(13);
+  }
+  if (opposite_points) {
+    print_signed_float(PSTR("yz"), z_pt[_BC]);
+    print_signed_float(PSTR("zx"), z_pt[_CA]);
+    print_signed_float(PSTR("xy"), z_pt[_AB]);
+  }
+  SERIAL_EOL();
+}
+
+/**
+ *  - Calculate the standard deviation from the zero plane
+ */
+static float std_dev_points(float z_pt[NPP + 1], const bool _0p_cal, const bool _1p_cal, const bool _4p_cal, const bool _4p_opp) {
+  if (!_0p_cal) {
+    float S2 = sq(z_pt[CEN]);
+    int16_t N = 1;
+    if (!_1p_cal) { // std dev from zero plane
+      LOOP_CAL_ACT(rad, _4p_cal, _4p_opp) {
+        S2 += sq(z_pt[rad]);
+        N++;
+      }
+      return LROUND(SQRT(S2 / N) * 1000.0f) / 1000.0f + 0.00001f;
+    }
+  }
+  return 0.00001f;
+}
+
+/**
+ *  - Probe a point
+ */
+static float calibration_probe(const xy_pos_t &xy, const bool stow) {
+  #if HAS_BED_PROBE
+    return probe.probe_at_point(xy, stow ? PROBE_PT_STOW : PROBE_PT_RAISE, 0, true, false);
+  #else
+    UNUSED(stow);
+    return lcd_probe_pt(xy);
+  #endif
+}
+
+/**
+ *  - Probe a grid
+ */
+static bool probe_calibration_points(float z_pt[NPP + 1], const int8_t probe_points, const bool towers_set, const bool stow_after_each) {
+  const bool _0p_calibration      = probe_points == 0,
+             _1p_calibration      = probe_points == 1 || probe_points == -1,
+             _4p_calibration      = probe_points == 2,
+             _4p_opposite_points  = _4p_calibration && !towers_set,
+             _7p_calibration      = probe_points >= 3,
+             _7p_no_intermediates = probe_points == 3,
+             _7p_1_intermediates  = probe_points == 4,
+             _7p_2_intermediates  = probe_points == 5,
+             _7p_4_intermediates  = probe_points == 6,
+             _7p_6_intermediates  = probe_points == 7,
+             _7p_8_intermediates  = probe_points == 8,
+             _7p_11_intermediates = probe_points == 9,
+             _7p_14_intermediates = probe_points == 10,
+             _7p_intermed_points  = probe_points >= 4,
+             _7p_6_center         = probe_points >= 5 && probe_points <= 7,
+             _7p_9_center         = probe_points >= 8;
+
+  LOOP_CAL_ALL(rad) z_pt[rad] = 0.0f;
+
+  if (!_0p_calibration) {
+
+    const float dcr = delta_calibration_radius();
+
+    if (!_7p_no_intermediates && !_7p_4_intermediates && !_7p_11_intermediates) { // probe the center
+      const xy_pos_t center{0};
+      z_pt[CEN] += calibration_probe(center, stow_after_each);
+      if (isnan(z_pt[CEN])) return false;
+    }
+
+    if (_7p_calibration) { // probe extra center points
+      const float start  = _7p_9_center ? float(_CA) + _7P_STEP / 3.0f : _7p_6_center ? float(_CA) : float(__C),
+                  steps  = _7p_9_center ? _4P_STEP / 3.0f : _7p_6_center ? _7P_STEP : _4P_STEP;
+      I_LOOP_CAL_PT(rad, start, steps) {
+        const float a = RADIANS(210 + (360 / NPP) *  (rad - 1)),
+                    r = dcr * 0.1;
+        const xy_pos_t vec = { cos(a), sin(a) };
+        z_pt[CEN] += calibration_probe(vec * r, stow_after_each);
+        if (isnan(z_pt[CEN])) return false;
+     }
+      z_pt[CEN] /= float(_7p_2_intermediates ? 7 : probe_points);
+    }
+
+    if (!_1p_calibration) {  // probe the radius
+      const CalEnum start  = _4p_opposite_points ? _AB : __A;
+      const float   steps  = _7p_14_intermediates ? _7P_STEP / 15.0f : // 15r * 6 + 10c = 100
+                             _7p_11_intermediates ? _7P_STEP / 12.0f : // 12r * 6 +  9c = 81
+                             _7p_8_intermediates  ? _7P_STEP /  9.0f : //  9r * 6 + 10c = 64
+                             _7p_6_intermediates  ? _7P_STEP /  7.0f : //  7r * 6 +  7c = 49
+                             _7p_4_intermediates  ? _7P_STEP /  5.0f : //  5r * 6 +  6c = 36
+                             _7p_2_intermediates  ? _7P_STEP /  3.0f : //  3r * 6 +  7c = 25
+                             _7p_1_intermediates  ? _7P_STEP /  2.0f : //  2r * 6 +  4c = 16
+                             _7p_no_intermediates ? _7P_STEP :        //  1r * 6 +  3c = 9
+                             _4P_STEP;                                // .5r * 6 +  1c = 4
+      bool zig_zag = true;
+      F_LOOP_CAL_PT(rad, start, _7p_9_center ? steps * 3 : steps) {
+        const int8_t offset = _7p_9_center ? 2 : 0;
+        for (int8_t circle = 0; circle <= offset; circle++) {
+          const float a = RADIANS(210 + (360 / NPP) *  (rad - 1)),
+                      r = dcr * (1 - 0.1 * (zig_zag ? offset - circle : circle)),
+                      interpol = FMOD(rad, 1);
+          const xy_pos_t vec = { cos(a), sin(a) };
+          const float z_temp = calibration_probe(vec * r, stow_after_each);
+          if (isnan(z_temp)) return false;
+          // split probe point to neighbouring calibration points
+          z_pt[uint8_t(LROUND(rad - interpol + NPP - 1)) % NPP + 1] += z_temp * sq(cos(RADIANS(interpol * 90)));
+          z_pt[uint8_t(LROUND(rad - interpol))           % NPP + 1] += z_temp * sq(sin(RADIANS(interpol * 90)));
+        }
+        zig_zag = !zig_zag;
+      }
+      if (_7p_intermed_points)
+        LOOP_CAL_RAD(rad)
+          z_pt[rad] /= _7P_STEP / steps;
+
+      do_blocking_move_to_xy(0.0f, 0.0f);
+    }
+  }
+  return true;
+}
+
+/**
+ * kinematics routines and auto tune matrix scaling parameters:
+ * see https://github.com/LVD-AC/Marlin-AC/tree/1.1.x-AC/documentation for
+ *  - formulae for approximative forward kinematics in the end-stop displacement matrix
+ *  - definition of the matrix scaling parameters
+ */
+static void reverse_kinematics_probe_points(float z_pt[NPP + 1], abc_float_t mm_at_pt_axis[NPP + 1]) {
+  xyz_pos_t pos{0};
+
+  const float dcr = delta_calibration_radius();
+  LOOP_CAL_ALL(rad) {
+    const float a = RADIANS(210 + (360 / NPP) *  (rad - 1)),
+                r = (rad == CEN ? 0.0f : dcr);
+    pos.set(cos(a) * r, sin(a) * r, z_pt[rad]);
+    inverse_kinematics(pos);
+    mm_at_pt_axis[rad] = delta;
+  }
+}
+
+static void forward_kinematics_probe_points(abc_float_t mm_at_pt_axis[NPP + 1], float z_pt[NPP + 1]) {
+  const float r_quot = delta_calibration_radius() / delta_radius;
+
+  #define ZPP(N,I,A) (((1.0f + r_quot * (N)) / 3.0f) * mm_at_pt_axis[I].A)
+  #define Z00(I, A) ZPP( 0, I, A)
+  #define Zp1(I, A) ZPP(+1, I, A)
+  #define Zm1(I, A) ZPP(-1, I, A)
+  #define Zp2(I, A) ZPP(+2, I, A)
+  #define Zm2(I, A) ZPP(-2, I, A)
+
+  z_pt[CEN] = Z00(CEN, a) + Z00(CEN, b) + Z00(CEN, c);
+  z_pt[__A] = Zp2(__A, a) + Zm1(__A, b) + Zm1(__A, c);
+  z_pt[__B] = Zm1(__B, a) + Zp2(__B, b) + Zm1(__B, c);
+  z_pt[__C] = Zm1(__C, a) + Zm1(__C, b) + Zp2(__C, c);
+  z_pt[_BC] = Zm2(_BC, a) + Zp1(_BC, b) + Zp1(_BC, c);
+  z_pt[_CA] = Zp1(_CA, a) + Zm2(_CA, b) + Zp1(_CA, c);
+  z_pt[_AB] = Zp1(_AB, a) + Zp1(_AB, b) + Zm2(_AB, c);
+}
+
+static void calc_kinematics_diff_probe_points(float z_pt[NPP + 1], abc_float_t delta_e, const float delta_r, abc_float_t delta_t) {
+  const float z_center = z_pt[CEN];
+  abc_float_t diff_mm_at_pt_axis[NPP + 1], new_mm_at_pt_axis[NPP + 1];
+
+  reverse_kinematics_probe_points(z_pt, diff_mm_at_pt_axis);
+
+  delta_radius += delta_r;
+  delta_tower_angle_trim += delta_t;
+  recalc_delta_settings();
+  reverse_kinematics_probe_points(z_pt, new_mm_at_pt_axis);
+
+  LOOP_CAL_ALL(rad) diff_mm_at_pt_axis[rad] -= new_mm_at_pt_axis[rad] + delta_e;
+  forward_kinematics_probe_points(diff_mm_at_pt_axis, z_pt);
+
+  LOOP_CAL_RAD(rad) z_pt[rad] -= z_pt[CEN] - z_center;
+  z_pt[CEN] = z_center;
+
+  delta_radius -= delta_r;
+  delta_tower_angle_trim -= delta_t;
+  recalc_delta_settings();
+}
+
+static float auto_tune_h() {
+  const float r_quot = delta_calibration_radius() / delta_radius;
+  return RECIPROCAL(r_quot / (2.0f / 3.0f));  // (2/3)/CR
+}
+
+static float auto_tune_r() {
+  constexpr float diff = 0.01f, delta_r = diff;
+  float r_fac = 0.0f, z_pt[NPP + 1] = { 0.0f };
+  abc_float_t delta_e = { 0.0f }, delta_t = { 0.0f };
+
+  calc_kinematics_diff_probe_points(z_pt, delta_e, delta_r, delta_t);
+  r_fac = -(z_pt[__A] + z_pt[__B] + z_pt[__C] + z_pt[_BC] + z_pt[_CA] + z_pt[_AB]) / 6.0f;
+  r_fac = diff / r_fac / 3.0f; // 1/(3*delta_Z)
+  return r_fac;
+}
+
+static float auto_tune_a() {
+  constexpr float diff = 0.01f, delta_r = 0.0f;
+  float a_fac = 0.0f, z_pt[NPP + 1] = { 0.0f };
+  abc_float_t delta_e = { 0.0f }, delta_t = { 0.0f };
+
+  delta_t.reset();
+  LOOP_XYZ(axis) {
+    delta_t[axis] = diff;
+    calc_kinematics_diff_probe_points(z_pt, delta_e, delta_r, delta_t);
+    delta_t[axis] = 0;
+    a_fac += z_pt[uint8_t((axis * _4P_STEP) - _7P_STEP + NPP) % NPP + 1] / 6.0f;
+    a_fac -= z_pt[uint8_t((axis * _4P_STEP) + 1 + _7P_STEP)] / 6.0f;
+  }
+  a_fac = diff / a_fac / 3.0f; // 1/(3*delta_Z)
+  return a_fac;
+}
+
+/**
+ * G33 - Delta '1-4-7-point' Auto-Calibration
+ *       Calibrate height, z_offset, endstops, delta radius, and tower angles.
+ *
+ * Parameters:
+ *
+ *   Pn  Number of probe points:
+ *      P0       Normalizes calibration.
+ *      P1       Calibrates height only with center probe.
+ *      P2       Probe center and towers. Calibrate height, endstops and delta radius.
+ *      P3       Probe all positions: center, towers and opposite towers. Calibrate all.
+ *      P4-P10   Probe all positions at different intermediate locations and average them.
+ *
+ *   T   Don't calibrate tower angle corrections
+ *
+ *   Cn.nn  Calibration precision; when omitted calibrates to maximum precision
+ *
+ *   Fn  Force to run at least n iterations and take the best result
+ *
+ *   Vn  Verbose level:
+ *      V0  Dry-run mode. Report settings and probe results. No calibration.
+ *      V1  Report start and end settings only
+ *      V2  Report settings at each iteration
+ *      V3  Report settings and probe results
+ *
+ *   E   Engage the probe for each point
+ */
+void GcodeSuite::G33() {
+
+  const int8_t probe_points = parser.intval('P', DELTA_CALIBRATION_DEFAULT_POINTS);
+  if (!WITHIN(probe_points, 0, 10)) {
+    SERIAL_ECHOLNPGM("?(P)oints implausible (0-10).");
+    return;
+  }
+
+  const bool towers_set = !parser.seen('T');
+
+  const float calibration_precision = parser.floatval('C', 0.0f);
+  if (calibration_precision < 0) {
+    SERIAL_ECHOLNPGM("?(C)alibration precision implausible (>=0).");
+    return;
+  }
+
+  const int8_t force_iterations = parser.intval('F', 0);
+  if (!WITHIN(force_iterations, 0, 30)) {
+    SERIAL_ECHOLNPGM("?(F)orce iteration implausible (0-30).");
+    return;
+  }
+
+  const int8_t verbose_level = parser.byteval('V', 1);
+  if (!WITHIN(verbose_level, 0, 3)) {
+    SERIAL_ECHOLNPGM("?(V)erbose level implausible (0-3).");
+    return;
+  }
+
+  const bool stow_after_each = parser.seen('E');
+
+  const bool _0p_calibration      = probe_points == 0,
+             _1p_calibration      = probe_points == 1 || probe_points == -1,
+             _4p_calibration      = probe_points == 2,
+             _4p_opposite_points  = _4p_calibration && !towers_set,
+             _7p_9_center         = probe_points >= 8,
+             _tower_results       = (_4p_calibration && towers_set) || probe_points >= 3,
+             _opposite_results    = (_4p_calibration && !towers_set) || probe_points >= 3,
+             _endstop_results     = probe_points != 1 && probe_points != -1 && probe_points != 0,
+             _angle_results       = probe_points >= 3 && towers_set;
+  int8_t iterations = 0;
+  float test_precision,
+        zero_std_dev = (verbose_level ? 999.0f : 0.0f), // 0.0 in dry-run mode : forced end
+        zero_std_dev_min = zero_std_dev,
+        zero_std_dev_old = zero_std_dev,
+        h_factor, r_factor, a_factor,
+        r_old = delta_radius,
+        h_old = delta_height;
+
+  abc_pos_t e_old = delta_endstop_adj, a_old = delta_tower_angle_trim;
+
+  SERIAL_ECHOLNPGM("G33 Auto Calibrate");
+
+  const float dcr = delta_calibration_radius();
+
+  if (!_1p_calibration && !_0p_calibration) { // test if the outer radius is reachable
+    LOOP_CAL_RAD(axis) {
+      const float a = RADIANS(210 + (360 / NPP) *  (axis - 1));
+      if (!position_is_reachable(cos(a) * dcr, sin(a) * dcr)) {
+        SERIAL_ECHOLNPGM("?Bed calibration radius implausible.");
+        return;
+      }
+    }
+  }
+
+  // Report settings
+  PGM_P const checkingac = PSTR("Checking... AC");
+  serialprintPGM(checkingac);
+  if (verbose_level == 0) SERIAL_ECHOPGM(" (DRY-RUN)");
+  SERIAL_EOL();
+  ui.set_status_P(checkingac);
+
+  print_calibration_settings(_endstop_results, _angle_results);
+
+  ac_setup(!_0p_calibration && !_1p_calibration);
+
+  if (!_0p_calibration) ac_home();
+
+  do { // start iterations
+
+    float z_at_pt[NPP + 1] = { 0.0f };
+
+    test_precision = zero_std_dev_old != 999.0f ? (zero_std_dev + zero_std_dev_old) / 2.0f : zero_std_dev;
+    iterations++;
+
+    // Probe the points
+    zero_std_dev_old = zero_std_dev;
+    if (!probe_calibration_points(z_at_pt, probe_points, towers_set, stow_after_each)) {
+      SERIAL_ECHOLNPGM("Correct delta settings with M665 and M666");
+      return ac_cleanup(TERN_(HAS_MULTI_HOTEND, old_tool_index));
+    }
+    zero_std_dev = std_dev_points(z_at_pt, _0p_calibration, _1p_calibration, _4p_calibration, _4p_opposite_points);
+
+    // Solve matrices
+
+    if ((zero_std_dev < test_precision || iterations <= force_iterations) && zero_std_dev > calibration_precision) {
+
+      #if !HAS_BED_PROBE
+        test_precision = 0.0f; // forced end
+      #endif
+
+      if (zero_std_dev < zero_std_dev_min) {
+        // set roll-back point
+        e_old = delta_endstop_adj;
+        r_old = delta_radius;
+        h_old = delta_height;
+        a_old = delta_tower_angle_trim;
+      }
+
+      abc_float_t e_delta = { 0.0f }, t_delta = { 0.0f };
+      float r_delta = 0.0f;
+
+      /**
+       * convergence matrices:
+       * see https://github.com/LVD-AC/Marlin-AC/tree/1.1.x-AC/documentation for
+       *  - definition of the matrix scaling parameters
+       *  - matrices for 4 and 7 point calibration
+       */
+      #define ZP(N,I) ((N) * z_at_pt[I] / 4.0f) // 4.0 = divider to normalize to integers
+      #define Z12(I) ZP(12, I)
+      #define Z4(I) ZP(4, I)
+      #define Z2(I) ZP(2, I)
+      #define Z1(I) ZP(1, I)
+      #define Z0(I) ZP(0, I)
+
+      // calculate factors
+      if (_7p_9_center) calibration_radius_factor = 0.9f;
+      h_factor = auto_tune_h();
+      r_factor = auto_tune_r();
+      a_factor = auto_tune_a();
+      calibration_radius_factor = 1.0f;
+
+      switch (probe_points) {
+        case 0:
+          test_precision = 0.0f; // forced end
+          break;
+
+        case 1:
+          test_precision = 0.0f; // forced end
+          LOOP_XYZ(axis) e_delta[axis] = +Z4(CEN);
+          break;
+
+        case 2:
+          if (towers_set) { // see 4 point calibration (towers) matrix
+            e_delta.set((+Z4(__A) -Z2(__B) -Z2(__C)) * h_factor  +Z4(CEN),
+                        (-Z2(__A) +Z4(__B) -Z2(__C)) * h_factor  +Z4(CEN),
+                        (-Z2(__A) -Z2(__B) +Z4(__C)) * h_factor  +Z4(CEN));
+            r_delta   = (+Z4(__A) +Z4(__B) +Z4(__C) -Z12(CEN)) * r_factor;
+          }
+          else { // see 4 point calibration (opposites) matrix
+            e_delta.set((-Z4(_BC) +Z2(_CA) +Z2(_AB)) * h_factor  +Z4(CEN),
+                        (+Z2(_BC) -Z4(_CA) +Z2(_AB)) * h_factor  +Z4(CEN),
+                        (+Z2(_BC) +Z2(_CA) -Z4(_AB)) * h_factor  +Z4(CEN));
+            r_delta   = (+Z4(_BC) +Z4(_CA) +Z4(_AB) -Z12(CEN)) * r_factor;
+          }
+          break;
+
+        default: // see 7 point calibration (towers & opposites) matrix
+          e_delta.set((+Z2(__A) -Z1(__B) -Z1(__C) -Z2(_BC) +Z1(_CA) +Z1(_AB)) * h_factor  +Z4(CEN),
+                      (-Z1(__A) +Z2(__B) -Z1(__C) +Z1(_BC) -Z2(_CA) +Z1(_AB)) * h_factor  +Z4(CEN),
+                      (-Z1(__A) -Z1(__B) +Z2(__C) +Z1(_BC) +Z1(_CA) -Z2(_AB)) * h_factor  +Z4(CEN));
+          r_delta   = (+Z2(__A) +Z2(__B) +Z2(__C) +Z2(_BC) +Z2(_CA) +Z2(_AB) -Z12(CEN)) * r_factor;
+
+          if (towers_set) { // see 7 point tower angle calibration (towers & opposites) matrix
+            t_delta.set((+Z0(__A) -Z4(__B) +Z4(__C) +Z0(_BC) -Z4(_CA) +Z4(_AB) +Z0(CEN)) * a_factor,
+                        (+Z4(__A) +Z0(__B) -Z4(__C) +Z4(_BC) +Z0(_CA) -Z4(_AB) +Z0(CEN)) * a_factor,
+                        (-Z4(__A) +Z4(__B) +Z0(__C) -Z4(_BC) +Z4(_CA) +Z0(_AB) +Z0(CEN)) * a_factor);
+          }
+          break;
+      }
+      delta_endstop_adj += e_delta;
+      delta_radius += r_delta;
+      delta_tower_angle_trim += t_delta;
+    }
+    else if (zero_std_dev >= test_precision) {
+      // roll back
+      delta_endstop_adj = e_old;
+      delta_radius = r_old;
+      delta_height = h_old;
+      delta_tower_angle_trim = a_old;
+    }
+
+    if (verbose_level != 0) {                                    // !dry run
+
+      // Normalize angles to least-squares
+      if (_angle_results) {
+        float a_sum = 0.0f;
+        LOOP_XYZ(axis) a_sum += delta_tower_angle_trim[axis];
+        LOOP_XYZ(axis) delta_tower_angle_trim[axis] -= a_sum / 3.0f;
+      }
+
+      // adjust delta_height and endstops by the max amount
+      const float z_temp = _MAX(delta_endstop_adj.a, delta_endstop_adj.b, delta_endstop_adj.c);
+      delta_height -= z_temp;
+      LOOP_XYZ(axis) delta_endstop_adj[axis] -= z_temp;
+    }
+    recalc_delta_settings();
+    NOMORE(zero_std_dev_min, zero_std_dev);
+
+    // print report
+
+    if (verbose_level == 3)
+      print_calibration_results(z_at_pt, _tower_results, _opposite_results);
+
+    if (verbose_level != 0) { // !dry run
+      if ((zero_std_dev >= test_precision && iterations > force_iterations) || zero_std_dev <= calibration_precision) { // end iterations
+        SERIAL_ECHOPGM("Calibration OK");
+        SERIAL_ECHO_SP(32);
+        #if HAS_BED_PROBE
+          if (zero_std_dev >= test_precision && !_1p_calibration && !_0p_calibration)
+            SERIAL_ECHOPGM("rolling back.");
+          else
+        #endif
+          {
+            SERIAL_ECHOPAIR_F("std dev:", zero_std_dev_min, 3);
+          }
+        SERIAL_EOL();
+        char mess[21];
+        strcpy_P(mess, PSTR("Calibration sd:"));
+        if (zero_std_dev_min < 1)
+          sprintf_P(&mess[15], PSTR("0.%03i"), (int)LROUND(zero_std_dev_min * 1000.0f));
+        else
+          sprintf_P(&mess[15], PSTR("%03i.x"), (int)LROUND(zero_std_dev_min));
+        ui.set_status(mess);
+        print_calibration_settings(_endstop_results, _angle_results);
+        SERIAL_ECHOLNPGM("Save with M500 and/or copy to Configuration.h");
+      }
+      else { // !end iterations
+        char mess[15];
+        if (iterations < 31)
+          sprintf_P(mess, PSTR("Iteration : %02i"), (unsigned int)iterations);
+        else
+          strcpy_P(mess, PSTR("No convergence"));
+        SERIAL_ECHO(mess);
+        SERIAL_ECHO_SP(32);
+        SERIAL_ECHOLNPAIR_F("std dev:", zero_std_dev, 3);
+        ui.set_status(mess);
+        if (verbose_level > 1)
+          print_calibration_settings(_endstop_results, _angle_results);
+      }
+    }
+    else { // dry run
+      PGM_P const enddryrun = PSTR("End DRY-RUN");
+      serialprintPGM(enddryrun);
+      SERIAL_ECHO_SP(35);
+      SERIAL_ECHOLNPAIR_F("std dev:", zero_std_dev, 3);
+
+      char mess[21];
+      strcpy_P(mess, enddryrun);
+      strcpy_P(&mess[11], PSTR(" sd:"));
+      if (zero_std_dev < 1)
+        sprintf_P(&mess[15], PSTR("0.%03i"), (int)LROUND(zero_std_dev * 1000.0f));
+      else
+        sprintf_P(&mess[15], PSTR("%03i.x"), (int)LROUND(zero_std_dev));
+      ui.set_status(mess);
+    }
+    ac_home();
+  }
+  while (((zero_std_dev < test_precision && iterations < 31) || iterations <= force_iterations) && zero_std_dev > calibration_precision);
+
+  ac_cleanup(TERN_(HAS_MULTI_HOTEND, old_tool_index));
+}
+
+#endif // DELTA_AUTO_CALIBRATION
diff --git a/Marlin/src/gcode/calibrate/G34.cpp b/Marlin/src/gcode/calibrate/G34.cpp
new file mode 100644
index 0000000..bcca00d
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/G34.cpp
@@ -0,0 +1,157 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if ENABLED(MECHANICAL_GANTRY_CALIBRATION)
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+#include "../../module/stepper.h"
+#include "../../module/endstops.h"
+
+#if HAS_LEVELING
+  #include "../../feature/bedlevel/bedlevel.h"
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../../core/debug_out.h"
+
+void GcodeSuite::G34() {
+
+  // Home before the alignment procedure
+  if (!all_axes_trusted()) home_all_axes();
+
+  TERN_(HAS_LEVELING, TEMPORARY_BED_LEVELING_STATE(false));
+
+  SET_SOFT_ENDSTOP_LOOSE(true);
+  TemporaryGlobalEndstopsState unlock_z(false);
+
+  #ifdef GANTRY_CALIBRATION_COMMANDS_PRE
+    gcode.process_subcommands_now_P(PSTR(GANTRY_CALIBRATION_COMMANDS_PRE));
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Sub Commands Processed");
+  #endif
+
+  #ifdef GANTRY_CALIBRATION_SAFE_POSITION
+    // Move XY to safe position
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Parking XY");
+    const xy_pos_t safe_pos = GANTRY_CALIBRATION_SAFE_POSITION;
+    do_blocking_move_to(safe_pos, MMM_TO_MMS(GANTRY_CALIBRATION_XY_PARK_FEEDRATE));
+  #endif
+
+  const float move_distance = parser.intval('Z', GANTRY_CALIBRATION_EXTRA_HEIGHT),
+              zbase = ENABLED(GANTRY_CALIBRATION_TO_MIN) ? Z_MIN_POS : Z_MAX_POS,
+              zpounce = zbase - move_distance, zgrind = zbase + move_distance;
+
+  // Move Z to pounce position
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Setting Z Pounce");
+  do_blocking_move_to_z(zpounce, homing_feedrate(Z_AXIS));
+
+  // Store current motor settings, then apply reduced value
+
+  #define _REDUCE_CURRENT ANY(HAS_MOTOR_CURRENT_SPI, HAS_MOTOR_CURRENT_PWM, HAS_MOTOR_CURRENT_DAC, HAS_MOTOR_CURRENT_I2C, HAS_TRINAMIC_CONFIG)
+  #if _REDUCE_CURRENT
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Reducing Current");
+  #endif
+
+  #if HAS_MOTOR_CURRENT_SPI
+    const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT);
+    const uint32_t previous_current = stepper.motor_current_setting[Z_AXIS];
+    stepper.set_digipot_current(Z_AXIS, target_current);
+  #elif HAS_MOTOR_CURRENT_PWM
+    const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT);
+    const uint32_t previous_current = stepper.motor_current_setting[Z_AXIS];
+    stepper.set_digipot_current(1, target_current);
+  #elif ENABLED(HAS_MOTOR_CURRENT_DAC)
+    const float target_current = parser.floatval('S', GANTRY_CALIBRATION_CURRENT);
+    const float previous_current = dac_amps(Z_AXIS, target_current);
+    stepper_dac.set_current_value(Z_AXIS, target_current);
+  #elif ENABLED(HAS_MOTOR_CURRENT_I2C)
+    const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT);
+    previous_current = dac_amps(Z_AXIS);
+    digipot_i2c.set_current(Z_AXIS, target_current)
+  #elif HAS_TRINAMIC_CONFIG
+    const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT);
+    static uint16_t previous_current_arr[NUM_Z_STEPPER_DRIVERS];
+    #if AXIS_IS_TMC(Z)
+      previous_current_arr[0] = stepperZ.getMilliamps();
+      stepperZ.rms_current(target_current);
+    #endif
+    #if AXIS_IS_TMC(Z2)
+      previous_current_arr[1] = stepperZ2.getMilliamps();
+      stepperZ2.rms_current(target_current);
+    #endif
+    #if AXIS_IS_TMC(Z3)
+      previous_current_arr[2] = stepperZ3.getMilliamps();
+      stepperZ3.rms_current(target_current);
+    #endif
+    #if AXIS_IS_TMC(Z4)
+      previous_current_arr[3] = stepperZ4.getMilliamps();
+      stepperZ4.rms_current(target_current);
+    #endif
+  #endif
+
+  // Do Final Z move to adjust
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Final Z Move");
+  do_blocking_move_to_z(zgrind, MMM_TO_MMS(GANTRY_CALIBRATION_FEEDRATE));
+
+  // Back off end plate, back to normal motion range
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Z Backoff");
+  do_blocking_move_to_z(zpounce, MMM_TO_MMS(GANTRY_CALIBRATION_FEEDRATE));
+
+  #if _REDUCE_CURRENT
+    // Reset current to original values
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Restore Current");
+  #endif
+
+  #if HAS_MOTOR_CURRENT_SPI
+    stepper.set_digipot_current(Z_AXIS, previous_current);
+  #elif HAS_MOTOR_CURRENT_PWM
+    stepper.set_digipot_current(1, previous_current);
+  #elif ENABLED(HAS_MOTOR_CURRENT_DAC)
+    stepper_dac.set_current_value(Z_AXIS, previous_current);
+  #elif ENABLED(HAS_MOTOR_CURRENT_I2C)
+    digipot_i2c.set_current(Z_AXIS, previous_current)
+  #elif HAS_TRINAMIC_CONFIG
+    #if AXIS_IS_TMC(Z)
+      stepperZ.rms_current(previous_current_arr[0]);
+    #endif
+    #if AXIS_IS_TMC(Z2)
+      stepperZ2.rms_current(previous_current_arr[1]);
+    #endif
+    #if AXIS_IS_TMC(Z3)
+      stepperZ3.rms_current(previous_current_arr[2]);
+    #endif
+    #if AXIS_IS_TMC(Z4)
+      stepperZ4.rms_current(previous_current_arr[3]);
+    #endif
+  #endif
+
+  #ifdef GANTRY_CALIBRATION_COMMANDS_POST
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Running Post Commands");
+    gcode.process_subcommands_now_P(PSTR(GANTRY_CALIBRATION_COMMANDS_POST));
+  #endif
+
+  SET_SOFT_ENDSTOP_LOOSE(false);
+}
+
+#endif // MECHANICAL_GANTRY_CALIBRATION
diff --git a/Marlin/src/gcode/calibrate/G34_M422.cpp b/Marlin/src/gcode/calibrate/G34_M422.cpp
new file mode 100644
index 0000000..0bcf954
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/G34_M422.cpp
@@ -0,0 +1,533 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if EITHER(Z_MULTI_ENDSTOPS, Z_STEPPER_AUTO_ALIGN)
+
+#include "../../feature/z_stepper_align.h"
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+#include "../../module/stepper.h"
+#include "../../module/planner.h"
+#include "../../module/probe.h"
+#include "../../lcd/marlinui.h" // for LCD_MESSAGEPGM
+
+#if HAS_LEVELING
+  #include "../../feature/bedlevel/bedlevel.h"
+#endif
+
+#if HAS_MULTI_HOTEND
+  #include "../../module/tool_change.h"
+#endif
+
+#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+  #include "../../libs/least_squares_fit.h"
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../../core/debug_out.h"
+
+/**
+ * G34: Z-Stepper automatic alignment
+ *
+ * Manual stepper lock controls (reset by G28):
+ *   L                 Unlock all steppers
+ *   Z<1-4>            Z stepper to lock / unlock
+ *   S<state>          0=UNLOCKED 1=LOCKED. If omitted, assume LOCKED.
+ *
+ *   Examples:
+ *     G34 Z1     ; Lock Z1
+ *     G34 L Z2   ; Unlock all, then lock Z2
+ *     G34 Z2 S0  ; Unlock Z2
+ *
+ * With Z_STEPPER_AUTO_ALIGN:
+ *   I<iterations>     Number of tests. If omitted, Z_STEPPER_ALIGN_ITERATIONS.
+ *   T<accuracy>       Target Accuracy factor. If omitted, Z_STEPPER_ALIGN_ACC.
+ *   A<amplification>  Provide an Amplification value. If omitted, Z_STEPPER_ALIGN_AMP.
+ *   R                 Flag to recalculate points based on current probe offsets
+ */
+void GcodeSuite::G34() {
+  DEBUG_SECTION(log_G34, "G34", DEBUGGING(LEVELING));
+  if (DEBUGGING(LEVELING)) log_machine_info();
+
+  planner.synchronize();  // Prevent damage
+
+  const bool seenL = parser.seen('L');
+  if (seenL) stepper.set_all_z_lock(false);
+
+  const bool seenZ = parser.seenval('Z');
+  if (seenZ) {
+    const bool state = parser.boolval('S', true);
+    switch (parser.intval('Z')) {
+      case 1: stepper.set_z1_lock(state); break;
+      case 2: stepper.set_z2_lock(state); break;
+      #if NUM_Z_STEPPER_DRIVERS >= 3
+        case 3: stepper.set_z3_lock(state); break;
+        #if NUM_Z_STEPPER_DRIVERS >= 4
+          case 4: stepper.set_z4_lock(state); break;
+        #endif
+      #endif
+    }
+  }
+
+  if (seenL || seenZ) {
+    stepper.set_separate_multi_axis(seenZ);
+    return;
+  }
+
+  #if ENABLED(Z_STEPPER_AUTO_ALIGN)
+    do { // break out on error
+
+      #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
+
+      const int8_t z_auto_align_iterations = parser.intval('I', Z_STEPPER_ALIGN_ITERATIONS);
+      if (!WITHIN(z_auto_align_iterations, 1, 30)) {
+        SERIAL_ECHOLNPGM("?(I)teration out of bounds (1-30).");
+        break;
+      }
+
+      const float z_auto_align_accuracy = parser.floatval('T', Z_STEPPER_ALIGN_ACC);
+      if (!WITHIN(z_auto_align_accuracy, 0.01f, 1.0f)) {
+        SERIAL_ECHOLNPGM("?(T)arget accuracy out of bounds (0.01-1.0).");
+        break;
+      }
+
+      const float z_auto_align_amplification = TERN(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS, Z_STEPPER_ALIGN_AMP, parser.floatval('A', Z_STEPPER_ALIGN_AMP));
+      if (!WITHIN(ABS(z_auto_align_amplification), 0.5f, 2.0f)) {
+        SERIAL_ECHOLNPGM("?(A)mplification out of bounds (0.5-2.0).");
+        break;
+      }
+
+      if (parser.seen('R')) z_stepper_align.reset_to_default();
+
+      const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE;
+
+      // Disable the leveling matrix before auto-aligning
+      #if HAS_LEVELING
+        TERN_(RESTORE_LEVELING_AFTER_G34, const bool leveling_was_active = planner.leveling_active);
+        set_bed_leveling_enabled(false);
+      #endif
+
+      TERN_(CNC_WORKSPACE_PLANES, workspace_plane = PLANE_XY);
+
+      // Always home with tool 0 active
+      #if HAS_MULTI_HOTEND
+        const uint8_t old_tool_index = active_extruder;
+        tool_change(0, true);
+      #endif
+
+      TERN_(HAS_DUPLICATION_MODE, set_duplication_enabled(false));
+
+      // In BLTOUCH HS mode, the probe travels in a deployed state.
+      // Users of G34 might have a badly misaligned bed, so raise Z by the
+      // length of the deployed pin (BLTOUCH stroke < 7mm)
+      #define Z_BASIC_CLEARANCE (Z_CLEARANCE_BETWEEN_PROBES + 7.0f * BOTH(BLTOUCH, BLTOUCH_HS_MODE))
+
+      // Compute a worst-case clearance height to probe from. After the first
+      // iteration this will be re-calculated based on the actual bed position
+      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
+          _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
+          magnitude2(0, 1)
+        #endif
+      );
+
+      // Home before the alignment procedure
+      if (!all_axes_trusted()) home_all_axes();
+
+      // Move the Z coordinate realm towards the positive - dirty trick
+      current_position.z += z_probe * 0.5f;
+      sync_plan_position();
+      // Now, the Z origin lies below the build plate. That allows to probe deeper, before run_z_probe throws an error.
+      // 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, 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;
+
+      #if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+        bool adjustment_reverse = false;
+      #endif
+
+      #if HAS_DISPLAY
+        PGM_P const msg_iteration = GET_TEXT(MSG_ITERATION);
+        const uint8_t iter_str_len = strlen_P(msg_iteration);
+      #endif
+
+      // Final z and iteration values will be used after breaking the loop
+      float z_measured_min;
+      uint8_t iteration = 0;
+      bool err_break = false; // To break out of nested loops
+      while (iteration < z_auto_align_iterations) {
+        if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> probing all positions.");
+
+        const int iter = iteration + 1;
+        SERIAL_ECHOLNPAIR("\nG34 Iteration: ", iter);
+        #if HAS_DISPLAY
+          char str[iter_str_len + 2 + 1];
+          sprintf_P(str, msg_iteration, iter);
+          ui.set_status(str);
+        #endif
+
+        // Initialize minimum value
+        z_measured_min =  100000.0f;
+        float z_measured_max = -100000.0f;
+
+        // Probe all positions (one per Z-Stepper)
+        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;
+
+          // Safe clearance even on an incline
+          if ((iteration == 0 || i > 0) && z_probe > current_position.z) do_blocking_move_to_z(z_probe);
+
+          if (DEBUGGING(LEVELING))
+            DEBUG_ECHOLNPAIR_P(PSTR("Probing X"), z_stepper_align.xy[iprobe].x, SP_Y_STR, z_stepper_align.xy[iprobe].y);
+
+          // Probe a Z height for each stepper.
+          // Probing sanity check is disabled, as it would trigger even in normal cases because
+          // current_position.z has been manually altered in the "dirty trick" above.
+          const float z_probed_height = probe.probe_at_point(z_stepper_align.xy[iprobe], raise_after, 0, true, false);
+          if (isnan(z_probed_height)) {
+            SERIAL_ECHOLNPGM("Probing failed");
+            LCD_MESSAGEPGM(MSG_LCD_PROBING_FAILED);
+            err_break = true;
+            break;
+          }
+
+          // Add height to each value, to provide a more useful target height for
+          // the next iteration of probing. This allows adjustments to be made away from the bed.
+          z_measured[iprobe] = z_probed_height + Z_CLEARANCE_BETWEEN_PROBES;
+
+          if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", int(iprobe + 1), " measured position is ", z_measured[iprobe]);
+
+          // Remember the minimum measurement to calculate the correction later on
+          z_measured_min = _MIN(z_measured_min, z_measured[iprobe]);
+          z_measured_max = _MAX(z_measured_max, z_measured[iprobe]);
+        } // for (i)
+
+        if (err_break) break;
+
+        // Adapt the next probe clearance height based on the new measurements.
+        // Safe_height = lowest distance to bed (= highest measurement) plus highest measured misalignment.
+        z_maxdiff = z_measured_max - z_measured_min;
+        z_probe = Z_BASIC_CLEARANCE + z_measured_max + z_maxdiff;
+
+        #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+          // Replace the initial values in z_measured with calculated heights at
+          // each stepper position. This allows the adjustment algorithm to be
+          // shared between both possible probing mechanisms.
+
+          // This must be done after the next z_probe height is calculated, so that
+          // the height is calculated from actual print area positions, and not
+          // extrapolated motor movements.
+
+          // Compute the least-squares fit for all probed points.
+          // Calculate the Z position of each stepper and store it in z_measured.
+          // This allows the actual adjustment logic to be shared by both algorithms.
+          linear_fit_data lfd;
+          incremental_LSF_reset(&lfd);
+          LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) {
+            SERIAL_ECHOLNPAIR("PROBEPT_", int(i), ": ", z_measured[i]);
+            incremental_LSF(&lfd, z_stepper_align.xy[i], z_measured[i]);
+          }
+          finish_incremental_LSF(&lfd);
+
+          z_measured_min = 100000.0f;
+          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 + lfd.D);
+            z_measured_min = _MIN(z_measured_min, z_measured[i]);
+          }
+
+          SERIAL_ECHOLNPAIR("CALCULATED STEPPER POSITIONS: Z1=", z_measured[0], " Z2=", z_measured[1], " Z3=", z_measured[2]);
+        #endif
+
+        SERIAL_ECHOLNPAIR("\n"
+          "DIFFERENCE Z1-Z2=", ABS(z_measured[0] - z_measured[1])
+          #if NUM_Z_STEPPER_DRIVERS == 3
+            , " Z2-Z3=", ABS(z_measured[1] - z_measured[2])
+            , " Z3-Z1=", ABS(z_measured[2] - z_measured[0])
+          #endif
+        );
+        #if HAS_DISPLAY
+          char fstr1[10];
+          #if NUM_Z_STEPPER_DRIVERS == 2
+            char msg[6 + (6 + 5) * 1 + 1];
+          #else
+            char msg[6 + (6 + 5) * 3 + 1], fstr2[10], fstr3[10];
+          #endif
+          sprintf_P(msg,
+            PSTR("Diffs Z1-Z2=%s"
+              #if NUM_Z_STEPPER_DRIVERS == 3
+                " Z2-Z3=%s"
+                " Z3-Z1=%s"
+              #endif
+            ), dtostrf(ABS(z_measured[0] - z_measured[1]), 1, 3, fstr1)
+            #if NUM_Z_STEPPER_DRIVERS == 3
+              , dtostrf(ABS(z_measured[1] - z_measured[2]), 1, 3, fstr2)
+              , dtostrf(ABS(z_measured[2] - z_measured[0]), 1, 3, fstr3)
+            #endif
+          );
+          ui.set_status(msg);
+        #endif
+
+        auto decreasing_accuracy = [](const float &v1, const float &v2){
+          if (v1 < v2 * 0.7f) {
+            SERIAL_ECHOLNPGM("Decreasing Accuracy Detected.");
+            LCD_MESSAGEPGM(MSG_DECREASING_ACCURACY);
+            return true;
+          }
+          return false;
+        };
+
+        #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
+          err_break = decreasing_accuracy(last_z_align_level_indicator, z_align_level_indicator);
+          if (err_break) 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
+        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;
+
+            // Check for less accuracy compared to last move
+            if (decreasing_accuracy(last_z_align_move[zstepper], z_align_abs)) {
+              if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", int(zstepper + 1), " last_z_align_move = ", last_z_align_move[zstepper]);
+              if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", int(zstepper + 1), " z_align_abs = ", z_align_abs);
+              adjustment_reverse = !adjustment_reverse;
+            }
+
+            // Remember the alignment for the next iteration, but only if steppers move,
+            // otherwise it would be just zero (in case this stepper was at z_measured_min already)
+            if (z_align_abs > 0) 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;
+
+          if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", int(zstepper + 1), " corrected by ", z_align_move);
+
+          // Lock all steppers except one
+          stepper.set_all_z_lock(true, zstepper);
+
+          #if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+            // Decreasing accuracy was detected so move was inverted.
+            // Will match reversed Z steppers on dual steppers. Triple will need more work to map.
+            if (adjustment_reverse) {
+              z_align_move = -z_align_move;
+              if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", int(zstepper + 1), " correction reversed to ", z_align_move);
+            }
+          #endif
+
+          // Do a move to correct part of the misalignment for the current stepper
+          do_blocking_move_to_z(amplification * z_align_move + current_position.z);
+        } // for (zstepper)
+
+        // Back to normal stepper operations
+        stepper.set_all_z_lock(false);
+        stepper.set_separate_multi_axis(false);
+
+        if (err_break) break;
+
+        if (success_break) {
+          SERIAL_ECHOLNPGM("Target accuracy achieved.");
+          LCD_MESSAGEPGM(MSG_ACCURACY_ACHIEVED);
+          break;
+        }
+
+        iteration++;
+      } // while (iteration < z_auto_align_iterations)
+
+      if (err_break)
+        SERIAL_ECHOLNPGM("G34 aborted.");
+      else {
+        SERIAL_ECHOLNPAIR("Did ", int(iteration + (iteration != z_auto_align_iterations)), " of ", int(z_auto_align_iterations));
+        SERIAL_ECHOLNPAIR_F("Accuracy: ", z_maxdiff);
+      }
+
+      // 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_never_homed(Z_AXIS);
+        // Home Z after the alignment procedure
+        process_subcommands_now_P(PSTR("G28Z"));
+      #else
+        // Use the probed height from the last iteration to determine the Z height.
+        // z_measured_min is used, because all steppers are aligned to z_measured_min.
+        // Ideally, this would be equal to the 'z_probe * 0.5f' which was added earlier.
+        current_position.z -= z_measured_min - (float)Z_CLEARANCE_BETWEEN_PROBES;
+        sync_plan_position();
+      #endif
+
+      // Restore the active tool after homing
+      TERN_(HAS_MULTI_HOTEND, tool_change(old_tool_index, DISABLED(PARKING_EXTRUDER))); // Fetch previous tool for parking extruder
+
+      #if BOTH(HAS_LEVELING, RESTORE_LEVELING_AFTER_G34)
+        set_bed_leveling_enabled(leveling_was_active);
+      #endif
+
+    }while(0);
+  #endif
+}
+
+#endif // Z_MULTI_ENDSTOPS || Z_STEPPER_AUTO_ALIGN
+
+#if ENABLED(Z_STEPPER_AUTO_ALIGN)
+
+/**
+ * M422: Set a Z-Stepper automatic alignment XY point.
+ *       Use repeatedly to set multiple points.
+ *
+ *   S<index> : Index of the probe point to set
+ *
+ * With Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS:
+ *   W<index> : Index of the Z stepper position to set
+ *              The W and S parameters may not be combined.
+ *
+ * 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);
+    #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+      LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS)
+        SERIAL_ECHOLNPAIR_P(PSTR("M422 W"), int(i + 1), SP_X_STR, z_stepper_align.stepper_xy[i].x, SP_Y_STR, z_stepper_align.stepper_xy[i].y);
+    #endif
+    return;
+  }
+
+  const bool is_probe_point = parser.seen('S');
+
+  if (TERN0(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS, is_probe_point && parser.seen('W'))) {
+    SERIAL_ECHOLNPGM("?(S) and (W) may not be combined.");
+    return;
+  }
+
+  xy_pos_t *pos_dest = (
+    TERN_(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS, !is_probe_point ? z_stepper_align.stepper_xy :)
+    z_stepper_align.xy
+  );
+
+  if (!is_probe_point && TERN1(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS, !parser.seen('W'))) {
+    SERIAL_ECHOLNPGM("?(S)" TERN_(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS, " or (W)") " is required.");
+    return;
+  }
+
+  // Get the Probe Position Index or Z Stepper Index
+  int8_t position_index;
+  if (is_probe_point) {
+    position_index = parser.intval('S') - 1;
+    if (!WITHIN(position_index, 0, int8_t(NUM_Z_STEPPER_DRIVERS) - 1)) {
+      SERIAL_ECHOLNPGM("?(S) Probe-position index invalid.");
+      return;
+    }
+  }
+  else {
+    #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+      position_index = parser.intval('W') - 1;
+      if (!WITHIN(position_index, 0, NUM_Z_STEPPER_DRIVERS - 1)) {
+        SERIAL_ECHOLNPGM("?(W) Z-stepper index invalid.");
+        return;
+      }
+    #endif
+  }
+
+  const xy_pos_t pos = {
+    parser.floatval('X', pos_dest[position_index].x),
+    parser.floatval('Y', pos_dest[position_index].y)
+  };
+
+  if (is_probe_point) {
+    if (!probe.can_reach(pos.x, Y_CENTER)) {
+      SERIAL_ECHOLNPGM("?(X) out of bounds.");
+      return;
+    }
+    if (!probe.can_reach(pos)) {
+      SERIAL_ECHOLNPGM("?(Y) out of bounds.");
+      return;
+    }
+  }
+
+  pos_dest[position_index] = pos;
+}
+
+#endif // Z_STEPPER_AUTO_ALIGN
diff --git a/Marlin/src/gcode/calibrate/G425.cpp b/Marlin/src/gcode/calibrate/G425.cpp
new file mode 100644
index 0000000..9510da7
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/G425.cpp
@@ -0,0 +1,623 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../MarlinCore.h"
+
+#if ENABLED(CALIBRATION_GCODE)
+
+#include "../gcode.h"
+
+#if ENABLED(BACKLASH_GCODE)
+  #include "../../feature/backlash.h"
+#endif
+
+#include "../../lcd/marlinui.h"
+#include "../../module/motion.h"
+#include "../../module/planner.h"
+#include "../../module/tool_change.h"
+#include "../../module/endstops.h"
+#include "../../feature/bedlevel/bedlevel.h"
+
+#if !AXIS_CAN_CALIBRATE(X)
+  #undef CALIBRATION_MEASURE_LEFT
+  #undef CALIBRATION_MEASURE_RIGHT
+#endif
+
+#if !AXIS_CAN_CALIBRATE(Y)
+  #undef CALIBRATION_MEASURE_FRONT
+  #undef CALIBRATION_MEASURE_BACK
+#endif
+
+#if !AXIS_CAN_CALIBRATE(Z)
+  #undef CALIBRATION_MEASURE_AT_TOP_EDGES
+#endif
+
+/**
+ * G425 backs away from the calibration object by various distances
+ * depending on the confidence level:
+ *
+ *   UNKNOWN   - No real notion on where the calibration object is on the bed
+ *   UNCERTAIN - Measurement may be uncertain due to backlash
+ *   CERTAIN   - Measurement obtained with backlash compensation
+ */
+
+#ifndef CALIBRATION_MEASUREMENT_UNKNOWN
+  #define CALIBRATION_MEASUREMENT_UNKNOWN   5.0 // mm
+#endif
+#ifndef CALIBRATION_MEASUREMENT_UNCERTAIN
+  #define CALIBRATION_MEASUREMENT_UNCERTAIN 1.0 // mm
+#endif
+#ifndef CALIBRATION_MEASUREMENT_CERTAIN
+  #define CALIBRATION_MEASUREMENT_CERTAIN   0.5 // mm
+#endif
+
+#if BOTH(CALIBRATION_MEASURE_LEFT, CALIBRATION_MEASURE_RIGHT)
+  #define HAS_X_CENTER 1
+#endif
+#if BOTH(CALIBRATION_MEASURE_FRONT, CALIBRATION_MEASURE_BACK)
+  #define HAS_Y_CENTER 1
+#endif
+
+enum side_t : uint8_t { TOP, RIGHT, FRONT, LEFT, BACK, NUM_SIDES };
+
+static constexpr xyz_pos_t true_center CALIBRATION_OBJECT_CENTER;
+static constexpr xyz_float_t dimensions CALIBRATION_OBJECT_DIMENSIONS;
+static constexpr xy_float_t nod = { CALIBRATION_NOZZLE_OUTER_DIAMETER, CALIBRATION_NOZZLE_OUTER_DIAMETER };
+
+struct measurements_t {
+  xyz_pos_t obj_center = true_center; // Non-static must be assigned from xyz_pos_t
+
+  float obj_side[NUM_SIDES], backlash[NUM_SIDES];
+  xyz_float_t pos_error;
+
+  xy_float_t nozzle_outer_dimension = nod;
+};
+
+#if ENABLED(BACKLASH_GCODE)
+  #define TEMPORARY_BACKLASH_CORRECTION(value) REMEMBER(tbst, backlash.correction, value)
+#else
+  #define TEMPORARY_BACKLASH_CORRECTION(value)
+#endif
+
+#if ENABLED(BACKLASH_GCODE) && defined(BACKLASH_SMOOTHING_MM)
+  #define TEMPORARY_BACKLASH_SMOOTHING(value) REMEMBER(tbsm, backlash.smoothing_mm, value)
+#else
+  #define TEMPORARY_BACKLASH_SMOOTHING(value)
+#endif
+
+inline void calibration_move() {
+  do_blocking_move_to(current_position, MMM_TO_MMS(CALIBRATION_FEEDRATE_TRAVEL));
+}
+
+/**
+ * Move to the exact center above the calibration object
+ *
+ *   m                  in     - Measurement record
+ *   uncertainty        in     - How far away from the object top to park
+ */
+inline void park_above_object(measurements_t &m, const float uncertainty) {
+  // Move to safe distance above calibration object
+  current_position.z = m.obj_center.z + dimensions.z / 2 + uncertainty;
+  calibration_move();
+
+  // Move to center of calibration object in XY
+  current_position = xy_pos_t(m.obj_center);
+  calibration_move();
+}
+
+#if HAS_MULTI_HOTEND
+  inline void set_nozzle(measurements_t &m, const uint8_t extruder) {
+    if (extruder != active_extruder) {
+      park_above_object(m, CALIBRATION_MEASUREMENT_UNKNOWN);
+      tool_change(extruder);
+    }
+  }
+#endif
+
+#if HAS_HOTEND_OFFSET
+
+  inline void normalize_hotend_offsets() {
+    LOOP_S_L_N(e, 1, HOTENDS)
+      hotend_offset[e] -= hotend_offset[0];
+    hotend_offset[0].reset();
+  }
+
+#endif
+
+#if !PIN_EXISTS(CALIBRATION)
+  #include "../../module/probe.h"
+#endif
+
+inline bool read_calibration_pin() {
+  return (
+    #if PIN_EXISTS(CALIBRATION)
+      READ(CALIBRATION_PIN) != CALIBRATION_PIN_INVERTING
+    #else
+      PROBE_TRIGGERED()
+    #endif
+  );
+}
+
+/**
+ * Move along axis in the specified dir until the probe value becomes stop_state,
+ * then return the axis value.
+ *
+ *   axis         in - Axis along which the measurement will take place
+ *   dir          in - Direction along that axis (-1 or 1)
+ *   stop_state   in - Move until probe pin becomes this value
+ *   fast         in - Fast vs. precise measurement
+ */
+float measuring_movement(const AxisEnum axis, const int dir, const bool stop_state, const bool fast) {
+  const float step     = fast ? 0.25 : CALIBRATION_MEASUREMENT_RESOLUTION;
+  const feedRate_t mms = fast ? MMM_TO_MMS(CALIBRATION_FEEDRATE_FAST) : MMM_TO_MMS(CALIBRATION_FEEDRATE_SLOW);
+  const float limit    = fast ? 50 : 5;
+
+  destination = current_position;
+  for (float travel = 0; travel < limit; travel += step) {
+    destination[axis] += dir * step;
+    do_blocking_move_to(destination, mms);
+    planner.synchronize();
+    if (read_calibration_pin() == stop_state) break;
+  }
+  return destination[axis];
+}
+
+/**
+ * Move along axis until the probe is triggered. Move toolhead to its starting
+ * point and return the measured value.
+ *
+ *   axis               in     - Axis along which the measurement will take place
+ *   dir                in     - Direction along that axis (-1 or 1)
+ *   stop_state         in     - Move until probe pin becomes this value
+ *   backlash_ptr       in/out - When not nullptr, measure and record axis backlash
+ *   uncertainty        in     - If uncertainty is CALIBRATION_MEASUREMENT_UNKNOWN, do a fast probe.
+ */
+inline float measure(const AxisEnum axis, const int dir, const bool stop_state, float * const backlash_ptr, const float uncertainty) {
+  const bool fast = uncertainty == CALIBRATION_MEASUREMENT_UNKNOWN;
+
+  // Save position
+  destination = current_position;
+  const float start_pos = destination[axis];
+  const float measured_pos = measuring_movement(axis, dir, stop_state, fast);
+  // Measure backlash
+  if (backlash_ptr && !fast) {
+    const float release_pos = measuring_movement(axis, -dir, !stop_state, fast);
+    *backlash_ptr = ABS(release_pos - measured_pos);
+  }
+  // Return to starting position
+  destination[axis] = start_pos;
+  do_blocking_move_to(destination, MMM_TO_MMS(CALIBRATION_FEEDRATE_TRAVEL));
+  return measured_pos;
+}
+
+/**
+ * Probe one side of the calibration object
+ *
+ *   m                  in/out - Measurement record, m.obj_center and m.obj_side will be updated.
+ *   uncertainty        in     - How far away from the calibration object to begin probing
+ *   side               in     - Side of probe where probe will occur
+ *   probe_top_at_edge  in     - When probing sides, probe top of calibration object nearest edge
+ *                               to find out height of edge
+ */
+inline void probe_side(measurements_t &m, const float uncertainty, const side_t side, const bool probe_top_at_edge=false) {
+  const xyz_float_t dimensions = CALIBRATION_OBJECT_DIMENSIONS;
+  AxisEnum axis;
+  float dir = 1;
+
+  park_above_object(m, uncertainty);
+
+  switch (side) {
+    #if AXIS_CAN_CALIBRATE(Z)
+      case TOP: {
+        const float measurement = measure(Z_AXIS, -1, true, &m.backlash[TOP], uncertainty);
+        m.obj_center.z = measurement - dimensions.z / 2;
+        m.obj_side[TOP] = measurement;
+        return;
+      }
+    #endif
+    #if AXIS_CAN_CALIBRATE(X)
+      case LEFT:  axis = X_AXIS; break;
+      case RIGHT: axis = X_AXIS; dir = -1; break;
+    #endif
+    #if AXIS_CAN_CALIBRATE(Y)
+      case FRONT: axis = Y_AXIS; break;
+      case BACK:  axis = Y_AXIS; dir = -1; break;
+    #endif
+    default: return;
+  }
+
+  if (probe_top_at_edge) {
+    #if AXIS_CAN_CALIBRATE(Z)
+      // Probe top nearest the side we are probing
+      current_position[axis] = m.obj_center[axis] + (-dir) * (dimensions[axis] / 2 - m.nozzle_outer_dimension[axis]);
+      calibration_move();
+      m.obj_side[TOP] = measure(Z_AXIS, -1, true, &m.backlash[TOP], uncertainty);
+      m.obj_center.z = m.obj_side[TOP] - dimensions.z / 2;
+    #endif
+  }
+
+  if ((AXIS_CAN_CALIBRATE(X) && axis == X_AXIS) || (AXIS_CAN_CALIBRATE(Y) && axis == Y_AXIS)) {
+    // Move to safe distance to the side of the calibration object
+    current_position[axis] = m.obj_center[axis] + (-dir) * (dimensions[axis] / 2 + m.nozzle_outer_dimension[axis] / 2 + uncertainty);
+    calibration_move();
+
+    // Plunge below the side of the calibration object and measure
+    current_position.z = m.obj_side[TOP] - (CALIBRATION_NOZZLE_TIP_HEIGHT) * 0.7f;
+    calibration_move();
+    const float measurement = measure(axis, dir, true, &m.backlash[side], uncertainty);
+    m.obj_center[axis] = measurement + dir * (dimensions[axis] / 2 + m.nozzle_outer_dimension[axis] / 2);
+    m.obj_side[side] = measurement;
+  }
+}
+
+/**
+ * Probe all sides of the calibration calibration object
+ *
+ *   m                  in/out - Measurement record: center, backlash and error values be updated.
+ *   uncertainty        in     - How far away from the calibration object to begin probing
+ */
+inline void probe_sides(measurements_t &m, const float uncertainty) {
+  #if ENABLED(CALIBRATION_MEASURE_AT_TOP_EDGES)
+    constexpr bool probe_top_at_edge = true;
+  #else
+    // Probing at the exact center only works if the center is flat. Probing on a washer
+    // or bolt will require probing the top near the side edges, away from the center.
+    constexpr bool probe_top_at_edge = false;
+    probe_side(m, uncertainty, TOP);
+  #endif
+
+  TERN_(CALIBRATION_MEASURE_RIGHT, probe_side(m, uncertainty, RIGHT, probe_top_at_edge));
+  TERN_(CALIBRATION_MEASURE_FRONT, probe_side(m, uncertainty, FRONT, probe_top_at_edge));
+  TERN_(CALIBRATION_MEASURE_LEFT,  probe_side(m, uncertainty, LEFT,  probe_top_at_edge));
+  TERN_(CALIBRATION_MEASURE_BACK,  probe_side(m, uncertainty, BACK,  probe_top_at_edge));
+
+  // Compute the measured center of the calibration object.
+  TERN_(HAS_X_CENTER, m.obj_center.x = (m.obj_side[LEFT] + m.obj_side[RIGHT]) / 2);
+  TERN_(HAS_Y_CENTER, m.obj_center.y = (m.obj_side[FRONT] + m.obj_side[BACK]) / 2);
+
+  // Compute the outside diameter of the nozzle at the height
+  // at which it makes contact with the calibration object
+  TERN_(HAS_X_CENTER, m.nozzle_outer_dimension.x = m.obj_side[RIGHT] - m.obj_side[LEFT] - dimensions.x);
+  TERN_(HAS_Y_CENTER, m.nozzle_outer_dimension.y = m.obj_side[BACK]  - m.obj_side[FRONT] - dimensions.y);
+
+  park_above_object(m, uncertainty);
+
+  // The difference between the known and the measured location
+  // of the calibration object is the positional error
+  m.pos_error.x = (0
+    #if HAS_X_CENTER
+      + true_center.x - m.obj_center.x
+    #endif
+  );
+  m.pos_error.y = (0
+    #if HAS_Y_CENTER
+      + true_center.y - m.obj_center.y
+    #endif
+  );
+  m.pos_error.z = true_center.z - m.obj_center.z;
+}
+
+#if ENABLED(CALIBRATION_REPORTING)
+  inline void report_measured_faces(const measurements_t &m) {
+    SERIAL_ECHOLNPGM("Sides:");
+    #if AXIS_CAN_CALIBRATE(Z)
+      SERIAL_ECHOLNPAIR("  Top: ", m.obj_side[TOP]);
+    #endif
+    #if ENABLED(CALIBRATION_MEASURE_LEFT)
+      SERIAL_ECHOLNPAIR("  Left: ", m.obj_side[LEFT]);
+    #endif
+    #if ENABLED(CALIBRATION_MEASURE_RIGHT)
+      SERIAL_ECHOLNPAIR("  Right: ", m.obj_side[RIGHT]);
+    #endif
+    #if ENABLED(CALIBRATION_MEASURE_FRONT)
+      SERIAL_ECHOLNPAIR("  Front: ", m.obj_side[FRONT]);
+    #endif
+    #if ENABLED(CALIBRATION_MEASURE_BACK)
+      SERIAL_ECHOLNPAIR("  Back: ", m.obj_side[BACK]);
+    #endif
+    SERIAL_EOL();
+  }
+
+  inline void report_measured_center(const measurements_t &m) {
+    SERIAL_ECHOLNPGM("Center:");
+    #if HAS_X_CENTER
+      SERIAL_ECHOLNPAIR_P(SP_X_STR, m.obj_center.x);
+    #endif
+    #if HAS_Y_CENTER
+      SERIAL_ECHOLNPAIR_P(SP_Y_STR, m.obj_center.y);
+    #endif
+    SERIAL_ECHOLNPAIR_P(SP_Z_STR, m.obj_center.z);
+    SERIAL_EOL();
+  }
+
+  inline void report_measured_backlash(const measurements_t &m) {
+    SERIAL_ECHOLNPGM("Backlash:");
+    #if AXIS_CAN_CALIBRATE(X)
+      #if ENABLED(CALIBRATION_MEASURE_LEFT)
+        SERIAL_ECHOLNPAIR("  Left: ", m.backlash[LEFT]);
+      #endif
+      #if ENABLED(CALIBRATION_MEASURE_RIGHT)
+        SERIAL_ECHOLNPAIR("  Right: ", m.backlash[RIGHT]);
+      #endif
+    #endif
+    #if AXIS_CAN_CALIBRATE(Y)
+      #if ENABLED(CALIBRATION_MEASURE_FRONT)
+        SERIAL_ECHOLNPAIR("  Front: ", m.backlash[FRONT]);
+      #endif
+      #if ENABLED(CALIBRATION_MEASURE_BACK)
+        SERIAL_ECHOLNPAIR("  Back: ", m.backlash[BACK]);
+      #endif
+    #endif
+    #if AXIS_CAN_CALIBRATE(Z)
+      SERIAL_ECHOLNPAIR("  Top: ", m.backlash[TOP]);
+    #endif
+    SERIAL_EOL();
+  }
+
+  inline void report_measured_positional_error(const measurements_t &m) {
+    SERIAL_CHAR('T');
+    SERIAL_ECHO(int(active_extruder));
+    SERIAL_ECHOLNPGM(" Positional Error:");
+    #if HAS_X_CENTER
+      SERIAL_ECHOLNPAIR_P(SP_X_STR, m.pos_error.x);
+    #endif
+    #if HAS_Y_CENTER
+      SERIAL_ECHOLNPAIR_P(SP_Y_STR, m.pos_error.y);
+    #endif
+    if (AXIS_CAN_CALIBRATE(Z)) SERIAL_ECHOLNPAIR_P(SP_Z_STR, m.pos_error.z);
+    SERIAL_EOL();
+  }
+
+  inline void report_measured_nozzle_dimensions(const measurements_t &m) {
+    SERIAL_ECHOLNPGM("Nozzle Tip Outer Dimensions:");
+    #if HAS_X_CENTER || HAS_Y_CENTER
+      #if HAS_X_CENTER
+        SERIAL_ECHOLNPAIR_P(SP_X_STR, m.nozzle_outer_dimension.x);
+      #endif
+      #if HAS_Y_CENTER
+        SERIAL_ECHOLNPAIR_P(SP_Y_STR, m.nozzle_outer_dimension.y);
+      #endif
+    #else
+      UNUSED(m);
+    #endif
+    SERIAL_EOL();
+  }
+
+  #if HAS_HOTEND_OFFSET
+    //
+    // This function requires normalize_hotend_offsets() to be called
+    //
+    inline void report_hotend_offsets() {
+      LOOP_S_L_N(e, 1, HOTENDS)
+        SERIAL_ECHOLNPAIR_P(PSTR("T"), int(e), PSTR(" Hotend Offset X"), hotend_offset[e].x, SP_Y_STR, hotend_offset[e].y, SP_Z_STR, hotend_offset[e].z);
+    }
+  #endif
+
+#endif // CALIBRATION_REPORTING
+
+/**
+ * Probe around the calibration object to measure backlash
+ *
+ *   m              in/out - Measurement record, updated with new readings
+ *   uncertainty    in     - How far away from the object to begin probing
+ */
+inline void calibrate_backlash(measurements_t &m, const float uncertainty) {
+  // Backlash compensation should be off while measuring backlash
+
+  {
+    // New scope for TEMPORARY_BACKLASH_CORRECTION
+    TEMPORARY_BACKLASH_CORRECTION(all_off);
+    TEMPORARY_BACKLASH_SMOOTHING(0.0f);
+
+    probe_sides(m, uncertainty);
+
+    #if ENABLED(BACKLASH_GCODE)
+
+      #if HAS_X_CENTER
+        backlash.distance_mm.x = (m.backlash[LEFT] + m.backlash[RIGHT]) / 2;
+      #elif ENABLED(CALIBRATION_MEASURE_LEFT)
+        backlash.distance_mm.x = m.backlash[LEFT];
+      #elif ENABLED(CALIBRATION_MEASURE_RIGHT)
+        backlash.distance_mm.x = m.backlash[RIGHT];
+      #endif
+
+      #if HAS_Y_CENTER
+        backlash.distance_mm.y = (m.backlash[FRONT] + m.backlash[BACK]) / 2;
+      #elif ENABLED(CALIBRATION_MEASURE_FRONT)
+        backlash.distance_mm.y = m.backlash[FRONT];
+      #elif ENABLED(CALIBRATION_MEASURE_BACK)
+        backlash.distance_mm.y = m.backlash[BACK];
+      #endif
+
+      if (AXIS_CAN_CALIBRATE(Z)) backlash.distance_mm.z = m.backlash[TOP];
+    #endif
+  }
+
+  #if ENABLED(BACKLASH_GCODE)
+    // Turn on backlash compensation and move in all
+    // allowed directions to take up any backlash
+    {
+      // New scope for TEMPORARY_BACKLASH_CORRECTION
+      TEMPORARY_BACKLASH_CORRECTION(all_on);
+      TEMPORARY_BACKLASH_SMOOTHING(0.0f);
+      const xyz_float_t move = { AXIS_CAN_CALIBRATE(X) * 3, AXIS_CAN_CALIBRATE(Y) * 3, AXIS_CAN_CALIBRATE(Z) * 3 };
+      current_position += move; calibration_move();
+      current_position -= move; calibration_move();
+    }
+  #endif
+}
+
+inline void update_measurements(measurements_t &m, const AxisEnum axis) {
+  current_position[axis] += m.pos_error[axis];
+  m.obj_center[axis] = true_center[axis];
+  m.pos_error[axis] = 0;
+}
+
+/**
+ * Probe around the calibration object. Adjust the position and toolhead offset
+ * using the deviation from the known position of the calibration object.
+ *
+ *   m              in/out - Measurement record, updated with new readings
+ *   uncertainty    in     - How far away from the object to begin probing
+ *   extruder       in     - What extruder to probe
+ *
+ * Prerequisites:
+ *    - Call calibrate_backlash() beforehand for best accuracy
+ */
+inline void calibrate_toolhead(measurements_t &m, const float uncertainty, const uint8_t extruder) {
+  TEMPORARY_BACKLASH_CORRECTION(all_on);
+  TEMPORARY_BACKLASH_SMOOTHING(0.0f);
+
+  #if HAS_MULTI_HOTEND
+    set_nozzle(m, extruder);
+  #else
+    UNUSED(extruder);
+  #endif
+
+  probe_sides(m, uncertainty);
+
+  // Adjust the hotend offset
+  #if HAS_HOTEND_OFFSET
+    if (ENABLED(HAS_X_CENTER) && AXIS_CAN_CALIBRATE(X)) hotend_offset[extruder].x += m.pos_error.x;
+    if (ENABLED(HAS_Y_CENTER) && AXIS_CAN_CALIBRATE(Y)) hotend_offset[extruder].y += m.pos_error.y;
+                             if (AXIS_CAN_CALIBRATE(Z)) hotend_offset[extruder].z += m.pos_error.z;
+    normalize_hotend_offsets();
+  #endif
+
+  // Correct for positional error, so the object
+  // is at the known actual spot
+  planner.synchronize();
+  if (ENABLED(HAS_X_CENTER) && AXIS_CAN_CALIBRATE(X)) update_measurements(m, X_AXIS);
+  if (ENABLED(HAS_Y_CENTER) && AXIS_CAN_CALIBRATE(Y)) update_measurements(m, Y_AXIS);
+                           if (AXIS_CAN_CALIBRATE(Z)) update_measurements(m, Z_AXIS);
+
+  sync_plan_position();
+}
+
+/**
+ * Probe around the calibration object for all toolheads, adjusting the coordinate
+ * system for the first nozzle and the nozzle offset for subsequent nozzles.
+ *
+ *   m              in/out - Measurement record, updated with new readings
+ *   uncertainty    in     - How far away from the object to begin probing
+ */
+inline void calibrate_all_toolheads(measurements_t &m, const float uncertainty) {
+  TEMPORARY_BACKLASH_CORRECTION(all_on);
+  TEMPORARY_BACKLASH_SMOOTHING(0.0f);
+
+  HOTEND_LOOP() calibrate_toolhead(m, uncertainty, e);
+
+  TERN_(HAS_HOTEND_OFFSET, normalize_hotend_offsets());
+
+  TERN_(HAS_MULTI_HOTEND, set_nozzle(m, 0));
+}
+
+/**
+ * Perform a full auto-calibration routine:
+ *
+ *   1) For each nozzle, touch top and sides of object to determine object position and
+ *      nozzle offsets. Do a fast but rough search over a wider area.
+ *   2) With the first nozzle, touch top and sides of object to determine backlash values
+ *      for all axis (if BACKLASH_GCODE is enabled)
+ *   3) For each nozzle, touch top and sides of object slowly to determine precise
+ *      position of object. Adjust coordinate system and nozzle offsets so probed object
+ *      location corresponds to known object location with a high degree of precision.
+ */
+inline void calibrate_all() {
+  measurements_t m;
+
+  TERN_(HAS_HOTEND_OFFSET, reset_hotend_offsets());
+
+  TEMPORARY_BACKLASH_CORRECTION(all_on);
+  TEMPORARY_BACKLASH_SMOOTHING(0.0f);
+
+  // Do a fast and rough calibration of the toolheads
+  calibrate_all_toolheads(m, CALIBRATION_MEASUREMENT_UNKNOWN);
+
+  TERN_(BACKLASH_GCODE, calibrate_backlash(m, CALIBRATION_MEASUREMENT_UNCERTAIN));
+
+  // Cycle the toolheads so the servos settle into their "natural" positions
+  #if HAS_MULTI_HOTEND
+    HOTEND_LOOP() set_nozzle(m, e);
+  #endif
+
+  // Do a slow and precise calibration of the toolheads
+  calibrate_all_toolheads(m, CALIBRATION_MEASUREMENT_UNCERTAIN);
+
+  current_position.x = X_CENTER;
+  calibration_move();         // Park nozzle away from calibration object
+}
+
+/**
+ * G425: Perform calibration with calibration object.
+ *
+ *   B           - Perform calibration of backlash only.
+ *   T<extruder> - Perform calibration of toolhead only.
+ *   V           - Probe object and print position, error, backlash and hotend offset.
+ *   U           - Uncertainty, how far to start probe away from the object (mm)
+ *
+ *   no args     - Perform entire calibration sequence (backlash + position on all toolheads)
+ */
+void GcodeSuite::G425() {
+
+  #ifdef CALIBRATION_SCRIPT_PRE
+    GcodeSuite::process_subcommands_now_P(PSTR(CALIBRATION_SCRIPT_PRE));
+  #endif
+
+  if (homing_needed_error()) return;
+
+  TEMPORARY_BED_LEVELING_STATE(false);
+  SET_SOFT_ENDSTOP_LOOSE(true);
+
+  measurements_t m;
+  float uncertainty = parser.seenval('U') ? parser.value_float() : CALIBRATION_MEASUREMENT_UNCERTAIN;
+
+  if (parser.seen('B'))
+    calibrate_backlash(m, uncertainty);
+  else if (parser.seen('T'))
+    calibrate_toolhead(m, uncertainty, parser.has_value() ? parser.value_int() : active_extruder);
+  #if ENABLED(CALIBRATION_REPORTING)
+    else if (parser.seen('V')) {
+      probe_sides(m, uncertainty);
+      SERIAL_EOL();
+      report_measured_faces(m);
+      report_measured_center(m);
+      report_measured_backlash(m);
+      report_measured_nozzle_dimensions(m);
+      report_measured_positional_error(m);
+      #if HAS_HOTEND_OFFSET
+        normalize_hotend_offsets();
+        report_hotend_offsets();
+      #endif
+    }
+  #endif
+  else
+    calibrate_all();
+
+  SET_SOFT_ENDSTOP_LOOSE(false);
+
+  #ifdef CALIBRATION_SCRIPT_POST
+    GcodeSuite::process_subcommands_now_P(PSTR(CALIBRATION_SCRIPT_POST));
+  #endif
+}
+
+#endif // CALIBRATION_GCODE
diff --git a/Marlin/src/gcode/calibrate/G76_M192_M871.cpp b/Marlin/src/gcode/calibrate/G76_M192_M871.cpp
new file mode 100644
index 0000000..5d0bb0d
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/G76_M192_M871.cpp
@@ -0,0 +1,369 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * G76_M871.cpp - Temperature calibration/compensation for z-probing
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(PROBE_TEMP_COMPENSATION)
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+#include "../../module/planner.h"
+#include "../../module/probe.h"
+#include "../../feature/bedlevel/bedlevel.h"
+#include "../../module/temperature.h"
+#include "../../module/probe.h"
+#include "../../feature/probe_temp_comp.h"
+
+#include "../../lcd/marlinui.h"
+#include "../../MarlinCore.h" // for wait_for_heatup, idle()
+
+#if ENABLED(PRINTJOB_TIMER_AUTOSTART)
+  #include "../../module/printcounter.h"
+#endif
+
+#if ENABLED(PRINTER_EVENTS_LEDS)
+  #include "../../feature/leds/leds.h"
+#endif
+
+/**
+ * G76: calibrate probe and/or bed temperature offsets
+ *  Notes:
+ *  - When calibrating probe, bed temperature is held constant.
+ *    Compensation values are deltas to first probe measurement at probe temp. = 30°C.
+ *  - When calibrating bed, probe temperature is held constant.
+ *    Compensation values are deltas to first probe measurement at bed temp. = 60°C.
+ *  - The hotend will not be heated at any time.
+ *  - On my Průša MK3S clone I put a piece of paper between the probe and the hotend
+ *    so the hotend fan would not cool my probe constantly. Alternativly you could just
+ *    make sure the fan is not running while running the calibration process.
+ *
+ *  Probe calibration:
+ *  - Moves probe to cooldown point.
+ *  - Heats up bed to 100°C.
+ *  - Moves probe to probing point (1mm above heatbed).
+ *  - Waits until probe reaches target temperature (30°C).
+ *  - Does a z-probing (=base value) and increases target temperature by 5°C.
+ *  - Waits until probe reaches increased target temperature.
+ *  - Does a z-probing (delta to base value will be a compensation value) and increases target temperature by 5°C.
+ *  - Repeats last two steps until max. temperature reached or timeout (i.e. probe does not heat up any further).
+ *  - Compensation values of higher temperatures will be extrapolated (using linear regression first).
+ *    While this is not exact by any means it is still better than simply using the last compensation value.
+ *
+ *  Bed calibration:
+ *  - Moves probe to cooldown point.
+ *  - Heats up bed to 60°C.
+ *  - Moves probe to probing point (1mm above heatbed).
+ *  - Waits until probe reaches target temperature (30°C).
+ *  - Does a z-probing (=base value) and increases bed temperature by 5°C.
+ *  - Moves probe to cooldown point.
+ *  - Waits until probe is below 30°C and bed has reached target temperature.
+ *  - Moves probe to probing point and waits until it reaches target temperature (30°C).
+ *  - Does a z-probing (delta to base value will be a compensation value) and increases bed temperature by 5°C.
+ *  - Repeats last four points until max. bed temperature reached (110°C) or timeout.
+ *  - Compensation values of higher temperatures will be extrapolated (using linear regression first).
+ *    While this is not exact by any means it is still better than simply using the last compensation value.
+ *
+ *  G76 [B | P]
+ *  - no flag - Both calibration procedures will be run.
+ *  - `B` - Run bed temperature calibration.
+ *  - `P` - Run probe temperature calibration.
+ */
+
+static void say_waiting_for()               { SERIAL_ECHOPGM("Waiting for "); }
+static void say_waiting_for_probe_heating() { say_waiting_for(); SERIAL_ECHOLNPGM("probe heating."); }
+static void say_successfully_calibrated()   { SERIAL_ECHOPGM("Successfully calibrated"); }
+static void say_failed_to_calibrate()       { SERIAL_ECHOPGM("!Failed to calibrate"); }
+
+void GcodeSuite::G76() {
+  // Check if heated bed is available and z-homing is done with probe
+  #if TEMP_SENSOR_BED == 0 || !(HOMING_Z_WITH_PROBE)
+    return;
+  #endif
+
+  auto report_temps = [](millis_t &ntr, millis_t timeout=0) {
+    idle_no_sleep();
+    const millis_t ms = millis();
+    if (ELAPSED(ms, ntr)) {
+      ntr = ms + 1000;
+      thermalManager.print_heater_states(active_extruder);
+    }
+    return (timeout && ELAPSED(ms, timeout));
+  };
+
+  auto wait_for_temps = [&](const float tb, const float tp, millis_t &ntr, const millis_t timeout=0) {
+    say_waiting_for(); SERIAL_ECHOLNPGM("bed and probe temperature.");
+    while (fabs(thermalManager.degBed() - tb) > 0.1f || thermalManager.degProbe() > tp)
+      if (report_temps(ntr, timeout)) return true;
+    return false;
+  };
+
+  auto g76_probe = [](const TempSensorID sid, uint16_t &targ, const xy_pos_t &nozpos) {
+    do_z_clearance(5.0); // Raise nozzle before probing
+    const float measured_z = probe.probe_at_point(nozpos, PROBE_PT_STOW, 0, false);  // verbose=0, probe_relative=false
+    if (isnan(measured_z))
+      SERIAL_ECHOLNPGM("!Received NAN. Aborting.");
+    else {
+      SERIAL_ECHOLNPAIR_F("Measured: ", measured_z);
+      if (targ == cali_info_init[sid].start_temp)
+        temp_comp.prepare_new_calibration(measured_z);
+      else
+        temp_comp.push_back_new_measurement(sid, measured_z);
+      targ += cali_info_init[sid].temp_res;
+    }
+    return measured_z;
+  };
+
+  #if ENABLED(BLTOUCH)
+    // Make sure any BLTouch error condition is cleared
+    bltouch_command(BLTOUCH_RESET, BLTOUCH_RESET_DELAY);
+    set_bltouch_deployed(false);
+  #endif
+
+  bool do_bed_cal = parser.boolval('B'), do_probe_cal = parser.boolval('P');
+  if (!do_bed_cal && !do_probe_cal) do_bed_cal = do_probe_cal = true;
+
+  // Synchronize with planner
+  planner.synchronize();
+
+  const xyz_pos_t parkpos = temp_comp.park_point,
+            probe_pos_xyz = xyz_pos_t(temp_comp.measure_point) + xyz_pos_t({ 0.0f, 0.0f, PTC_PROBE_HEATING_OFFSET }),
+              noz_pos_xyz = probe_pos_xyz - probe.offset_xy;  // Nozzle position based on probe position
+
+  if (do_bed_cal || do_probe_cal) {
+    // Ensure park position is reachable
+    bool reachable = position_is_reachable(parkpos) || WITHIN(parkpos.z, Z_MIN_POS - fslop, Z_MAX_POS + fslop);
+    if (!reachable)
+      SERIAL_ECHOLNPGM("!Park");
+    else {
+      // Ensure probe position is reachable
+      reachable = probe.can_reach(probe_pos_xyz);
+      if (!reachable) SERIAL_ECHOLNPGM("!Probe");
+    }
+
+    if (!reachable) {
+      SERIAL_ECHOLNPGM(" position unreachable - aborting.");
+      return;
+    }
+
+    process_subcommands_now_P(G28_STR);
+  }
+
+  remember_feedrate_scaling_off();
+
+
+  /******************************************
+   * Calibrate bed temperature offsets
+   ******************************************/
+
+  // Report temperatures every second and handle heating timeouts
+  millis_t next_temp_report = millis() + 1000;
+
+  auto report_targets = [&](const uint16_t tb, const uint16_t tp) {
+    SERIAL_ECHOLNPAIR("Target Bed:", tb, " Probe:", tp);
+  };
+
+  if (do_bed_cal) {
+
+    uint16_t target_bed = cali_info_init[TSI_BED].start_temp,
+             target_probe = temp_comp.bed_calib_probe_temp;
+
+    say_waiting_for(); SERIAL_ECHOLNPGM(" cooling.");
+    while (thermalManager.degBed() > target_bed || thermalManager.degProbe() > target_probe)
+      report_temps(next_temp_report);
+
+    // Disable leveling so it won't mess with us
+    TERN_(HAS_LEVELING, set_bed_leveling_enabled(false));
+
+    for (;;) {
+      thermalManager.setTargetBed(target_bed);
+
+      report_targets(target_bed, target_probe);
+
+      // Park nozzle
+      do_blocking_move_to(parkpos);
+
+      // Wait for heatbed to reach target temp and probe to cool below target temp
+      if (wait_for_temps(target_bed, target_probe, next_temp_report, millis() + MIN_TO_MS(15))) {
+        SERIAL_ECHOLNPGM("!Bed heating timeout.");
+        break;
+      }
+
+      // Move the nozzle to the probing point and wait for the probe to reach target temp
+      do_blocking_move_to(noz_pos_xyz);
+      say_waiting_for_probe_heating();
+      SERIAL_EOL();
+      while (thermalManager.degProbe() < target_probe)
+        report_temps(next_temp_report);
+
+      const float measured_z = g76_probe(TSI_BED, target_bed, noz_pos_xyz);
+      if (isnan(measured_z) || target_bed > BED_MAX_TARGET) break;
+    }
+
+    SERIAL_ECHOLNPAIR("Retrieved measurements: ", temp_comp.get_index());
+    if (temp_comp.finish_calibration(TSI_BED)) {
+      say_successfully_calibrated();
+      SERIAL_ECHOLNPGM(" bed.");
+    }
+    else {
+      say_failed_to_calibrate();
+      SERIAL_ECHOLNPGM(" bed. Values reset.");
+    }
+
+    // Cleanup
+    thermalManager.setTargetBed(0);
+    TERN_(HAS_LEVELING, set_bed_leveling_enabled(true));
+  } // do_bed_cal
+
+  /********************************************
+   * Calibrate probe temperature offsets
+   ********************************************/
+
+  if (do_probe_cal) {
+
+    // Park nozzle
+    do_blocking_move_to(parkpos);
+
+    // Initialize temperatures
+    const uint16_t target_bed = temp_comp.probe_calib_bed_temp;
+    thermalManager.setTargetBed(target_bed);
+
+    uint16_t target_probe = cali_info_init[TSI_PROBE].start_temp;
+
+    report_targets(target_bed, target_probe);
+
+    // Wait for heatbed to reach target temp and probe to cool below target temp
+    wait_for_temps(target_bed, target_probe, next_temp_report);
+
+    // Disable leveling so it won't mess with us
+    TERN_(HAS_LEVELING, set_bed_leveling_enabled(false));
+
+    bool timeout = false;
+    for (;;) {
+      // Move probe to probing point and wait for it to reach target temperature
+      do_blocking_move_to(noz_pos_xyz);
+
+      say_waiting_for_probe_heating();
+      SERIAL_ECHOLNPAIR(" Bed:", target_bed, " Probe:", target_probe);
+      const millis_t probe_timeout_ms = millis() + SEC_TO_MS(900UL);
+      while (thermalManager.degProbe() < target_probe) {
+        if (report_temps(next_temp_report, probe_timeout_ms)) {
+          SERIAL_ECHOLNPGM("!Probe heating timed out.");
+          timeout = true;
+          break;
+        }
+      }
+      if (timeout) break;
+
+      const float measured_z = g76_probe(TSI_PROBE, target_probe, noz_pos_xyz);
+      if (isnan(measured_z) || target_probe > cali_info_init[TSI_PROBE].end_temp) break;
+    }
+
+    SERIAL_ECHOLNPAIR("Retrieved measurements: ", temp_comp.get_index());
+    if (temp_comp.finish_calibration(TSI_PROBE))
+      say_successfully_calibrated();
+    else
+      say_failed_to_calibrate();
+    SERIAL_ECHOLNPGM(" probe.");
+
+    // Cleanup
+    thermalManager.setTargetBed(0);
+    TERN_(HAS_LEVELING, set_bed_leveling_enabled(true));
+
+    SERIAL_ECHOLNPGM("Final compensation values:");
+    temp_comp.print_offsets();
+  } // do_probe_cal
+
+  restore_feedrate_and_scaling();
+}
+
+/**
+ * M871: Report / reset temperature compensation offsets.
+ *       Note: This does not affect values in EEPROM until M500.
+ *
+ *   M871 [ R | B | P | E ]
+ *
+ *    No Parameters - Print current offset values.
+ *
+ * Select only one of these flags:
+ *    R - Reset all offsets to zero (i.e., disable compensation).
+ *    B - Manually set offset for bed
+ *    P - Manually set offset for probe
+ *    E - Manually set offset for extruder
+ *
+ * With B, P, or E:
+ *    I[index] - Index in the array
+ *    V[value] - Adjustment in µm
+ */
+void GcodeSuite::M871() {
+
+  if (parser.seen('R')) {
+    // Reset z-probe offsets to factory defaults
+    temp_comp.clear_all_offsets();
+    SERIAL_ECHOLNPGM("Offsets reset to default.");
+  }
+  else if (parser.seen("BPE")) {
+    if (!parser.seenval('V')) return;
+    const int16_t offset_val = parser.value_int();
+    if (!parser.seenval('I')) return;
+    const int16_t idx = parser.value_int();
+    const TempSensorID mod = (parser.seen('B') ? TSI_BED :
+                                #if ENABLED(USE_TEMP_EXT_COMPENSATION)
+                                  parser.seen('E') ? TSI_EXT :
+                                #endif
+                                TSI_PROBE
+                              );
+    if (idx > 0 && temp_comp.set_offset(mod, idx - 1, offset_val))
+      SERIAL_ECHOLNPAIR("Set value: ", offset_val);
+    else
+      SERIAL_ECHOLNPGM("!Invalid index. Failed to set value (note: value at index 0 is constant).");
+
+  }
+  else // Print current Z-probe adjustments. Note: Values in EEPROM might differ.
+    temp_comp.print_offsets();
+}
+
+/**
+ * M192: Wait for probe temperature sensor to reach a target
+ *
+ * Select only one of these flags:
+ *    R - Wait for heating or cooling
+ *    S - Wait only for heating
+ */
+void GcodeSuite::M192() {
+  if (DEBUGGING(DRYRUN)) return;
+
+  const bool no_wait_for_cooling = parser.seenval('S');
+  if (!no_wait_for_cooling && ! parser.seenval('R')) {
+    SERIAL_ERROR_MSG("No target temperature set.");
+    return;
+  }
+
+  const float target_temp = parser.value_celsius();
+  ui.set_status_P(thermalManager.isProbeBelowTemp(target_temp) ? GET_TEXT(MSG_PROBE_HEATING) : GET_TEXT(MSG_PROBE_COOLING));
+  thermalManager.wait_for_probe(target_temp, no_wait_for_cooling);
+}
+
+#endif // PROBE_TEMP_COMPENSATION
diff --git a/Marlin/src/gcode/calibrate/M100.cpp b/Marlin/src/gcode/calibrate/M100.cpp
new file mode 100644
index 0000000..9ac2380
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/M100.cpp
@@ -0,0 +1,379 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(M100_FREE_MEMORY_WATCHER)
+
+#include "../gcode.h"
+#include "../queue.h"
+#include "../../libs/hex_print.h"
+
+#include "../../MarlinCore.h" // for idle()
+
+/**
+ * M100 Free Memory Watcher
+ *
+ * This code watches the free memory block between the bottom of the heap and the top of the stack.
+ * This memory block is initialized and watched via the M100 command.
+ *
+ * M100 I   Initializes the free memory block and prints vitals statistics about the area
+ *
+ * M100 F   Identifies how much of the free memory block remains free and unused. It also
+ *          detects and reports any corruption within the free memory block that may have
+ *          happened due to errant firmware.
+ *
+ * M100 D   Does a hex display of the free memory block along with a flag for any errant
+ *          data that does not match the expected value.
+ *
+ * M100 C x Corrupts x locations within the free memory block. This is useful to check the
+ *          correctness of the M100 F and M100 D commands.
+ *
+ * Also, there are two support functions that can be called from a developer's C code.
+ *
+ *    uint16_t check_for_free_memory_corruption(PGM_P const free_memory_start);
+ *    void M100_dump_routine(PGM_P const title, const char * const start, const char * const end);
+ *
+ * Initial version by Roxy-3D
+ */
+#define M100_FREE_MEMORY_DUMPER     // Enable for the `M100 D` Dump sub-command
+#define M100_FREE_MEMORY_CORRUPTOR  // Enable for the `M100 C` Corrupt sub-command
+
+#define TEST_BYTE ((char) 0xE5)
+
+#if EITHER(__AVR__, IS_32BIT_TEENSY)
+
+  extern char __bss_end;
+  char *end_bss = &__bss_end,
+       *free_memory_start = end_bss, *free_memory_end = 0,
+       *stacklimit = 0, *heaplimit = 0;
+
+  #define MEMORY_END_CORRECTION 0
+
+#elif defined(TARGET_LPC1768)
+
+  extern char __bss_end__, __StackLimit, __HeapLimit;
+
+  char *end_bss = &__bss_end__,
+       *stacklimit = &__StackLimit,
+       *heaplimit = &__HeapLimit;
+
+  #define MEMORY_END_CORRECTION 0x200
+
+  char *free_memory_start = heaplimit,
+       *free_memory_end = stacklimit - MEMORY_END_CORRECTION;
+
+#elif defined(__SAM3X8E__)
+
+  extern char _ebss;
+
+  char *end_bss = &_ebss,
+       *free_memory_start = end_bss,
+       *free_memory_end = 0,
+       *stacklimit = 0,
+       *heaplimit = 0;
+
+  #define MEMORY_END_CORRECTION 0x10000  // need to stay well below 0x20080000 or M100 F crashes
+
+#elif defined(__SAMD51__)
+
+  extern unsigned int __bss_end__, __StackLimit, __HeapLimit;
+  extern "C" void * _sbrk(int incr);
+
+  void *end_bss = &__bss_end__,
+       *stacklimit = &__StackLimit,
+       *heaplimit = &__HeapLimit;
+
+  #define MEMORY_END_CORRECTION 0x400
+
+  char *free_memory_start = (char *)_sbrk(0) + 0x200,     //  Leave some heap space
+       *free_memory_end = (char *)stacklimit - MEMORY_END_CORRECTION;
+
+#else
+  #error "M100 - unsupported CPU"
+#endif
+
+//
+// Utility functions
+//
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wreturn-local-addr"
+
+// Location of a variable in its stack frame.
+// The returned address will be above the stack (after it returns).
+char *top_of_stack() {
+  char x;
+  return &x + 1; // x is pulled on return;
+}
+
+#pragma GCC diagnostic pop
+
+// Count the number of test bytes at the specified location.
+inline int32_t count_test_bytes(const char * const start_free_memory) {
+  for (uint32_t i = 0; i < 32000; i++)
+    if (char(start_free_memory[i]) != TEST_BYTE)
+      return i - 1;
+
+  return -1;
+}
+
+//
+// M100 sub-commands
+//
+
+#if ENABLED(M100_FREE_MEMORY_DUMPER)
+  /**
+   * M100 D
+   *  Dump the free memory block from brkval to the stack pointer.
+   *  malloc() eats memory from the start of the block and the stack grows
+   *  up from the bottom of the block. Solid test bytes indicate nothing has
+   *  used that memory yet. There should not be anything but test bytes within
+   *  the block. If so, it may indicate memory corruption due to a bad pointer.
+   *  Unexpected bytes are flagged in the right column.
+   */
+  inline void dump_free_memory(char *start_free_memory, char *end_free_memory) {
+    //
+    // Start and end the dump on a nice 16 byte boundary
+    // (even though the values are not 16-byte aligned).
+    //
+    start_free_memory = (char*)(uintptr_t(uint32_t(start_free_memory) & ~0xFUL)); // Align to 16-byte boundary
+    end_free_memory   = (char*)(uintptr_t(uint32_t(end_free_memory)   |  0xFUL)); // Align end_free_memory to the 15th byte (at or above end_free_memory)
+
+    // Dump command main loop
+    while (start_free_memory < end_free_memory) {
+      print_hex_address(start_free_memory);             // Print the address
+      SERIAL_CHAR(':');
+      LOOP_L_N(i, 16) {  // and 16 data bytes
+        if (i == 8) SERIAL_CHAR('-');
+        print_hex_byte(start_free_memory[i]);
+        SERIAL_CHAR(' ');
+      }
+      serial_delay(25);
+      SERIAL_CHAR('|');                   // Point out non test bytes
+      LOOP_L_N(i, 16) {
+        char ccc = (char)start_free_memory[i]; // cast to char before automatically casting to char on assignment, in case the compiler is broken
+        ccc = (ccc == TEST_BYTE) ? ' ' : '?';
+        SERIAL_CHAR(ccc);
+      }
+      SERIAL_EOL();
+      start_free_memory += 16;
+      serial_delay(25);
+      idle();
+    }
+  }
+
+  void M100_dump_routine(PGM_P const title, const char * const start, const char * const end) {
+    serialprintPGM(title);
+    SERIAL_EOL();
+    //
+    // Round the start and end locations to produce full lines of output
+    //
+    dump_free_memory(
+      (char*)(uintptr_t(uint32_t(start) & ~0xFUL)), // Align to 16-byte boundary
+      (char*)(uintptr_t(uint32_t(end)   |  0xFUL))  // Align end_free_memory to the 15th byte (at or above end_free_memory)
+    );
+  }
+
+#endif // M100_FREE_MEMORY_DUMPER
+
+inline int check_for_free_memory_corruption(PGM_P const title) {
+  serialprintPGM(title);
+
+  char *start_free_memory = free_memory_start, *end_free_memory = free_memory_end;
+  int n = end_free_memory - start_free_memory;
+
+  SERIAL_ECHOPAIR("\nfmc() n=", n);
+  SERIAL_ECHOPAIR("\nfree_memory_start=", hex_address(free_memory_start));
+  SERIAL_ECHOLNPAIR("  end_free_memory=", hex_address(end_free_memory));
+
+  if (end_free_memory < start_free_memory)  {
+    SERIAL_ECHOPGM(" end_free_memory < Heap ");
+    // SET_INPUT_PULLUP(63);           // if the developer has a switch wired up to their controller board
+    // safe_delay(5);                  // this code can be enabled to pause the display as soon as the
+    // while ( READ(63))               // malfunction is detected.   It is currently defaulting to a switch
+    //   idle();                       // being on pin-63 which is unassigend and available on most controller
+    // safe_delay(20);                 // boards.
+    // while ( !READ(63))
+    //   idle();
+    serial_delay(20);
+    #if ENABLED(M100_FREE_MEMORY_DUMPER)
+      M100_dump_routine(PSTR("   Memory corruption detected with end_free_memory<Heap\n"), (const char*)0x1B80, (const char*)0x21FF);
+    #endif
+  }
+
+  // Scan through the range looking for the biggest block of 0xE5's we can find
+  int block_cnt = 0;
+  for (int i = 0; i < n; i++) {
+    if (start_free_memory[i] == TEST_BYTE) {
+      int32_t j = count_test_bytes(start_free_memory + i);
+      if (j > 8) {
+        // SERIAL_ECHOPAIR("Found ", j);
+        // SERIAL_ECHOLNPAIR(" bytes free at ", hex_address(start_free_memory + i));
+        i += j;
+        block_cnt++;
+        SERIAL_ECHOPAIR(" (", block_cnt);
+        SERIAL_ECHOPAIR(") found=", j);
+        SERIAL_ECHOLNPGM("   ");
+      }
+    }
+  }
+  SERIAL_ECHOPAIR("  block_found=", block_cnt);
+
+  if (block_cnt != 1)
+    SERIAL_ECHOLNPGM("\nMemory Corruption detected in free memory area.");
+
+  if (block_cnt == 0)       // Make sure the special case of no free blocks shows up as an
+    block_cnt = -1;         //  error to the calling code!
+
+  SERIAL_ECHOPGM(" return=");
+  if (block_cnt == 1) {
+    SERIAL_CHAR('0');       // If the block_cnt is 1, nothing has broken up the free memory
+    SERIAL_EOL();           //  area and it is appropriate to say 'no corruption'.
+    return 0;
+  }
+  SERIAL_ECHOLNPGM("true");
+  return block_cnt;
+}
+
+/**
+ * M100 F
+ *  Return the number of free bytes in the memory pool,
+ *  with other vital statistics defining the pool.
+ */
+inline void free_memory_pool_report(char * const start_free_memory, const int32_t size) {
+  int32_t max_cnt = -1, block_cnt = 0;
+  char *max_addr = nullptr;
+  // Find the longest block of test bytes in the buffer
+  for (int32_t i = 0; i < size; i++) {
+    char *addr = start_free_memory + i;
+    if (*addr == TEST_BYTE) {
+      const int32_t j = count_test_bytes(addr);
+      if (j > 8) {
+        SERIAL_ECHOPAIR("Found ", j);
+        SERIAL_ECHOLNPAIR(" bytes free at ", hex_address(addr));
+        if (j > max_cnt) {
+          max_cnt  = j;
+          max_addr = addr;
+        }
+        i += j;
+        block_cnt++;
+      }
+    }
+  }
+  if (block_cnt > 1) {
+    SERIAL_ECHOLNPGM("\nMemory Corruption detected in free memory area.");
+    SERIAL_ECHOPAIR("\nLargest free block is ", max_cnt);
+    SERIAL_ECHOLNPAIR(" bytes at ", hex_address(max_addr));
+  }
+  SERIAL_ECHOLNPAIR("check_for_free_memory_corruption() = ", check_for_free_memory_corruption(PSTR("M100 F ")));
+}
+
+#if ENABLED(M100_FREE_MEMORY_CORRUPTOR)
+  /**
+   * M100 C<num>
+   *  Corrupt <num> locations in the free memory pool and report the corrupt addresses.
+   *  This is useful to check the correctness of the M100 D and the M100 F commands.
+   */
+  inline void corrupt_free_memory(char *start_free_memory, const uint32_t size) {
+    start_free_memory += 8;
+    const uint32_t near_top = top_of_stack() - start_free_memory - 250, // -250 to avoid interrupt activity that's altered the stack.
+                   j = near_top / (size + 1);
+
+    SERIAL_ECHOLNPGM("Corrupting free memory block.\n");
+    for (uint32_t i = 1; i <= size; i++) {
+      char * const addr = start_free_memory + i * j;
+      *addr = i;
+      SERIAL_ECHOPAIR("\nCorrupting address: ", hex_address(addr));
+    }
+    SERIAL_EOL();
+  }
+#endif // M100_FREE_MEMORY_CORRUPTOR
+
+/**
+ * M100 I
+ *  Init memory for the M100 tests. (Automatically applied on the first M100.)
+ */
+inline void init_free_memory(char *start_free_memory, int32_t size) {
+  SERIAL_ECHOLNPGM("Initializing free memory block.\n\n");
+
+  size -= 250;    // -250 to avoid interrupt activity that's altered the stack.
+  if (size < 0) {
+    SERIAL_ECHOLNPGM("Unable to initialize.\n");
+    return;
+  }
+
+  start_free_memory += 8;       // move a few bytes away from the heap just because we don't want
+                  // to be altering memory that close to it.
+  memset(start_free_memory, TEST_BYTE, size);
+
+  SERIAL_ECHO(size);
+  SERIAL_ECHOLNPGM(" bytes of memory initialized.\n");
+
+  for (int32_t i = 0; i < size; i++) {
+    if (start_free_memory[i] != TEST_BYTE) {
+      SERIAL_ECHOPAIR("? address : ", hex_address(start_free_memory + i));
+      SERIAL_ECHOLNPAIR("=", hex_byte(start_free_memory[i]));
+      SERIAL_EOL();
+    }
+  }
+}
+
+/**
+ * M100: Free Memory Check
+ */
+void GcodeSuite::M100() {
+
+  char *sp = top_of_stack();
+  if (!free_memory_end) free_memory_end = sp - MEMORY_END_CORRECTION;
+  SERIAL_ECHOPAIR("\nbss_end               : ", hex_address(end_bss));
+  if (heaplimit) SERIAL_ECHOPAIR("\n__heaplimit           : ", hex_address(heaplimit));
+  SERIAL_ECHOPAIR("\nfree_memory_start     : ", hex_address(free_memory_start));
+  if (stacklimit) SERIAL_ECHOPAIR("\n__stacklimit          : ", hex_address(stacklimit));
+  SERIAL_ECHOPAIR("\nfree_memory_end       : ", hex_address(free_memory_end));
+  if (MEMORY_END_CORRECTION)  SERIAL_ECHOPAIR("\nMEMORY_END_CORRECTION: ", MEMORY_END_CORRECTION);
+  SERIAL_ECHOLNPAIR("\nStack Pointer         : ", hex_address(sp));
+
+  // Always init on the first invocation of M100
+  static bool m100_not_initialized = true;
+  if (m100_not_initialized || parser.seen('I')) {
+    m100_not_initialized = false;
+    init_free_memory(free_memory_start, free_memory_end - free_memory_start);
+  }
+
+  #if ENABLED(M100_FREE_MEMORY_DUMPER)
+    if (parser.seen('D'))
+      return dump_free_memory(free_memory_start, free_memory_end);
+  #endif
+
+  if (parser.seen('F'))
+    return free_memory_pool_report(free_memory_start, free_memory_end - free_memory_start);
+
+  #if ENABLED(M100_FREE_MEMORY_CORRUPTOR)
+
+    if (parser.seen('C'))
+      return corrupt_free_memory(free_memory_start, parser.value_int());
+
+  #endif
+}
+
+#endif // M100_FREE_MEMORY_WATCHER
diff --git a/Marlin/src/gcode/calibrate/M12.cpp b/Marlin/src/gcode/calibrate/M12.cpp
new file mode 100644
index 0000000..da24454
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/M12.cpp
@@ -0,0 +1,39 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#include "../../inc/MarlinConfigPre.h"
+
+#if ENABLED(EXTERNAL_CLOSED_LOOP_CONTROLLER)
+
+#include "../gcode.h"
+#include "../../module/planner.h"
+#include "../../feature/closedloop.h"
+
+void GcodeSuite::M12() {
+
+  planner.synchronize();
+
+  if (parser.seenval('S'))
+    closedloop.set(parser.value_int()); // Force a CLC set
+
+}
+
+#endif
diff --git a/Marlin/src/gcode/calibrate/M425.cpp b/Marlin/src/gcode/calibrate/M425.cpp
new file mode 100644
index 0000000..40441ac
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/M425.cpp
@@ -0,0 +1,111 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(BACKLASH_GCODE)
+
+#include "../../feature/backlash.h"
+#include "../../module/planner.h"
+
+#include "../gcode.h"
+
+/**
+ * M425: Enable and tune backlash correction.
+ *
+ *   F<fraction>     Enable/disable/fade-out backlash correction (0.0 to 1.0)
+ *   S<smoothing_mm> Distance over which backlash correction is spread
+ *   X<distance_mm>  Set the backlash distance on X (0 to disable)
+ *   Y<distance_mm>                        ... on Y
+ *   Z<distance_mm>                        ... on Z
+ *   X               If a backlash measurement was done on X, copy that value
+ *   Y                                              ... on Y
+ *   Z                                              ... on Z
+ *
+ * Type M425 without any arguments to show active values.
+ */
+void GcodeSuite::M425() {
+  bool noArgs = true;
+
+  auto axis_can_calibrate = [](const uint8_t a) {
+    switch (a) {
+      default:
+      case X_AXIS: return AXIS_CAN_CALIBRATE(X);
+      case Y_AXIS: return AXIS_CAN_CALIBRATE(Y);
+      case Z_AXIS: return AXIS_CAN_CALIBRATE(Z);
+    }
+  };
+
+  LOOP_XYZ(a) {
+    if (axis_can_calibrate(a) && parser.seen(XYZ_CHAR(a))) {
+      planner.synchronize();
+      backlash.distance_mm[a] = parser.has_value() ? parser.value_linear_units() : backlash.get_measurement(AxisEnum(a));
+      noArgs = false;
+    }
+  }
+
+  if (parser.seen('F')) {
+    planner.synchronize();
+    backlash.set_correction(parser.value_float());
+    noArgs = false;
+  }
+
+  #ifdef BACKLASH_SMOOTHING_MM
+    if (parser.seen('S')) {
+      planner.synchronize();
+      backlash.smoothing_mm = parser.value_linear_units();
+      noArgs = false;
+    }
+  #endif
+
+  if (noArgs) {
+    SERIAL_ECHOPGM("Backlash Correction ");
+    if (!backlash.correction) SERIAL_ECHOPGM("in");
+    SERIAL_ECHOLNPGM("active:");
+    SERIAL_ECHOLNPAIR("  Correction Amount/Fade-out:     F", backlash.get_correction(), " (F1.0 = full, F0.0 = none)");
+    SERIAL_ECHOPGM("  Backlash Distance (mm):        ");
+    LOOP_XYZ(a) if (axis_can_calibrate(a)) {
+      SERIAL_CHAR(' ', XYZ_CHAR(a));
+      SERIAL_ECHO(backlash.distance_mm[a]);
+      SERIAL_EOL();
+    }
+
+    #ifdef BACKLASH_SMOOTHING_MM
+      SERIAL_ECHOLNPAIR("  Smoothing (mm):                 S", backlash.smoothing_mm);
+    #endif
+
+    #if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
+      SERIAL_ECHOPGM("  Average measured backlash (mm):");
+      if (backlash.has_any_measurement()) {
+        LOOP_XYZ(a) if (axis_can_calibrate(a) && backlash.has_measurement(AxisEnum(a))) {
+          SERIAL_CHAR(' ', XYZ_CHAR(a));
+          SERIAL_ECHO(backlash.get_measurement(AxisEnum(a)));
+        }
+      }
+      else
+        SERIAL_ECHOPGM(" (Not yet measured)");
+      SERIAL_EOL();
+    #endif
+  }
+}
+
+#endif // BACKLASH_GCODE
diff --git a/Marlin/src/gcode/calibrate/M48.cpp b/Marlin/src/gcode/calibrate/M48.cpp
new file mode 100644
index 0000000..97aea59
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/M48.cpp
@@ -0,0 +1,275 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST)
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+#include "../../module/probe.h"
+#include "../../lcd/marlinui.h"
+
+#include "../../feature/bedlevel/bedlevel.h"
+
+#if HAS_LEVELING
+  #include "../../module/planner.h"
+#endif
+
+/**
+ * M48: Z probe repeatability measurement function.
+ *
+ * Usage:
+ *   M48 <P#> <X#> <Y#> <V#> <E> <L#> <S>
+ *     P = Number of sampled points (4-50, default 10)
+ *     X = Sample X position
+ *     Y = Sample Y position
+ *     V = Verbose level (0-4, default=1)
+ *     E = Engage Z probe for each reading
+ *     L = Number of legs of movement before probe
+ *     S = Schizoid (Or Star if you prefer)
+ *
+ * This function requires the machine to be homed before invocation.
+ */
+
+void GcodeSuite::M48() {
+
+  if (homing_needed_error()) return;
+
+  const int8_t verbose_level = parser.byteval('V', 1);
+  if (!WITHIN(verbose_level, 0, 4)) {
+    SERIAL_ECHOLNPGM("?(V)erbose level implausible (0-4).");
+    return;
+  }
+
+  if (verbose_level > 0)
+    SERIAL_ECHOLNPGM("M48 Z-Probe Repeatability Test");
+
+  const int8_t n_samples = parser.byteval('P', 10);
+  if (!WITHIN(n_samples, 4, 50)) {
+    SERIAL_ECHOLNPGM("?Sample size not plausible (4-50).");
+    return;
+  }
+
+  const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE;
+
+  // Test at the current position by default, overridden by X and Y
+  const xy_pos_t test_position = {
+    parser.linearval('X', current_position.x + probe.offset_xy.x),  // If no X use the probe's current X position
+    parser.linearval('Y', current_position.y + probe.offset_xy.y)   // If no Y, ditto
+  };
+
+  if (!probe.can_reach(test_position)) {
+    ui.set_status_P(GET_TEXT(MSG_M48_OUT_OF_BOUNDS), 99);
+    SERIAL_ECHOLNPGM("? (X,Y) out of bounds.");
+    return;
+  }
+
+  // Get the number of leg moves per test-point
+  bool seen_L = parser.seen('L');
+  uint8_t n_legs = seen_L ? parser.value_byte() : 0;
+  if (n_legs > 15) {
+    SERIAL_ECHOLNPGM("?Legs of movement implausible (0-15).");
+    return;
+  }
+  if (n_legs == 1) n_legs = 2;
+
+  // Schizoid motion as an optional stress-test
+  const bool schizoid_flag = parser.boolval('S');
+  if (schizoid_flag && !seen_L) n_legs = 7;
+
+  if (verbose_level > 2)
+    SERIAL_ECHOLNPGM("Positioning the probe...");
+
+  // Always disable Bed Level correction before probing...
+
+  #if HAS_LEVELING
+    const bool was_enabled = planner.leveling_active;
+    set_bed_leveling_enabled(false);
+  #endif
+
+  // Work with reasonable feedrates
+  remember_feedrate_scaling_off();
+
+  // Working variables
+  float mean = 0.0,     // The average of all points so far, used to calculate deviation
+        sigma = 0.0,    // Standard deviation of all points so far
+        min = 99999.9,  // Smallest value sampled so far
+        max = -99999.9, // Largest value sampled so far
+        sample_set[n_samples];  // Storage for sampled values
+
+  auto dev_report = [](const bool verbose, const float &mean, const float &sigma, const float &min, const float &max, const bool final=false) {
+    if (verbose) {
+      SERIAL_ECHOPAIR_F("Mean: ", mean, 6);
+      if (!final) SERIAL_ECHOPAIR_F(" Sigma: ", sigma, 6);
+      SERIAL_ECHOPAIR_F(" Min: ", min, 3);
+      SERIAL_ECHOPAIR_F(" Max: ", max, 3);
+      SERIAL_ECHOPAIR_F(" Range: ", max-min, 3);
+      if (final) SERIAL_EOL();
+    }
+    if (final) {
+      SERIAL_ECHOLNPAIR_F("Standard Deviation: ", sigma, 6);
+      SERIAL_EOL();
+    }
+  };
+
+  // Move to the first point, deploy, and probe
+  const float t = probe.probe_at_point(test_position, raise_after, verbose_level);
+  bool probing_good = !isnan(t);
+
+  if (probing_good) {
+    randomSeed(millis());
+
+    float sample_sum = 0.0;
+
+    LOOP_L_N(n, n_samples) {
+      #if HAS_WIRED_LCD
+        // Display M48 progress in the status bar
+        ui.status_printf_P(0, PSTR(S_FMT ": %d/%d"), GET_TEXT(MSG_M48_POINT), int(n + 1), int(n_samples));
+      #endif
+
+      // When there are "legs" of movement move around the point before probing
+      if (n_legs) {
+
+        // Pick a random direction, starting angle, and radius
+        const int dir = (random(0, 10) > 5.0) ? -1 : 1;  // clockwise or counter clockwise
+        float angle = random(0, 360);
+        const float radius = random(
+          #if ENABLED(DELTA)
+            int(0.1250000000 * (DELTA_PRINTABLE_RADIUS)),
+            int(0.3333333333 * (DELTA_PRINTABLE_RADIUS))
+          #else
+            int(5), int(0.125 * _MIN(X_BED_SIZE, Y_BED_SIZE))
+          #endif
+        );
+        if (verbose_level > 3) {
+          SERIAL_ECHOPAIR("Start radius:", radius, " angle:", angle, " dir:");
+          if (dir > 0) SERIAL_CHAR('C');
+          SERIAL_ECHOLNPGM("CW");
+        }
+
+        // Move from leg to leg in rapid succession
+        LOOP_L_N(l, n_legs - 1) {
+
+          // Move some distance around the perimeter
+          float delta_angle;
+          if (schizoid_flag) {
+            // The points of a 5 point star are 72 degrees apart.
+            // Skip a point and go to the next one on the star.
+            delta_angle = dir * 2.0 * 72.0;
+          }
+          else {
+            // Just move further along the perimeter.
+            delta_angle = dir * (float)random(25, 45);
+          }
+          angle += delta_angle;
+
+          // Trig functions work without clamping, but just to be safe...
+          while (angle > 360.0) angle -= 360.0;
+          while (angle < 0.0) angle += 360.0;
+
+          // Choose the next position as an offset to chosen test position
+          const xy_pos_t noz_pos = test_position - probe.offset_xy;
+          xy_pos_t next_pos = {
+            noz_pos.x + float(cos(RADIANS(angle))) * radius,
+            noz_pos.y + float(sin(RADIANS(angle))) * radius
+          };
+
+          #if ENABLED(DELTA)
+            // If the probe can't reach the point on a round bed...
+            // Simply scale the numbers to bring them closer to origin.
+            while (!probe.can_reach(next_pos)) {
+              next_pos *= 0.8f;
+              if (verbose_level > 3)
+                SERIAL_ECHOLNPAIR_P(PSTR("Moving inward: X"), next_pos.x, SP_Y_STR, next_pos.y);
+            }
+          #else
+            // For a rectangular bed just keep the probe in bounds
+            LIMIT(next_pos.x, X_MIN_POS, X_MAX_POS);
+            LIMIT(next_pos.y, Y_MIN_POS, Y_MAX_POS);
+          #endif
+
+          if (verbose_level > 3)
+            SERIAL_ECHOLNPAIR_P(PSTR("Going to: X"), next_pos.x, SP_Y_STR, next_pos.y);
+
+          do_blocking_move_to_xy(next_pos);
+        } // n_legs loop
+      } // n_legs
+
+      // Probe a single point
+      const float pz = probe.probe_at_point(test_position, raise_after, 0);
+
+      // Break the loop if the probe fails
+      probing_good = !isnan(pz);
+      if (!probing_good) break;
+
+      // Store the new sample
+      sample_set[n] = pz;
+
+      // Keep track of the largest and smallest samples
+      NOMORE(min, pz);
+      NOLESS(max, pz);
+
+      // Get the mean value of all samples thus far
+      sample_sum += pz;
+      mean = sample_sum / (n + 1);
+
+      // Calculate the standard deviation so far.
+      // The value after the last sample will be the final output.
+      float dev_sum = 0.0;
+      LOOP_LE_N(j, n) dev_sum += sq(sample_set[j] - mean);
+      sigma = SQRT(dev_sum / (n + 1));
+
+      if (verbose_level > 1) {
+        SERIAL_ECHO((int)(n + 1));
+        SERIAL_ECHOPAIR(" of ", (int)n_samples);
+        SERIAL_ECHOPAIR_F(": z: ", pz, 3);
+        SERIAL_CHAR(' ');
+        dev_report(verbose_level > 2, mean, sigma, min, max);
+        SERIAL_EOL();
+      }
+
+    } // n_samples loop
+  }
+
+  probe.stow();
+
+  if (probing_good) {
+    SERIAL_ECHOLNPGM("Finished!");
+    dev_report(verbose_level > 0, mean, sigma, min, max, true);
+
+    #if HAS_WIRED_LCD
+      // Display M48 results in the status bar
+      char sigma_str[8];
+      ui.status_printf_P(0, PSTR(S_FMT ": %s"), GET_TEXT(MSG_M48_DEVIATION), dtostrf(sigma, 2, 6, sigma_str));
+    #endif
+  }
+
+  restore_feedrate_and_scaling();
+
+  // Re-enable bed level correction if it had been on
+  TERN_(HAS_LEVELING, set_bed_leveling_enabled(was_enabled));
+
+  report_current_position();
+}
+
+#endif // Z_MIN_PROBE_REPEATABILITY_TEST
diff --git a/Marlin/src/gcode/calibrate/M665.cpp b/Marlin/src/gcode/calibrate/M665.cpp
new file mode 100644
index 0000000..557204c
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/M665.cpp
@@ -0,0 +1,112 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if IS_KINEMATIC
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+
+#if ENABLED(DELTA)
+
+  #include "../../module/delta.h"
+  /**
+   * M665: Set delta configurations
+   *
+   *    H = delta height
+   *    L = diagonal rod
+   *    R = delta radius
+   *    S = segments per second
+   *    X = Alpha (Tower 1) angle trim
+   *    Y = Beta  (Tower 2) angle trim
+   *    Z = Gamma (Tower 3) angle trim
+   *    A = Alpha (Tower 1) digonal rod trim
+   *    B = Beta  (Tower 2) digonal rod trim
+   *    C = Gamma (Tower 3) digonal rod trim
+   */
+  void GcodeSuite::M665() {
+    if (parser.seen('H')) delta_height              = parser.value_linear_units();
+    if (parser.seen('L')) delta_diagonal_rod        = parser.value_linear_units();
+    if (parser.seen('R')) delta_radius              = parser.value_linear_units();
+    if (parser.seen('S')) delta_segments_per_second = parser.value_float();
+    if (parser.seen('X')) delta_tower_angle_trim.a  = parser.value_float();
+    if (parser.seen('Y')) delta_tower_angle_trim.b  = parser.value_float();
+    if (parser.seen('Z')) delta_tower_angle_trim.c  = parser.value_float();
+    if (parser.seen('A')) delta_diagonal_rod_trim.a = parser.value_float();
+    if (parser.seen('B')) delta_diagonal_rod_trim.b = parser.value_float();
+    if (parser.seen('C')) delta_diagonal_rod_trim.c = parser.value_float();
+    recalc_delta_settings();
+  }
+
+#elif IS_SCARA
+
+  #include "../../module/scara.h"
+
+  /**
+   * M665: Set SCARA settings
+   *
+   * Parameters:
+   *
+   *   S[segments-per-second] - Segments-per-second
+   *   P[theta-psi-offset]    - Theta-Psi offset, added to the shoulder (A/X) angle
+   *   T[theta-offset]        - Theta     offset, added to the elbow    (B/Y) angle
+   *   Z[z-offset]            - Z offset, added to Z
+   *
+   *   A, P, and X are all aliases for the shoulder angle
+   *   B, T, and Y are all aliases for the elbow angle
+   */
+  void GcodeSuite::M665() {
+    if (parser.seenval('S')) delta_segments_per_second = parser.value_float();
+
+    #if HAS_SCARA_OFFSET
+
+      if (parser.seenval('Z')) scara_home_offset.z = parser.value_linear_units();
+
+      const bool hasA = parser.seenval('A'), hasP = parser.seenval('P'), hasX = parser.seenval('X');
+      const uint8_t sumAPX = hasA + hasP + hasX;
+      if (sumAPX) {
+        if (sumAPX == 1)
+          scara_home_offset.a = parser.value_float();
+        else {
+          SERIAL_ERROR_MSG("Only one of A, P, or X is allowed.");
+          return;
+        }
+      }
+
+      const bool hasB = parser.seenval('B'), hasT = parser.seenval('T'), hasY = parser.seenval('Y');
+      const uint8_t sumBTY = hasB + hasT + hasY;
+      if (sumBTY) {
+        if (sumBTY == 1)
+          scara_home_offset.b = parser.value_float();
+        else {
+          SERIAL_ERROR_MSG("Only one of B, T, or Y is allowed.");
+          return;
+        }
+      }
+
+    #endif // HAS_SCARA_OFFSET
+  }
+
+#endif
+
+#endif // IS_KINEMATIC
diff --git a/Marlin/src/gcode/calibrate/M666.cpp b/Marlin/src/gcode/calibrate/M666.cpp
new file mode 100644
index 0000000..e915aa8
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/M666.cpp
@@ -0,0 +1,105 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(DELTA) || HAS_EXTRA_ENDSTOPS
+
+#include "../gcode.h"
+
+#if ENABLED(DELTA)
+
+  #include "../../module/delta.h"
+  #include "../../module/motion.h"
+
+  #define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+  #include "../../core/debug_out.h"
+
+  /**
+   * M666: Set delta endstop adjustment
+   */
+  void GcodeSuite::M666() {
+    DEBUG_SECTION(log_M666, "M666", DEBUGGING(LEVELING));
+    LOOP_XYZ(i) {
+      if (parser.seen(XYZ_CHAR(i))) {
+        const float v = parser.value_linear_units();
+        if (v * Z_HOME_DIR <= 0) delta_endstop_adj[i] = v;
+        if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("delta_endstop_adj[", XYZ_CHAR(i), "] = ", delta_endstop_adj[i]);
+      }
+    }
+  }
+
+#elif HAS_EXTRA_ENDSTOPS
+
+  #include "../../module/endstops.h"
+
+  /**
+   * M666: Set Dual Endstops offsets for X, Y, and/or Z.
+   *       With no parameters report current offsets.
+   *
+   * For Triple / Quad Z Endstops:
+   *   Set Z2 Only: M666 S2 Z<offset>
+   *   Set Z3 Only: M666 S3 Z<offset>
+   *   Set Z4 Only: M666 S4 Z<offset>
+   *       Set All: M666 Z<offset>
+   */
+  void GcodeSuite::M666() {
+    #if ENABLED(X_DUAL_ENDSTOPS)
+      if (parser.seenval('X')) endstops.x2_endstop_adj = parser.value_linear_units();
+    #endif
+    #if ENABLED(Y_DUAL_ENDSTOPS)
+      if (parser.seenval('Y')) endstops.y2_endstop_adj = parser.value_linear_units();
+    #endif
+    #if ENABLED(Z_MULTI_ENDSTOPS)
+      if (parser.seenval('Z')) {
+        #if NUM_Z_STEPPER_DRIVERS >= 3
+          const float z_adj = parser.value_linear_units();
+          const int ind = parser.intval('S');
+          if (!ind || ind == 2) endstops.z2_endstop_adj = z_adj;
+          if (!ind || ind == 3) endstops.z3_endstop_adj = z_adj;
+          #if NUM_Z_STEPPER_DRIVERS >= 4
+            if (!ind || ind == 4) endstops.z4_endstop_adj = z_adj;
+          #endif
+        #else
+          endstops.z2_endstop_adj = parser.value_linear_units();
+        #endif
+      }
+    #endif
+    if (!parser.seen("XYZ")) {
+      SERIAL_ECHOPGM("Dual Endstop Adjustment (mm): ");
+      #if ENABLED(X_DUAL_ENDSTOPS)
+        SERIAL_ECHOPAIR(" X2:", endstops.x2_endstop_adj);
+      #endif
+      #if ENABLED(Y_DUAL_ENDSTOPS)
+        SERIAL_ECHOPAIR(" Y2:", endstops.y2_endstop_adj);
+      #endif
+      #if ENABLED(Z_MULTI_ENDSTOPS)
+        #define _ECHO_ZADJ(N) SERIAL_ECHOPAIR(" Z" STRINGIFY(N) ":", endstops.z##N##_endstop_adj);
+        REPEAT_S(2, INCREMENT(NUM_Z_STEPPER_DRIVERS), _ECHO_ZADJ)
+      #endif
+      SERIAL_EOL();
+    }
+  }
+
+#endif // HAS_EXTRA_ENDSTOPS
+
+#endif // DELTA || HAS_EXTRA_ENDSTOPS
diff --git a/Marlin/src/gcode/calibrate/M852.cpp b/Marlin/src/gcode/calibrate/M852.cpp
new file mode 100644
index 0000000..b60f417
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/M852.cpp
@@ -0,0 +1,106 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(SKEW_CORRECTION_GCODE)
+
+#include "../gcode.h"
+#include "../../module/planner.h"
+
+/**
+ * M852: Get or set the machine skew factors. Reports current values with no arguments.
+ *
+ *  S[xy_factor] - Alias for 'I'
+ *  I[xy_factor] - New XY skew factor
+ *  J[xz_factor] - New XZ skew factor
+ *  K[yz_factor] - New YZ skew factor
+ */
+void GcodeSuite::M852() {
+  uint8_t ijk = 0, badval = 0, setval = 0;
+
+  if (parser.seen('I') || parser.seen('S')) {
+    ++ijk;
+    const float value = parser.value_linear_units();
+    if (WITHIN(value, SKEW_FACTOR_MIN, SKEW_FACTOR_MAX)) {
+      if (planner.skew_factor.xy != value) {
+        planner.skew_factor.xy = value;
+        ++setval;
+      }
+    }
+    else
+      ++badval;
+  }
+
+  #if ENABLED(SKEW_CORRECTION_FOR_Z)
+
+    if (parser.seen('J')) {
+      ++ijk;
+      const float value = parser.value_linear_units();
+      if (WITHIN(value, SKEW_FACTOR_MIN, SKEW_FACTOR_MAX)) {
+        if (planner.skew_factor.xz != value) {
+          planner.skew_factor.xz = value;
+          ++setval;
+        }
+      }
+      else
+        ++badval;
+    }
+
+    if (parser.seen('K')) {
+      ++ijk;
+      const float value = parser.value_linear_units();
+      if (WITHIN(value, SKEW_FACTOR_MIN, SKEW_FACTOR_MAX)) {
+        if (planner.skew_factor.yz != value) {
+          planner.skew_factor.yz = value;
+          ++setval;
+        }
+      }
+      else
+        ++badval;
+    }
+
+  #endif
+
+  if (badval)
+    SERIAL_ECHOLNPGM(STR_SKEW_MIN " " STRINGIFY(SKEW_FACTOR_MIN) " " STR_SKEW_MAX " " STRINGIFY(SKEW_FACTOR_MAX));
+
+  // When skew is changed the current position changes
+  if (setval) {
+    set_current_from_steppers_for_axis(ALL_AXES);
+    sync_plan_position();
+    report_current_position();
+  }
+
+  if (!ijk) {
+    SERIAL_ECHO_START();
+    serialprintPGM(GET_TEXT(MSG_SKEW_FACTOR));
+    SERIAL_ECHOPAIR_F(" XY: ", planner.skew_factor.xy, 6);
+    #if ENABLED(SKEW_CORRECTION_FOR_Z)
+      SERIAL_ECHOPAIR_F(" XZ: ", planner.skew_factor.xz, 6);
+      SERIAL_ECHOPAIR_F(" YZ: ", planner.skew_factor.yz, 6);
+    #endif
+    SERIAL_EOL();
+  }
+}
+
+#endif // SKEW_CORRECTION_GCODE