marlin/Marlin/src/gcode/bedlevel/abl/G29.cpp

/**
 * Marlin 3D Printer Firmware
 * Copyright (C) 2016 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 <http://www.gnu.org/licenses/>.
 *
 */

/**
 * G29.cpp - Auto Bed Leveling
 */

#include "../../../inc/MarlinConfig.h"

#if OLDSCHOOL_ABL

#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(LCD_BED_LEVELING) && ENABLED(PROBE_MANUALLY)
  #include "../../../lcd/ultralcd.h"
#endif

#if ENABLED(AUTO_BED_LEVELING_LINEAR)
  #include "../../../libs/least_squares_fit.h"
#endif

#if ABL_GRID
  #if ENABLED(PROBE_Y_FIRST)
    #define PR_OUTER_VAR xCount
    #define PR_OUTER_END abl_grid_points_x
    #define PR_INNER_VAR yCount
    #define PR_INNER_END abl_grid_points_y
  #else
    #define PR_OUTER_VAR yCount
    #define PR_OUTER_END abl_grid_points_y
    #define PR_INNER_VAR xCount
    #define PR_INNER_END abl_grid_points_x
  #endif
#endif

/**
 * 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
 *
 *  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)
 *
 *  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.
 *
 */
void GcodeSuite::G29() {

  #if ENABLED(DEBUG_LEVELING_FEATURE) || ENABLED(PROBE_MANUALLY)
    const bool seenQ = parser.seen('Q');
  #else
    constexpr bool seenQ = false;
  #endif

  // 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 |= DEBUG_LEVELING;
    if (DEBUGGING(LEVELING)) {
      DEBUG_POS(">>> G29", current_position);
      log_machine_info();
    }
    marlin_debug_flags = old_debug_flags;
    #if DISABLED(PROBE_MANUALLY)
      if (seenQ) return;
    #endif
  #endif

  #if ENABLED(PROBE_MANUALLY)
    const bool seenA = parser.seen('A');
  #else
    constexpr bool seenA = false;
  #endif

  const bool  no_action = seenA || seenQ,
              faux =
                #if ENABLED(DEBUG_LEVELING_FEATURE) && DISABLED(PROBE_MANUALLY)
                  parser.boolval('C')
                #else
                  no_action
                #endif
              ;

  // Don't allow auto-leveling without homing first
  if (axis_unhomed_error()) return;

  // Define local vars 'static' for manual probing, 'auto' otherwise
  #if ENABLED(PROBE_MANUALLY)
    #define ABL_VAR static
  #else
    #define ABL_VAR
  #endif

  ABL_VAR int verbose_level;
  ABL_VAR float xProbe, yProbe, measured_z;
  ABL_VAR bool dryrun, abl_should_enable;

  #if ENABLED(PROBE_MANUALLY) || ENABLED(AUTO_BED_LEVELING_LINEAR)
    ABL_VAR int abl_probe_index;
  #endif

  #if HAS_SOFTWARE_ENDSTOPS && ENABLED(PROBE_MANUALLY)
    ABL_VAR bool enable_soft_endstops = true;
  #endif

  #if ABL_GRID

    #if ENABLED(PROBE_MANUALLY)
      ABL_VAR uint8_t PR_OUTER_VAR;
      ABL_VAR  int8_t PR_INNER_VAR;
    #endif

    ABL_VAR int left_probe_bed_position, right_probe_bed_position, front_probe_bed_position, back_probe_bed_position;
    ABL_VAR float xGridSpacing = 0, yGridSpacing = 0;

    #if ENABLED(AUTO_BED_LEVELING_LINEAR)
      ABL_VAR uint8_t abl_grid_points_x = GRID_MAX_POINTS_X,
                      abl_grid_points_y = GRID_MAX_POINTS_Y;
      ABL_VAR bool do_topography_map;
    #else // Bilinear
      uint8_t constexpr abl_grid_points_x = GRID_MAX_POINTS_X,
                        abl_grid_points_y = GRID_MAX_POINTS_Y;
    #endif

    #if ENABLED(AUTO_BED_LEVELING_LINEAR)
      ABL_VAR int abl_points;
    #elif ENABLED(PROBE_MANUALLY) // Bilinear
      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

    // Probe at 3 arbitrary points
    ABL_VAR vector_3 points[3] = {
      vector_3(PROBE_PT_1_X, PROBE_PT_1_Y, 0),
      vector_3(PROBE_PT_2_X, PROBE_PT_2_Y, 0),
      vector_3(PROBE_PT_3_X, PROBE_PT_3_Y, 0)
    };

  #endif // AUTO_BED_LEVELING_3POINT

  #if ENABLED(AUTO_BED_LEVELING_LINEAR)
    struct linear_fit_data lsf_results;
    incremental_LSF_reset(&lsf_results);
  #endif

  /**
   * On the initial G29 fetch command parameters.
   */
  if (!g29_in_progress) {

    #if ENABLED(PROBE_MANUALLY) || ENABLED(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_START();
          SERIAL_ERRORLNPGM("No bilinear grid");
          return;
        }

        const float rz = parser.seenval('Z') ? RAW_Z_POSITION(parser.value_linear_units()) : current_position[Z_AXIS];
        if (!WITHIN(rz, -10, 10)) {
          SERIAL_ERROR_START();
          SERIAL_ERRORLNPGM("Bad Z value");
          return;
        }

        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_AXIS] + 0.5 * xGridSpacing) / xGridSpacing;
          j = (ry - bilinear_start[Y_AXIS] + 0.5 * yGridSpacing) / yGridSpacing;
          i = constrain(i, 0, GRID_MAX_POINTS_X - 1);
          j = constrain(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;
          #if ENABLED(ABL_BILINEAR_SUBDIVISION)
            bed_level_virt_interpolate();
          #endif
          set_bed_leveling_enabled(abl_should_enable);
          if (abl_should_enable) report_current_position();
        }
        return;
      } // parser.seen('W')

    #else

      constexpr bool seen_w = false;

    #endif

    // Jettison bed leveling data
    if (!seen_w && parser.seen('J')) {
      reset_bed_level();
      return;
    }

    verbose_level = parser.intval('V');
    if (!WITHIN(verbose_level, 0, 4)) {
      SERIAL_PROTOCOLLNPGM("?(V)erbose level is implausible (0-4).");
      return;
    }

    dryrun = parser.boolval('D')
      #if ENABLED(PROBE_MANUALLY)
        || no_action
      #endif
    ;

    #if ENABLED(AUTO_BED_LEVELING_LINEAR)

      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_x = parser.intval('X', GRID_MAX_POINTS_X);
      abl_grid_points_y = parser.intval('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_PROTOCOLLNPGM("?Probe points (X) is implausible (2-" STRINGIFY(GRID_MAX_POINTS_X) ").");
        return;
      }
      if (!WITHIN(abl_grid_points_y, 2, GRID_MAX_POINTS_Y)) {
        SERIAL_PROTOCOLLNPGM("?Probe points (Y) is implausible (2-" STRINGIFY(GRID_MAX_POINTS_Y) ").");
        return;
      }

      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));

      left_probe_bed_position  = parser.seenval('L') ? (int)RAW_X_POSITION(parser.value_linear_units()) : LEFT_PROBE_BED_POSITION;
      right_probe_bed_position = parser.seenval('R') ? (int)RAW_X_POSITION(parser.value_linear_units()) : RIGHT_PROBE_BED_POSITION;
      front_probe_bed_position = parser.seenval('F') ? (int)RAW_Y_POSITION(parser.value_linear_units()) : FRONT_PROBE_BED_POSITION;
      back_probe_bed_position  = parser.seenval('B') ? (int)RAW_Y_POSITION(parser.value_linear_units()) : BACK_PROBE_BED_POSITION;

      if ( !position_is_reachable_by_probe(left_probe_bed_position, front_probe_bed_position)
        || !position_is_reachable_by_probe(right_probe_bed_position, back_probe_bed_position)) {
        SERIAL_PROTOCOLLNPGM("? (L,R,F,B) out of bounds.");
        return;
      }

      // probe at the points of a lattice grid
      xGridSpacing = (right_probe_bed_position - left_probe_bed_position) / (abl_grid_points_x - 1);
      yGridSpacing = (back_probe_bed_position - front_probe_bed_position) / (abl_grid_points_y - 1);

    #endif // ABL_GRID

    if (verbose_level > 0) {
      SERIAL_PROTOCOLPGM("G29 Auto Bed Leveling");
      if (dryrun) SERIAL_PROTOCOLPGM(" (DRYRUN)");
      SERIAL_EOL();
    }

    stepper.synchronize();

    // Disable auto bed leveling during G29.
    // Be formal so G29 can be done successively without G28.
    if (!no_action) set_bed_leveling_enabled(false);

    #if HAS_BED_PROBE
      // Deploy the probe. Probe will raise if needed.
      if (DEPLOY_PROBE()) {
        set_bed_leveling_enabled(abl_should_enable);
        return;
      }
    #endif

    if (!faux) setup_for_endstop_or_probe_move();

    #if ENABLED(AUTO_BED_LEVELING_BILINEAR)

      #if ENABLED(PROBE_MANUALLY)
        if (!no_action)
      #endif
      if ( xGridSpacing != bilinear_grid_spacing[X_AXIS]
        || yGridSpacing != bilinear_grid_spacing[Y_AXIS]
        || left_probe_bed_position != bilinear_start[X_AXIS]
        || front_probe_bed_position != bilinear_start[Y_AXIS]
      ) {
        // Reset grid to 0.0 or "not probed". (Also disables ABL)
        reset_bed_level();

        // Initialize a grid with the given dimensions
        bilinear_grid_spacing[X_AXIS] = xGridSpacing;
        bilinear_grid_spacing[Y_AXIS] = yGridSpacing;
        bilinear_start[X_AXIS] = left_probe_bed_position;
        bilinear_start[Y_AXIS] = front_probe_bed_position;

        // Can't re-enable (on error) until the new grid is written
        abl_should_enable = false;
      }

    #endif // AUTO_BED_LEVELING_BILINEAR

    #if ENABLED(AUTO_BED_LEVELING_3POINT)

      #if ENABLED(DEBUG_LEVELING_FEATURE)
        if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("> 3-point Leveling");
      #endif

      // Probe at 3 arbitrary points
      points[0].z = points[1].z = points[2].z = 0;

    #endif // AUTO_BED_LEVELING_3POINT

  } // !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_PROTOCOLLNPGM("Manual G29 aborted");
      #if HAS_SOFTWARE_ENDSTOPS
        soft_endstops_enabled = enable_soft_endstops;
      #endif
      set_bed_leveling_enabled(abl_should_enable);
      g29_in_progress = false;
      #if ENABLED(LCD_BED_LEVELING)
        lcd_wait_for_move = false;
      #endif
    }

    // Query G29 status
    if (verbose_level || seenQ) {
      SERIAL_PROTOCOLPGM("Manual G29 ");
      if (g29_in_progress) {
        SERIAL_PROTOCOLPAIR("point ", min(abl_probe_index + 1, abl_points));
        SERIAL_PROTOCOLLNPAIR(" of ", abl_points);
      }
      else
        SERIAL_PROTOCOLLNPGM("idle");
    }

    if (no_action) return;

    if (abl_probe_index == 0) {
      // For the initial G29 S2 save software endstop state
      #if HAS_SOFTWARE_ENDSTOPS
        enable_soft_endstops = soft_endstops_enabled;
      #endif
      // Move close to the bed before the first point
      do_blocking_move_to_z(Z_MIN_POS);
    }
    else {

      #if ENABLED(AUTO_BED_LEVELING_LINEAR) || ENABLED(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_AXIS];

      #if ENABLED(AUTO_BED_LEVELING_LINEAR)

        mean += measured_z;
        eqnBVector[index] = measured_z;
        eqnAMatrix[index + 0 * abl_points] = xProbe;
        eqnAMatrix[index + 1 * abl_points] = yProbe;
        eqnAMatrix[index + 2 * abl_points] = 1;

        incremental_LSF(&lsf_results, xProbe, yProbe, measured_z);

      #elif ENABLED(AUTO_BED_LEVELING_3POINT)

        points[index].z = measured_z;

      #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)

        z_values[xCount][yCount] = measured_z + zoffset;

        #if ENABLED(DEBUG_LEVELING_FEATURE)
          if (DEBUGGING(LEVELING)) {
            SERIAL_PROTOCOLPAIR("Save X", xCount);
            SERIAL_PROTOCOLPAIR(" Y", yCount);
            SERIAL_PROTOCOLLNPAIR(" Z", measured_z + zoffset);
          }
        #endif

      #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 xCount, yCount 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
        bool zig = (PR_OUTER_VAR & 1); // != ((PR_OUTER_END) & 1);

        if (zig) PR_INNER_VAR = (PR_INNER_END - 1) - PR_INNER_VAR;

        const float xBase = xCount * xGridSpacing + left_probe_bed_position,
                    yBase = yCount * yGridSpacing + front_probe_bed_position;

        xProbe = FLOOR(xBase + (xBase < 0 ? 0 : 0.5));
        yProbe = FLOOR(yBase + (yBase < 0 ? 0 : 0.5));

        #if ENABLED(AUTO_BED_LEVELING_LINEAR)
          indexIntoAB[xCount][yCount] = abl_probe_index;
        #endif

        // Keep looping till a reachable point is found
        if (position_is_reachable(xProbe, yProbe)) break;
        ++abl_probe_index;
      }

      // Is there a next point to move to?
      if (abl_probe_index < abl_points) {
        _manual_goto_xy(xProbe, yProbe); // Can be used here too!
        #if HAS_SOFTWARE_ENDSTOPS
          // Disable software endstops to allow manual adjustment
          // If G29 is not completed, they will not be re-enabled
          soft_endstops_enabled = false;
        #endif
        return;
      }
      else {

        // Leveling done! Fall through to G29 finishing code below

        SERIAL_PROTOCOLLNPGM("Grid probing done.");

        // Re-enable software endstops, if needed
        #if HAS_SOFTWARE_ENDSTOPS
          soft_endstops_enabled = enable_soft_endstops;
        #endif
      }

    #elif ENABLED(AUTO_BED_LEVELING_3POINT)

      // Probe at 3 arbitrary points
      if (abl_probe_index < abl_points) {
        xProbe = points[abl_probe_index].x;
        yProbe = points[abl_probe_index].y;
        _manual_goto_xy(xProbe, yProbe);
        #if HAS_SOFTWARE_ENDSTOPS
          // Disable software endstops to allow manual adjustment
          // If G29 is not completed, they will not be re-enabled
          soft_endstops_enabled = false;
        #endif
        return;
      }
      else {

        SERIAL_PROTOCOLLNPGM("3-point probing done.");

        // Re-enable software endstops, if needed
        #if HAS_SOFTWARE_ENDSTOPS
          soft_endstops_enabled = enable_soft_endstops;
        #endif

        if (!dryrun) {
          vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal();
          if (planeNormal.z < 0) {
            planeNormal.x *= -1;
            planeNormal.y *= -1;
            planeNormal.z *= -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;

      // Outer loop is Y with PROBE_Y_FIRST disabled
      for (uint8_t PR_OUTER_VAR = 0; PR_OUTER_VAR < PR_OUTER_END && !isnan(measured_z); PR_OUTER_VAR++) {

        int8_t inStart, inStop, inInc;

        if (zig) { // away from origin
          inStart = 0;
          inStop = PR_INNER_END;
          inInc = 1;
        }
        else {     // towards origin
          inStart = PR_INNER_END - 1;
          inStop = -1;
          inInc = -1;
        }

        zig ^= true; // zag

        // Inner loop is Y with PROBE_Y_FIRST enabled
        for (int8_t PR_INNER_VAR = inStart; PR_INNER_VAR != inStop; PR_INNER_VAR += inInc) {

          float xBase = left_probe_bed_position + xGridSpacing * xCount,
                yBase = front_probe_bed_position + yGridSpacing * yCount;

          xProbe = FLOOR(xBase + (xBase < 0 ? 0 : 0.5));
          yProbe = FLOOR(yBase + (yBase < 0 ? 0 : 0.5));

          #if ENABLED(AUTO_BED_LEVELING_LINEAR)
            indexIntoAB[xCount][yCount] = ++abl_probe_index; // 0...
          #endif

          #if IS_KINEMATIC
            // Avoid probing outside the round or hexagonal area
            if (!position_is_reachable_by_probe(xProbe, yProbe)) continue;
          #endif

          measured_z = faux ? 0.001 * random(-100, 101) : probe_pt(xProbe, yProbe, raise_after, verbose_level);

          if (isnan(measured_z)) {
            set_bed_leveling_enabled(abl_should_enable);
            break;
          }

          #if ENABLED(AUTO_BED_LEVELING_LINEAR)

            mean += measured_z;
            eqnBVector[abl_probe_index] = measured_z;
            eqnAMatrix[abl_probe_index + 0 * abl_points] = xProbe;
            eqnAMatrix[abl_probe_index + 1 * abl_points] = yProbe;
            eqnAMatrix[abl_probe_index + 2 * abl_points] = 1;

            incremental_LSF(&lsf_results, xProbe, yProbe, measured_z);

          #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)

            z_values[xCount][yCount] = measured_z + zoffset;

          #endif

          abl_should_enable = false;
          idle();

        } // inner
      } // outer

    #elif ENABLED(AUTO_BED_LEVELING_3POINT)

      // Probe at 3 arbitrary points

      for (uint8_t i = 0; i < 3; ++i) {
        // Retain the last probe position
        xProbe = points[i].x;
        yProbe = points[i].y;
        measured_z = faux ? 0.001 * random(-100, 101) : probe_pt(xProbe, yProbe, 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.x *= -1;
          planeNormal.y *= -1;
          planeNormal.z *= -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

    // Stow the probe. No raise for FIX_MOUNTED_PROBE.
    if (STOW_PROBE()) {
      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 ENABLED(DEBUG_LEVELING_FEATURE)
    if (DEBUGGING(LEVELING)) DEBUG_POS("> probing complete", current_position);
  #endif

  #if ENABLED(PROBE_MANUALLY)
    g29_in_progress = false;
    #if ENABLED(LCD_BED_LEVELING)
      lcd_wait_for_move = false;
    #endif
  #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();

      #if ENABLED(ABL_BILINEAR_SUBDIVISION)
        print_bilinear_leveling_grid_virt();
      #endif

    #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
       */
      float plane_equation_coefficients[3];

      finish_incremental_LSF(&lsf_results);
      plane_equation_coefficients[0] = -lsf_results.A;  // We should be able to eliminate the '-' on these three lines and down below
      plane_equation_coefficients[1] = -lsf_results.B;  // but that is not yet tested.
      plane_equation_coefficients[2] = -lsf_results.D;

      mean /= abl_points;

      if (verbose_level) {
        SERIAL_PROTOCOLPGM("Eqn coefficients: a: ");
        SERIAL_PROTOCOL_F(plane_equation_coefficients[0], 8);
        SERIAL_PROTOCOLPGM(" b: ");
        SERIAL_PROTOCOL_F(plane_equation_coefficients[1], 8);
        SERIAL_PROTOCOLPGM(" d: ");
        SERIAL_PROTOCOL_F(plane_equation_coefficients[2], 8);
        SERIAL_EOL();
        if (verbose_level > 2) {
          SERIAL_PROTOCOLPGM("Mean of sampled points: ");
          SERIAL_PROTOCOL_F(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[0], -plane_equation_coefficients[1], 1)    // We can eliminate the '-' here and up above
        );

      // Show the Topography map if enabled
      if (do_topography_map) {

        SERIAL_PROTOCOLLNPGM("\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)");

        float min_diff = 999;

        for (int8_t yy = abl_grid_points_y - 1; yy >= 0; yy--) {
          for (uint8_t xx = 0; xx < abl_grid_points_x; xx++) {
            int ind = indexIntoAB[xx][yy];
            float diff = eqnBVector[ind] - mean,
                  x_tmp = eqnAMatrix[ind + 0 * abl_points],
                  y_tmp = eqnAMatrix[ind + 1 * abl_points],
                  z_tmp = 0;

            apply_rotation_xyz(planner.bed_level_matrix, x_tmp, y_tmp, z_tmp);

            NOMORE(min_diff, eqnBVector[ind] - z_tmp);

            if (diff >= 0.0)
              SERIAL_PROTOCOLPGM(" +");   // Include + for column alignment
            else
              SERIAL_PROTOCOLCHAR(' ');
            SERIAL_PROTOCOL_F(diff, 5);
          } // xx
          SERIAL_EOL();
        } // yy
        SERIAL_EOL();

        if (verbose_level > 3) {
          SERIAL_PROTOCOLLNPGM("\nCorrected Bed Height vs. Bed Topology:");

          for (int8_t yy = abl_grid_points_y - 1; yy >= 0; yy--) {
            for (uint8_t xx = 0; xx < abl_grid_points_x; xx++) {
              int ind = indexIntoAB[xx][yy];
              float x_tmp = eqnAMatrix[ind + 0 * abl_points],
                    y_tmp = eqnAMatrix[ind + 1 * abl_points],
                    z_tmp = 0;

              apply_rotation_xyz(planner.bed_level_matrix, x_tmp, y_tmp, z_tmp);

              float diff = eqnBVector[ind] - z_tmp - min_diff;
              if (diff >= 0.0)
                SERIAL_PROTOCOLPGM(" +");
              // Include + for column alignment
              else
                SERIAL_PROTOCOLCHAR(' ');
              SERIAL_PROTOCOL_F(diff, 5);
            } // xx
            SERIAL_EOL();
          } // yy
          SERIAL_EOL();
        }
      } //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 ENABLED(DEBUG_LEVELING_FEATURE)
          if (DEBUGGING(LEVELING)) DEBUG_POS("G29 uncorrected XYZ", current_position);
        #endif

        float converted[XYZ];
        COPY(converted, current_position);

        planner.leveling_active = true;
        planner.unapply_leveling(converted); // use conversion machinery
        planner.leveling_active = false;

        // Use the last measured distance to the bed, if possible
        if ( NEAR(current_position[X_AXIS], xProbe - (X_PROBE_OFFSET_FROM_EXTRUDER))
          && NEAR(current_position[Y_AXIS], yProbe - (Y_PROBE_OFFSET_FROM_EXTRUDER))
        ) {
          const float simple_z = current_position[Z_AXIS] - measured_z;
          #if ENABLED(DEBUG_LEVELING_FEATURE)
            if (DEBUGGING(LEVELING)) {
              SERIAL_ECHOPAIR("Z from Probe:", simple_z);
              SERIAL_ECHOPAIR("  Matrix:", converted[Z_AXIS]);
              SERIAL_ECHOLNPAIR("  Discrepancy:", simple_z - converted[Z_AXIS]);
            }
          #endif
          converted[Z_AXIS] = simple_z;
        }

        // The rotated XY and corrected Z are now current_position
        COPY(current_position, converted);

        #if ENABLED(DEBUG_LEVELING_FEATURE)
          if (DEBUGGING(LEVELING)) DEBUG_POS("G29 corrected XYZ", current_position);
        #endif
      }

    #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)

      if (!dryrun) {
        #if ENABLED(DEBUG_LEVELING_FEATURE)
          if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPAIR("G29 uncorrected Z:", current_position[Z_AXIS]);
        #endif

        // Unapply the offset because it is going to be immediately applied
        // and cause compensation movement in Z
        current_position[Z_AXIS] -= bilinear_z_offset(current_position);

        #if ENABLED(DEBUG_LEVELING_FEATURE)
          if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPAIR(" corrected Z:", current_position[Z_AXIS]);
        #endif
      }

    #endif // ABL_PLANAR

    #ifdef Z_PROBE_END_SCRIPT
      #if ENABLED(DEBUG_LEVELING_FEATURE)
        if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPAIR("Z Probe End Script: ", Z_PROBE_END_SCRIPT);
      #endif
      enqueue_and_echo_commands_P(PSTR(Z_PROBE_END_SCRIPT));
      stepper.synchronize();
    #endif

    // 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) clean_up_after_endstop_or_probe_move();

  #if ENABLED(DEBUG_LEVELING_FEATURE)
    if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("<<< G29");
  #endif

  KEEPALIVE_STATE(IN_HANDLER);

  if (planner.leveling_active)
    SYNC_PLAN_POSITION_KINEMATIC();

  #if HAS_BED_PROBE && Z_AFTER_PROBING
    move_z_after_probing();
  #endif

  report_current_position();
}

#endif // OLDSCHOOL_ABL