diff options
Diffstat (limited to 'Marlin/src/gcode/bedlevel/abl/G29.cpp')
| -rw-r--r-- | Marlin/src/gcode/bedlevel/abl/G29.cpp | 901 |
1 files changed, 901 insertions, 0 deletions
diff --git a/Marlin/src/gcode/bedlevel/abl/G29.cpp b/Marlin/src/gcode/bedlevel/abl/G29.cpp new file mode 100644 index 0000000..2e80f09 --- /dev/null +++ b/Marlin/src/gcode/bedlevel/abl/G29.cpp @@ -0,0 +1,901 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ + +/** + * G29.cpp - Auto Bed Leveling + */ + +#include "../../../inc/MarlinConfig.h" + +#if HAS_ABL_NOT_UBL + +#include "../../gcode.h" +#include "../../../feature/bedlevel/bedlevel.h" +#include "../../../module/motion.h" +#include "../../../module/planner.h" +#include "../../../module/stepper.h" +#include "../../../module/probe.h" +#include "../../queue.h" + +#if ENABLED(PROBE_TEMP_COMPENSATION) + #include "../../../feature/probe_temp_comp.h" + #include "../../../module/temperature.h" +#endif + +#if HAS_DISPLAY + #include "../../../lcd/marlinui.h" +#endif + +#if ENABLED(AUTO_BED_LEVELING_LINEAR) + #include "../../../libs/least_squares_fit.h" +#endif + +#if ABL_PLANAR + #include "../../../libs/vector_3.h" +#endif + +#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE) +#include "../../../core/debug_out.h" + +#if ENABLED(EXTENSIBLE_UI) + #include "../../../lcd/extui/ui_api.h" +#endif + +#if ENABLED(DWIN_CREALITY_LCD) + #include "../../../lcd/dwin/e3v2/dwin.h" +#endif + +#if HAS_MULTI_HOTEND + #include "../../../module/tool_change.h" +#endif + +#if ABL_GRID + #if ENABLED(PROBE_Y_FIRST) + #define PR_OUTER_VAR meshCount.x + #define PR_OUTER_END abl_grid_points.x + #define PR_INNER_VAR meshCount.y + #define PR_INNER_END abl_grid_points.y + #else + #define PR_OUTER_VAR meshCount.y + #define PR_OUTER_END abl_grid_points.y + #define PR_INNER_VAR meshCount.x + #define PR_INNER_END abl_grid_points.x + #endif +#endif + +#define G29_RETURN(b) return TERN_(G29_RETRY_AND_RECOVER, b) + +/** + * G29: Detailed Z probe, probes the bed at 3 or more points. + * Will fail if the printer has not been homed with G28. + * + * Enhanced G29 Auto Bed Leveling Probe Routine + * + * O Auto-level only if needed + * + * D Dry-Run mode. Just evaluate the bed Topology - Don't apply + * or alter the bed level data. Useful to check the topology + * after a first run of G29. + * + * J Jettison current bed leveling data + * + * V Set the verbose level (0-4). Example: "G29 V3" + * + * Parameters With LINEAR leveling only: + * + * P Set the size of the grid that will be probed (P x P points). + * Example: "G29 P4" + * + * X Set the X size of the grid that will be probed (X x Y points). + * Example: "G29 X7 Y5" + * + * Y Set the Y size of the grid that will be probed (X x Y points). + * + * T Generate a Bed Topology Report. Example: "G29 P5 T" for a detailed report. + * This is useful for manual bed leveling and finding flaws in the bed (to + * assist with part placement). + * Not supported by non-linear delta printer bed leveling. + * + * Parameters With LINEAR and BILINEAR leveling only: + * + * S Set the XY travel speed between probe points (in units/min) + * + * H Set bounds to a centered square H x H units in size + * + * -or- + * + * F Set the Front limit of the probing grid + * B Set the Back limit of the probing grid + * L Set the Left limit of the probing grid + * R Set the Right limit of the probing grid + * + * Parameters with DEBUG_LEVELING_FEATURE only: + * + * C Make a totally fake grid with no actual probing. + * For use in testing when no probing is possible. + * + * Parameters with BILINEAR leveling only: + * + * Z Supply an additional Z probe offset + * + * Extra parameters with PROBE_MANUALLY: + * + * To do manual probing simply repeat G29 until the procedure is complete. + * The first G29 accepts parameters. 'G29 Q' for status, 'G29 A' to abort. + * + * Q Query leveling and G29 state + * + * A Abort current leveling procedure + * + * Extra parameters with BILINEAR only: + * + * W Write a mesh point. (If G29 is idle.) + * I X index for mesh point + * J Y index for mesh point + * X X for mesh point, overrides I + * Y Y for mesh point, overrides J + * Z Z for mesh point. Otherwise, raw current Z. + * + * Without PROBE_MANUALLY: + * + * E By default G29 will engage the Z probe, test the bed, then disengage. + * Include "E" to engage/disengage the Z probe for each sample. + * There's no extra effect if you have a fixed Z probe. + */ +G29_TYPE GcodeSuite::G29() { + + reset_stepper_timeout(); + + const bool seenQ = EITHER(DEBUG_LEVELING_FEATURE, PROBE_MANUALLY) && parser.seen('Q'); + + // G29 Q is also available if debugging + #if ENABLED(DEBUG_LEVELING_FEATURE) + const uint8_t old_debug_flags = marlin_debug_flags; + if (seenQ) marlin_debug_flags |= MARLIN_DEBUG_LEVELING; + DEBUG_SECTION(log_G29, "G29", DEBUGGING(LEVELING)); + if (DEBUGGING(LEVELING)) log_machine_info(); + marlin_debug_flags = old_debug_flags; + if (DISABLED(PROBE_MANUALLY) && seenQ) G29_RETURN(false); + #endif + + const bool seenA = TERN0(PROBE_MANUALLY, parser.seen('A')), + no_action = seenA || seenQ, + faux = ENABLED(DEBUG_LEVELING_FEATURE) && DISABLED(PROBE_MANUALLY) ? parser.boolval('C') : no_action; + + if (!no_action && planner.leveling_active && parser.boolval('O')) { // Auto-level only if needed + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> Auto-level not needed, skip"); + G29_RETURN(false); + } + + // Send 'N' to force homing before G29 (internal only) + if (parser.seen('N')) + process_subcommands_now_P(TERN(G28_L0_ENSURES_LEVELING_OFF, PSTR("G28L0"), G28_STR)); + + // Don't allow auto-leveling without homing first + if (homing_needed_error()) G29_RETURN(false); + + // Define local vars 'static' for manual probing, 'auto' otherwise + #define ABL_VAR TERN_(PROBE_MANUALLY, static) + + ABL_VAR int verbose_level; + ABL_VAR xy_pos_t probePos; + ABL_VAR float measured_z; + ABL_VAR bool dryrun, abl_should_enable; + + #if EITHER(PROBE_MANUALLY, AUTO_BED_LEVELING_LINEAR) + ABL_VAR int abl_probe_index; + #endif + + #if ABL_GRID + + #if ENABLED(PROBE_MANUALLY) + ABL_VAR xy_int8_t meshCount; + #endif + + ABL_VAR xy_pos_t probe_position_lf, probe_position_rb; + ABL_VAR xy_float_t gridSpacing = { 0, 0 }; + + #if ENABLED(AUTO_BED_LEVELING_LINEAR) + ABL_VAR bool do_topography_map; + ABL_VAR xy_uint8_t abl_grid_points; + #else // Bilinear + constexpr xy_uint8_t abl_grid_points = { GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y }; + #endif + + #if ENABLED(AUTO_BED_LEVELING_LINEAR) + ABL_VAR int abl_points; + #else + int constexpr abl_points = GRID_MAX_POINTS; + #endif + + #if ENABLED(AUTO_BED_LEVELING_BILINEAR) + + ABL_VAR float zoffset; + + #elif ENABLED(AUTO_BED_LEVELING_LINEAR) + + ABL_VAR int indexIntoAB[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y]; + + ABL_VAR float eqnAMatrix[(GRID_MAX_POINTS) * 3], // "A" matrix of the linear system of equations + eqnBVector[GRID_MAX_POINTS], // "B" vector of Z points + mean; + #endif + + #elif ENABLED(AUTO_BED_LEVELING_3POINT) + + #if ENABLED(PROBE_MANUALLY) + int constexpr abl_points = 3; // used to show total points + #endif + + vector_3 points[3]; + probe.get_three_points(points); + + #endif // AUTO_BED_LEVELING_3POINT + + #if ENABLED(AUTO_BED_LEVELING_LINEAR) + struct linear_fit_data lsf_results; + #endif + + /** + * On the initial G29 fetch command parameters. + */ + if (!g29_in_progress) { + + TERN_(HAS_MULTI_HOTEND, if (active_extruder) tool_change(0)); + + #if EITHER(PROBE_MANUALLY, AUTO_BED_LEVELING_LINEAR) + abl_probe_index = -1; + #endif + + abl_should_enable = planner.leveling_active; + + #if ENABLED(AUTO_BED_LEVELING_BILINEAR) + + const bool seen_w = parser.seen('W'); + if (seen_w) { + if (!leveling_is_valid()) { + SERIAL_ERROR_MSG("No bilinear grid"); + G29_RETURN(false); + } + + const float rz = parser.seenval('Z') ? RAW_Z_POSITION(parser.value_linear_units()) : current_position.z; + if (!WITHIN(rz, -10, 10)) { + SERIAL_ERROR_MSG("Bad Z value"); + G29_RETURN(false); + } + + const float rx = RAW_X_POSITION(parser.linearval('X', NAN)), + ry = RAW_Y_POSITION(parser.linearval('Y', NAN)); + int8_t i = parser.byteval('I', -1), j = parser.byteval('J', -1); + + if (!isnan(rx) && !isnan(ry)) { + // Get nearest i / j from rx / ry + i = (rx - bilinear_start.x + 0.5 * gridSpacing.x) / gridSpacing.x; + j = (ry - bilinear_start.y + 0.5 * gridSpacing.y) / gridSpacing.y; + LIMIT(i, 0, GRID_MAX_POINTS_X - 1); + LIMIT(j, 0, GRID_MAX_POINTS_Y - 1); + } + if (WITHIN(i, 0, GRID_MAX_POINTS_X - 1) && WITHIN(j, 0, GRID_MAX_POINTS_Y)) { + set_bed_leveling_enabled(false); + z_values[i][j] = rz; + TERN_(ABL_BILINEAR_SUBDIVISION, bed_level_virt_interpolate()); + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(i, j, rz)); + set_bed_leveling_enabled(abl_should_enable); + if (abl_should_enable) report_current_position(); + } + G29_RETURN(false); + } // parser.seen('W') + + #else + + constexpr bool seen_w = false; + + #endif + + // Jettison bed leveling data + if (!seen_w && parser.seen('J')) { + reset_bed_level(); + G29_RETURN(false); + } + + verbose_level = parser.intval('V'); + if (!WITHIN(verbose_level, 0, 4)) { + SERIAL_ECHOLNPGM("?(V)erbose level implausible (0-4)."); + G29_RETURN(false); + } + + dryrun = parser.boolval('D') || TERN0(PROBE_MANUALLY, no_action); + + #if ENABLED(AUTO_BED_LEVELING_LINEAR) + + incremental_LSF_reset(&lsf_results); + + do_topography_map = verbose_level > 2 || parser.boolval('T'); + + // X and Y specify points in each direction, overriding the default + // These values may be saved with the completed mesh + abl_grid_points.set( + parser.byteval('X', GRID_MAX_POINTS_X), + parser.byteval('Y', GRID_MAX_POINTS_Y) + ); + if (parser.seenval('P')) abl_grid_points.x = abl_grid_points.y = parser.value_int(); + + if (!WITHIN(abl_grid_points.x, 2, GRID_MAX_POINTS_X)) { + SERIAL_ECHOLNPGM("?Probe points (X) implausible (2-" STRINGIFY(GRID_MAX_POINTS_X) ")."); + G29_RETURN(false); + } + if (!WITHIN(abl_grid_points.y, 2, GRID_MAX_POINTS_Y)) { + SERIAL_ECHOLNPGM("?Probe points (Y) implausible (2-" STRINGIFY(GRID_MAX_POINTS_Y) ")."); + G29_RETURN(false); + } + + abl_points = abl_grid_points.x * abl_grid_points.y; + mean = 0; + + #elif ENABLED(AUTO_BED_LEVELING_BILINEAR) + + zoffset = parser.linearval('Z'); + + #endif + + #if ABL_GRID + + xy_probe_feedrate_mm_s = MMM_TO_MMS(parser.linearval('S', XY_PROBE_SPEED)); + + const float x_min = probe.min_x(), x_max = probe.max_x(), + y_min = probe.min_y(), y_max = probe.max_y(); + + if (parser.seen('H')) { + const int16_t size = (int16_t)parser.value_linear_units(); + probe_position_lf.set(_MAX((X_CENTER) - size / 2, x_min), _MAX((Y_CENTER) - size / 2, y_min)); + probe_position_rb.set(_MIN(probe_position_lf.x + size, x_max), _MIN(probe_position_lf.y + size, y_max)); + } + else { + probe_position_lf.set(parser.linearval('L', x_min), parser.linearval('F', y_min)); + probe_position_rb.set(parser.linearval('R', x_max), parser.linearval('B', y_max)); + } + + if (!probe.good_bounds(probe_position_lf, probe_position_rb)) { + if (DEBUGGING(LEVELING)) { + DEBUG_ECHOLNPAIR("G29 L", probe_position_lf.x, " R", probe_position_rb.x, + " F", probe_position_lf.y, " B", probe_position_rb.y); + } + SERIAL_ECHOLNPGM("? (L,R,F,B) out of bounds."); + G29_RETURN(false); + } + + // Probe at the points of a lattice grid + gridSpacing.set((probe_position_rb.x - probe_position_lf.x) / (abl_grid_points.x - 1), + (probe_position_rb.y - probe_position_lf.y) / (abl_grid_points.y - 1)); + + #endif // ABL_GRID + + if (verbose_level > 0) { + SERIAL_ECHOPGM("G29 Auto Bed Leveling"); + if (dryrun) SERIAL_ECHOPGM(" (DRYRUN)"); + SERIAL_EOL(); + } + + planner.synchronize(); + + #if ENABLED(AUTO_BED_LEVELING_3POINT) + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> 3-point Leveling"); + points[0].z = points[1].z = points[2].z = 0; // Probe at 3 arbitrary points + #endif + + #if BOTH(AUTO_BED_LEVELING_BILINEAR, EXTENSIBLE_UI) + ExtUI::onMeshLevelingStart(); + #endif + + if (!faux) { + remember_feedrate_scaling_off(); + + #if ENABLED(PREHEAT_BEFORE_LEVELING) + if (!dryrun) probe.preheat_for_probing(LEVELING_NOZZLE_TEMP, LEVELING_BED_TEMP); + #endif + } + + // Disable auto bed leveling during G29. + // Be formal so G29 can be done successively without G28. + if (!no_action) set_bed_leveling_enabled(false); + + // Deploy certain probes before starting probing + #if HAS_BED_PROBE + if (ENABLED(BLTOUCH)) + do_z_clearance(Z_CLEARANCE_DEPLOY_PROBE); + else if (probe.deploy()) { + set_bed_leveling_enabled(abl_should_enable); + G29_RETURN(false); + } + #endif + + #if ENABLED(AUTO_BED_LEVELING_BILINEAR) + if (TERN1(PROBE_MANUALLY, !no_action) + && (gridSpacing != bilinear_grid_spacing || probe_position_lf != bilinear_start) + ) { + // Reset grid to 0.0 or "not probed". (Also disables ABL) + reset_bed_level(); + + // Initialize a grid with the given dimensions + bilinear_grid_spacing = gridSpacing; + bilinear_start = probe_position_lf; + + // Can't re-enable (on error) until the new grid is written + abl_should_enable = false; + } + #endif // AUTO_BED_LEVELING_BILINEAR + + } // !g29_in_progress + + #if ENABLED(PROBE_MANUALLY) + + // For manual probing, get the next index to probe now. + // On the first probe this will be incremented to 0. + if (!no_action) { + ++abl_probe_index; + g29_in_progress = true; + } + + // Abort current G29 procedure, go back to idle state + if (seenA && g29_in_progress) { + SERIAL_ECHOLNPGM("Manual G29 aborted"); + SET_SOFT_ENDSTOP_LOOSE(false); + set_bed_leveling_enabled(abl_should_enable); + g29_in_progress = false; + TERN_(LCD_BED_LEVELING, ui.wait_for_move = false); + } + + // Query G29 status + if (verbose_level || seenQ) { + SERIAL_ECHOPGM("Manual G29 "); + if (g29_in_progress) { + SERIAL_ECHOPAIR("point ", _MIN(abl_probe_index + 1, abl_points)); + SERIAL_ECHOLNPAIR(" of ", abl_points); + } + else + SERIAL_ECHOLNPGM("idle"); + } + + if (no_action) G29_RETURN(false); + + if (abl_probe_index == 0) { + // For the initial G29 S2 save software endstop state + SET_SOFT_ENDSTOP_LOOSE(true); + // Move close to the bed before the first point + do_blocking_move_to_z(0); + } + else { + + #if EITHER(AUTO_BED_LEVELING_LINEAR, AUTO_BED_LEVELING_3POINT) + const uint16_t index = abl_probe_index - 1; + #endif + + // For G29 after adjusting Z. + // Save the previous Z before going to the next point + measured_z = current_position.z; + + #if ENABLED(AUTO_BED_LEVELING_LINEAR) + + mean += measured_z; + eqnBVector[index] = measured_z; + eqnAMatrix[index + 0 * abl_points] = probePos.x; + eqnAMatrix[index + 1 * abl_points] = probePos.y; + eqnAMatrix[index + 2 * abl_points] = 1; + + incremental_LSF(&lsf_results, probePos, measured_z); + + #elif ENABLED(AUTO_BED_LEVELING_3POINT) + + points[index].z = measured_z; + + #elif ENABLED(AUTO_BED_LEVELING_BILINEAR) + + const float newz = measured_z + zoffset; + z_values[meshCount.x][meshCount.y] = newz; + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(meshCount, newz)); + + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR_P(PSTR("Save X"), meshCount.x, SP_Y_STR, meshCount.y, SP_Z_STR, measured_z + zoffset); + + #endif + } + + // + // If there's another point to sample, move there with optional lift. + // + + #if ABL_GRID + + // Skip any unreachable points + while (abl_probe_index < abl_points) { + + // Set meshCount.x, meshCount.y based on abl_probe_index, with zig-zag + PR_OUTER_VAR = abl_probe_index / PR_INNER_END; + PR_INNER_VAR = abl_probe_index - (PR_OUTER_VAR * PR_INNER_END); + + // Probe in reverse order for every other row/column + const bool zig = (PR_OUTER_VAR & 1); // != ((PR_OUTER_END) & 1); + if (zig) PR_INNER_VAR = (PR_INNER_END - 1) - PR_INNER_VAR; + + probePos = probe_position_lf + gridSpacing * meshCount.asFloat(); + + TERN_(AUTO_BED_LEVELING_LINEAR, indexIntoAB[meshCount.x][meshCount.y] = abl_probe_index); + + // Keep looping till a reachable point is found + if (position_is_reachable(probePos)) break; + ++abl_probe_index; + } + + // Is there a next point to move to? + if (abl_probe_index < abl_points) { + _manual_goto_xy(probePos); // Can be used here too! + // Disable software endstops to allow manual adjustment + // If G29 is not completed, they will not be re-enabled + SET_SOFT_ENDSTOP_LOOSE(true); + G29_RETURN(false); + } + else { + // Leveling done! Fall through to G29 finishing code below + SERIAL_ECHOLNPGM("Grid probing done."); + // Re-enable software endstops, if needed + SET_SOFT_ENDSTOP_LOOSE(false); + } + + #elif ENABLED(AUTO_BED_LEVELING_3POINT) + + // Probe at 3 arbitrary points + if (abl_probe_index < abl_points) { + probePos = points[abl_probe_index]; + _manual_goto_xy(probePos); + // Disable software endstops to allow manual adjustment + // If G29 is not completed, they will not be re-enabled + SET_SOFT_ENDSTOP_LOOSE(true); + G29_RETURN(false); + } + else { + + SERIAL_ECHOLNPGM("3-point probing done."); + + // Re-enable software endstops, if needed + SET_SOFT_ENDSTOP_LOOSE(false); + + if (!dryrun) { + vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal(); + if (planeNormal.z < 0) planeNormal *= -1; + planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal); + + // Can't re-enable (on error) until the new grid is written + abl_should_enable = false; + } + + } + + #endif // AUTO_BED_LEVELING_3POINT + + #else // !PROBE_MANUALLY + { + const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE; + + measured_z = 0; + + #if ABL_GRID + + bool zig = PR_OUTER_END & 1; // Always end at RIGHT and BACK_PROBE_BED_POSITION + + measured_z = 0; + + xy_int8_t meshCount; + + // Outer loop is X with PROBE_Y_FIRST enabled + // Outer loop is Y with PROBE_Y_FIRST disabled + for (PR_OUTER_VAR = 0; PR_OUTER_VAR < PR_OUTER_END && !isnan(measured_z); PR_OUTER_VAR++) { + + int8_t inStart, inStop, inInc; + + if (zig) { // Zig away from origin + inStart = 0; // Left or front + inStop = PR_INNER_END; // Right or back + inInc = 1; // Zig right + } + else { // Zag towards origin + inStart = PR_INNER_END - 1; // Right or back + inStop = -1; // Left or front + inInc = -1; // Zag left + } + + zig ^= true; // zag + + // An index to print current state + uint8_t pt_index = (PR_OUTER_VAR) * (PR_INNER_END) + 1; + + // Inner loop is Y with PROBE_Y_FIRST enabled + // Inner loop is X with PROBE_Y_FIRST disabled + for (PR_INNER_VAR = inStart; PR_INNER_VAR != inStop; pt_index++, PR_INNER_VAR += inInc) { + + probePos = probe_position_lf + gridSpacing * meshCount.asFloat(); + + TERN_(AUTO_BED_LEVELING_LINEAR, indexIntoAB[meshCount.x][meshCount.y] = ++abl_probe_index); // 0... + + // Avoid probing outside the round or hexagonal area + if (TERN0(IS_KINEMATIC, !probe.can_reach(probePos))) continue; + + if (verbose_level) SERIAL_ECHOLNPAIR("Probing mesh point ", int(pt_index), "/", abl_points, "."); + TERN_(HAS_DISPLAY, ui.status_printf_P(0, PSTR(S_FMT " %i/%i"), GET_TEXT(MSG_PROBING_MESH), int(pt_index), int(abl_points))); + + measured_z = faux ? 0.001f * random(-100, 101) : probe.probe_at_point(probePos, raise_after, verbose_level); + + if (isnan(measured_z)) { + set_bed_leveling_enabled(abl_should_enable); + break; // Breaks out of both loops + } + + #if ENABLED(PROBE_TEMP_COMPENSATION) + temp_comp.compensate_measurement(TSI_BED, thermalManager.degBed(), measured_z); + temp_comp.compensate_measurement(TSI_PROBE, thermalManager.degProbe(), measured_z); + TERN_(USE_TEMP_EXT_COMPENSATION, temp_comp.compensate_measurement(TSI_EXT, thermalManager.degHotend(), measured_z)); + #endif + + #if ENABLED(AUTO_BED_LEVELING_LINEAR) + + mean += measured_z; + eqnBVector[abl_probe_index] = measured_z; + eqnAMatrix[abl_probe_index + 0 * abl_points] = probePos.x; + eqnAMatrix[abl_probe_index + 1 * abl_points] = probePos.y; + eqnAMatrix[abl_probe_index + 2 * abl_points] = 1; + + incremental_LSF(&lsf_results, probePos, measured_z); + + #elif ENABLED(AUTO_BED_LEVELING_BILINEAR) + + const float z = measured_z + zoffset; + z_values[meshCount.x][meshCount.y] = z; + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(meshCount, z)); + + #endif + + abl_should_enable = false; + idle_no_sleep(); + + } // inner + } // outer + + #elif ENABLED(AUTO_BED_LEVELING_3POINT) + + // Probe at 3 arbitrary points + + LOOP_L_N(i, 3) { + if (verbose_level) SERIAL_ECHOLNPAIR("Probing point ", int(i + 1), "/3."); + TERN_(HAS_DISPLAY, ui.status_printf_P(0, PSTR(S_FMT " %i/3"), GET_TEXT(MSG_PROBING_MESH), int(i + 1))); + + // Retain the last probe position + probePos = points[i]; + measured_z = faux ? 0.001 * random(-100, 101) : probe.probe_at_point(probePos, raise_after, verbose_level); + if (isnan(measured_z)) { + set_bed_leveling_enabled(abl_should_enable); + break; + } + points[i].z = measured_z; + } + + if (!dryrun && !isnan(measured_z)) { + vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal(); + if (planeNormal.z < 0) planeNormal *= -1; + planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal); + + // Can't re-enable (on error) until the new grid is written + abl_should_enable = false; + } + + #endif // AUTO_BED_LEVELING_3POINT + + TERN_(HAS_DISPLAY, ui.reset_status()); + + // Stow the probe. No raise for FIX_MOUNTED_PROBE. + if (probe.stow()) { + set_bed_leveling_enabled(abl_should_enable); + measured_z = NAN; + } + } + #endif // !PROBE_MANUALLY + + // + // G29 Finishing Code + // + // Unless this is a dry run, auto bed leveling will + // definitely be enabled after this point. + // + // If code above wants to continue leveling, it should + // return or loop before this point. + // + + if (DEBUGGING(LEVELING)) DEBUG_POS("> probing complete", current_position); + + #if ENABLED(PROBE_MANUALLY) + g29_in_progress = false; + TERN_(LCD_BED_LEVELING, ui.wait_for_move = false); + #endif + + // Calculate leveling, print reports, correct the position + if (!isnan(measured_z)) { + #if ENABLED(AUTO_BED_LEVELING_BILINEAR) + + if (!dryrun) extrapolate_unprobed_bed_level(); + print_bilinear_leveling_grid(); + + refresh_bed_level(); + + TERN_(ABL_BILINEAR_SUBDIVISION, print_bilinear_leveling_grid_virt()); + + #elif ENABLED(AUTO_BED_LEVELING_LINEAR) + + // For LINEAR leveling calculate matrix, print reports, correct the position + + /** + * solve the plane equation ax + by + d = z + * A is the matrix with rows [x y 1] for all the probed points + * B is the vector of the Z positions + * the normal vector to the plane is formed by the coefficients of the + * plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0 + * so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z + */ + struct { float a, b, d; } plane_equation_coefficients; + + finish_incremental_LSF(&lsf_results); + plane_equation_coefficients.a = -lsf_results.A; // We should be able to eliminate the '-' on these three lines and down below + plane_equation_coefficients.b = -lsf_results.B; // but that is not yet tested. + plane_equation_coefficients.d = -lsf_results.D; + + mean /= abl_points; + + if (verbose_level) { + SERIAL_ECHOPAIR_F("Eqn coefficients: a: ", plane_equation_coefficients.a, 8); + SERIAL_ECHOPAIR_F(" b: ", plane_equation_coefficients.b, 8); + SERIAL_ECHOPAIR_F(" d: ", plane_equation_coefficients.d, 8); + if (verbose_level > 2) + SERIAL_ECHOPAIR_F("\nMean of sampled points: ", mean, 8); + SERIAL_EOL(); + } + + // Create the matrix but don't correct the position yet + if (!dryrun) + planner.bed_level_matrix = matrix_3x3::create_look_at( + vector_3(-plane_equation_coefficients.a, -plane_equation_coefficients.b, 1) // We can eliminate the '-' here and up above + ); + + // Show the Topography map if enabled + if (do_topography_map) { + + float min_diff = 999; + + auto print_topo_map = [&](PGM_P const title, const bool get_min) { + serialprintPGM(title); + for (int8_t yy = abl_grid_points.y - 1; yy >= 0; yy--) { + LOOP_L_N(xx, abl_grid_points.x) { + const int ind = indexIntoAB[xx][yy]; + xyz_float_t tmp = { eqnAMatrix[ind + 0 * abl_points], + eqnAMatrix[ind + 1 * abl_points], 0 }; + apply_rotation_xyz(planner.bed_level_matrix, tmp); + if (get_min) NOMORE(min_diff, eqnBVector[ind] - tmp.z); + const float subval = get_min ? mean : tmp.z + min_diff, + diff = eqnBVector[ind] - subval; + SERIAL_CHAR(' '); if (diff >= 0.0) SERIAL_CHAR('+'); // Include + for column alignment + SERIAL_ECHO_F(diff, 5); + } // xx + SERIAL_EOL(); + } // yy + SERIAL_EOL(); + }; + + print_topo_map(PSTR("\nBed Height Topography:\n" + " +--- BACK --+\n" + " | |\n" + " L | (+) | R\n" + " E | | I\n" + " F | (-) N (+) | G\n" + " T | | H\n" + " | (-) | T\n" + " | |\n" + " O-- FRONT --+\n" + " (0,0)\n"), true); + if (verbose_level > 3) + print_topo_map(PSTR("\nCorrected Bed Height vs. Bed Topology:\n"), false); + + } //do_topography_map + + #endif // AUTO_BED_LEVELING_LINEAR + + #if ABL_PLANAR + + // For LINEAR and 3POINT leveling correct the current position + + if (verbose_level > 0) + planner.bed_level_matrix.debug(PSTR("\n\nBed Level Correction Matrix:")); + + if (!dryrun) { + // + // Correct the current XYZ position based on the tilted plane. + // + + if (DEBUGGING(LEVELING)) DEBUG_POS("G29 uncorrected XYZ", current_position); + + xyze_pos_t converted = current_position; + planner.force_unapply_leveling(converted); // use conversion machinery + + // Use the last measured distance to the bed, if possible + if ( NEAR(current_position.x, probePos.x - probe.offset_xy.x) + && NEAR(current_position.y, probePos.y - probe.offset_xy.y) + ) { + const float simple_z = current_position.z - measured_z; + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Probed Z", simple_z, " Matrix Z", converted.z, " Discrepancy ", simple_z - converted.z); + converted.z = simple_z; + } + + // The rotated XY and corrected Z are now current_position + current_position = converted; + + if (DEBUGGING(LEVELING)) DEBUG_POS("G29 corrected XYZ", current_position); + } + + #elif ENABLED(AUTO_BED_LEVELING_BILINEAR) + + if (!dryrun) { + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("G29 uncorrected Z:", current_position.z); + + // Unapply the offset because it is going to be immediately applied + // and cause compensation movement in Z + const float fade_scaling_factor = TERN(ENABLE_LEVELING_FADE_HEIGHT, planner.fade_scaling_factor_for_z(current_position.z), 1); + current_position.z -= fade_scaling_factor * bilinear_z_offset(current_position); + + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR(" corrected Z:", current_position.z); + } + + #endif // ABL_PLANAR + + // Auto Bed Leveling is complete! Enable if possible. + planner.leveling_active = dryrun ? abl_should_enable : true; + } // !isnan(measured_z) + + // Restore state after probing + if (!faux) restore_feedrate_and_scaling(); + + // Sync the planner from the current_position + if (planner.leveling_active) sync_plan_position(); + + #if HAS_BED_PROBE + probe.move_z_after_probing(); + #endif + + #ifdef Z_PROBE_END_SCRIPT + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Z Probe End Script: ", Z_PROBE_END_SCRIPT); + planner.synchronize(); + process_subcommands_now_P(PSTR(Z_PROBE_END_SCRIPT)); + #endif + + #if ENABLED(DWIN_CREALITY_LCD) + DWIN_CompletedLeveling(); + #endif + + report_current_position(); + + G29_RETURN(isnan(measured_z)); +} + +#endif // HAS_ABL_NOT_UBL |