From e8701195e66f2d27ffe17fb514eae8173795aaf7 Mon Sep 17 00:00:00 2001
From: Georgiy Bondarenko <69736697+nehilo@users.noreply.github.com>
Date: Thu, 4 Mar 2021 22:54:23 +0500
Subject: Initial commit
---
Marlin/src/gcode/calibrate/G28.cpp | 493 ++++++++++++++++++++
Marlin/src/gcode/calibrate/G33.cpp | 648 +++++++++++++++++++++++++++
Marlin/src/gcode/calibrate/G34.cpp | 157 +++++++
Marlin/src/gcode/calibrate/G34_M422.cpp | 533 ++++++++++++++++++++++
Marlin/src/gcode/calibrate/G425.cpp | 623 +++++++++++++++++++++++++
Marlin/src/gcode/calibrate/G76_M192_M871.cpp | 369 +++++++++++++++
Marlin/src/gcode/calibrate/M100.cpp | 379 ++++++++++++++++
Marlin/src/gcode/calibrate/M12.cpp | 39 ++
Marlin/src/gcode/calibrate/M425.cpp | 111 +++++
Marlin/src/gcode/calibrate/M48.cpp | 275 ++++++++++++
Marlin/src/gcode/calibrate/M665.cpp | 112 +++++
Marlin/src/gcode/calibrate/M666.cpp | 105 +++++
Marlin/src/gcode/calibrate/M852.cpp | 106 +++++
13 files changed, 3950 insertions(+)
create mode 100644 Marlin/src/gcode/calibrate/G28.cpp
create mode 100644 Marlin/src/gcode/calibrate/G33.cpp
create mode 100644 Marlin/src/gcode/calibrate/G34.cpp
create mode 100644 Marlin/src/gcode/calibrate/G34_M422.cpp
create mode 100644 Marlin/src/gcode/calibrate/G425.cpp
create mode 100644 Marlin/src/gcode/calibrate/G76_M192_M871.cpp
create mode 100644 Marlin/src/gcode/calibrate/M100.cpp
create mode 100644 Marlin/src/gcode/calibrate/M12.cpp
create mode 100644 Marlin/src/gcode/calibrate/M425.cpp
create mode 100644 Marlin/src/gcode/calibrate/M48.cpp
create mode 100644 Marlin/src/gcode/calibrate/M665.cpp
create mode 100644 Marlin/src/gcode/calibrate/M666.cpp
create mode 100644 Marlin/src/gcode/calibrate/M852.cpp
(limited to 'Marlin/src/gcode/calibrate')
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 .
+ *
+ */
+
+#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 .
+ *
+ */
+
+#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; VARCEN+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 .
+ *
+ */
+
+#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 .
+ *
+ */
+
+#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 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 Number of tests. If omitted, Z_STEPPER_ALIGN_ITERATIONS.
+ * T Target Accuracy factor. If omitted, Z_STEPPER_ALIGN_ACC.
+ * A 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 of the probe point to set
+ *
+ * With Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS:
+ * W : 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 : X position to set (Unchanged if omitted)
+ * Y : 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 .
+ *
+ */
+
+#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 - 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 .
+ *
+ */
+
+/**
+ * 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 .
+ *
+ */
+
+#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 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
+ * Corrupt 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 .
+ *
+ */
+#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 .
+ *
+ */
+
+#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 Enable/disable/fade-out backlash correction (0.0 to 1.0)
+ * S Distance over which backlash correction is spread
+ * X Set the backlash distance on X (0 to disable)
+ * Y ... on Y
+ * Z ... 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 .
+ *
+ */
+
+#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 = 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 .
+ *
+ */
+
+#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 .
+ *
+ */
+
+#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
+ * Set Z3 Only: M666 S3 Z
+ * Set Z4 Only: M666 S4 Z
+ * Set All: M666 Z
+ */
+ 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 .
+ *
+ */
+
+#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
--
cgit v1.2.3