Initial commit

10 files changed, 2690 insertions, 0 deletions

diff --git a/Marlin/src/gcode/bedlevel/G26.cpp b/Marlin/src/gcode/bedlevel/G26.cpp
new file mode 100644
index 0000000..5a79aaa
--- /dev/null
+++ b/Marlin/src/gcode/bedlevel/G26.cpp
@@ -0,0 +1,872 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ *   G26 Mesh Validation Tool
+ *
+ *   G26 is a Mesh Validation Tool intended to provide support for the Marlin Unified Bed Leveling System.
+ *   In order to fully utilize and benefit from the Marlin Unified Bed Leveling System an accurate Mesh must
+ *   be defined. G29 is designed to allow the user to quickly validate the correctness of her Mesh. It will
+ *   first heat the bed and nozzle. It will then print lines and circles along the Mesh Cell boundaries and
+ *   the intersections of those lines (respectively).
+ *
+ *   This action allows the user to immediately see where the Mesh is properly defined and where it needs to
+ *   be edited. The command will generate the Mesh lines closest to the nozzle's starting position. Alternatively
+ *   the user can specify the X and Y position of interest with command parameters. This allows the user to
+ *   focus on a particular area of the Mesh where attention is needed.
+ *
+ *   B #  Bed         Set the Bed Temperature. If not specified, a default of 60 C. will be assumed.
+ *
+ *   C    Current     When searching for Mesh Intersection points to draw, use the current nozzle location
+ *                    as the base for any distance comparison.
+ *
+ *   D    Disable     Disable the Unified Bed Leveling System. In the normal case the user is invoking this
+ *                    command to see how well a Mesh as been adjusted to match a print surface. In order to do
+ *                    this the Unified Bed Leveling System is turned on by the G26 command. The D parameter
+ *                    alters the command's normal behavior and disables the Unified Bed Leveling System even if
+ *                    it is on.
+ *
+ *   H #  Hotend      Set the Nozzle Temperature. If not specified, a default of 205 C. will be assumed.
+ *
+ *   I #  Preset      Heat the Nozzle and Bed based on a Material Preset (if material presets are defined).
+ *
+ *   F #  Filament    Used to specify the diameter of the filament being used. If not specified
+ *                    1.75mm filament is assumed. If you are not getting acceptable results by using the
+ *                    'correct' numbers, you can scale this number up or down a little bit to change the amount
+ *                    of filament that is being extruded during the printing of the various lines on the bed.
+ *
+ *   K    Keep-On     Keep the heaters turned on at the end of the command.
+ *
+ *   L #  Layer       Layer height. (Height of nozzle above bed)  If not specified .20mm will be used.
+ *
+ *   O #  Ooooze      How much your nozzle will Ooooze filament while getting in position to print. This
+ *                    is over kill, but using this parameter will let you get the very first 'circle' perfect
+ *                    so you have a trophy to peel off of the bed and hang up to show how perfectly you have your
+ *                    Mesh calibrated. If not specified, a filament length of .3mm is assumed.
+ *
+ *   P #  Prime       Prime the nozzle with specified length of filament. If this parameter is not
+ *                    given, no prime action will take place. If the parameter specifies an amount, that much
+ *                    will be purged before continuing. If no amount is specified the command will start
+ *                    purging filament until the user provides an LCD Click and then it will continue with
+ *                    printing the Mesh. You can carefully remove the spent filament with a needle nose
+ *                    pliers while holding the LCD Click wheel in a depressed state. If you do not have
+ *                    an LCD, you must specify a value if you use P.
+ *
+ *   Q #  Multiplier  Retraction Multiplier. Normally not needed. Retraction defaults to 1.0mm and
+ *                    un-retraction is at 1.2mm   These numbers will be scaled by the specified amount
+ *
+ *   R #  Repeat      Prints the number of patterns given as a parameter, starting at the current location.
+ *                    If a parameter isn't given, every point will be printed unless G26 is interrupted.
+ *                    This works the same way that the UBL G29 P4 R parameter works.
+ *
+ *                    NOTE:  If you do not have an LCD, you -must- specify R. This is to ensure that you are
+ *                    aware that there's some risk associated with printing without the ability to abort in
+ *                    cases where mesh point Z value may be inaccurate. As above, if you do not include a
+ *                    parameter, every point will be printed.
+ *
+ *   S #  Nozzle      Used to control the size of nozzle diameter. If not specified, a .4mm nozzle is assumed.
+ *
+ *   U #  Random      Randomize the order that the circles are drawn on the bed. The search for the closest
+ *                    un-drawn circle is still done. But the distance to the location for each circle has a
+ *                    random number of the specified size added to it. Specifying S50 will give an interesting
+ *                    deviation from the normal behavior on a 10 x 10 Mesh.
+ *
+ *   X #  X Coord.    Specify the starting location of the drawing activity.
+ *
+ *   Y #  Y Coord.    Specify the starting location of the drawing activity.
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(G26_MESH_VALIDATION)
+
+#define G26_OK false
+#define G26_ERR true
+
+#include "../../gcode/gcode.h"
+#include "../../feature/bedlevel/bedlevel.h"
+
+#include "../../MarlinCore.h"
+#include "../../module/planner.h"
+#include "../../module/stepper.h"
+#include "../../module/motion.h"
+#include "../../module/tool_change.h"
+#include "../../module/temperature.h"
+#include "../../lcd/marlinui.h"
+
+#define EXTRUSION_MULTIPLIER 1.0
+#define PRIME_LENGTH 10.0
+#define OOZE_AMOUNT 0.3
+
+#define INTERSECTION_CIRCLE_RADIUS 5
+#define CROSSHAIRS_SIZE 3
+
+#ifndef G26_RETRACT_MULTIPLIER
+  #define G26_RETRACT_MULTIPLIER 1.0 // x 1mm
+#endif
+
+#ifndef G26_XY_FEEDRATE
+  #define G26_XY_FEEDRATE (PLANNER_XY_FEEDRATE() / 3.0)
+#endif
+
+#ifndef G26_XY_FEEDRATE_TRAVEL
+  #define G26_XY_FEEDRATE_TRAVEL (PLANNER_XY_FEEDRATE() / 1.5)
+#endif
+
+#if CROSSHAIRS_SIZE >= INTERSECTION_CIRCLE_RADIUS
+  #error "CROSSHAIRS_SIZE must be less than INTERSECTION_CIRCLE_RADIUS."
+#endif
+
+#define G26_OK false
+#define G26_ERR true
+
+#if ENABLED(ARC_SUPPORT)
+  void plan_arc(const xyze_pos_t&, const ab_float_t&, const bool, const uint8_t);
+#endif
+
+constexpr float g26_e_axis_feedrate = 0.025;
+
+static MeshFlags circle_flags, horizontal_mesh_line_flags, vertical_mesh_line_flags;
+float g26_random_deviation = 0.0;
+
+static bool g26_retracted = false; // Track the retracted state of the nozzle so mismatched
+                                   // retracts/recovers won't result in a bad state.
+
+float g26_extrusion_multiplier,
+      g26_retraction_multiplier,
+      g26_layer_height,
+      g26_prime_length;
+
+xy_pos_t g26_xy_pos; // = { 0, 0 }
+
+int16_t g26_bed_temp,
+        g26_hotend_temp;
+
+int8_t g26_prime_flag;
+
+#if HAS_LCD_MENU
+
+  /**
+   * If the LCD is clicked, cancel, wait for release, return true
+   */
+  bool user_canceled() {
+    if (!ui.button_pressed()) return false; // Return if the button isn't pressed
+    ui.set_status_P(GET_TEXT(MSG_G26_CANCELED), 99);
+    TERN_(HAS_LCD_MENU, ui.quick_feedback());
+    ui.wait_for_release();
+    return true;
+  }
+
+#endif
+
+mesh_index_pair find_closest_circle_to_print(const xy_pos_t &pos) {
+  float closest = 99999.99;
+  mesh_index_pair out_point;
+
+  out_point.pos = -1;
+
+  GRID_LOOP(i, j) {
+    if (!circle_flags.marked(i, j)) {
+      // We found a circle that needs to be printed
+      const xy_pos_t m = { _GET_MESH_X(i), _GET_MESH_Y(j) };
+
+      // Get the distance to this intersection
+      float f = (pos - m).magnitude();
+
+      // It is possible that we are being called with the values
+      // to let us find the closest circle to the start position.
+      // But if this is not the case, add a small weighting to the
+      // distance calculation to help it choose a better place to continue.
+      f += (g26_xy_pos - m).magnitude() / 15.0f;
+
+      // Add the specified amount of Random Noise to our search
+      if (g26_random_deviation > 1.0) f += random(0.0, g26_random_deviation);
+
+      if (f < closest) {
+        closest = f;          // Found a closer un-printed location
+        out_point.pos.set(i, j);  // Save its data
+        out_point.distance = closest;
+      }
+    }
+  }
+  circle_flags.mark(out_point); // Mark this location as done.
+  return out_point;
+}
+
+void move_to(const float &rx, const float &ry, const float &z, const float &e_delta) {
+  static float last_z = -999.99;
+
+  const xy_pos_t dest = { rx, ry };
+
+  const bool has_xy_component = dest != current_position; // Check if X or Y is involved in the movement.
+  const bool has_e_component = e_delta != 0.0;
+
+  destination = current_position;
+
+  if (z != last_z) {
+    last_z = destination.z = z;
+    const feedRate_t fr_mm_s = planner.settings.max_feedrate_mm_s[Z_AXIS] * 0.5f; // Use half of the Z_AXIS max feed rate
+    prepare_internal_move_to_destination(fr_mm_s);
+  }
+
+  // If X or Y in combination with E is involved do a 'normal' move.
+  // If X or Y with no E is involved do a 'fast' move
+  // Otherwise retract/recover/hop.
+  destination = dest;
+  destination.e += e_delta;
+  const feedRate_t fr_mm_s = has_xy_component
+    ? (has_e_component ? feedRate_t(G26_XY_FEEDRATE) : feedRate_t(G26_XY_FEEDRATE_TRAVEL))
+    : planner.settings.max_feedrate_mm_s[E_AXIS] * 0.666f;
+  prepare_internal_move_to_destination(fr_mm_s);
+}
+
+FORCE_INLINE void move_to(const xyz_pos_t &where, const float &de) { move_to(where.x, where.y, where.z, de); }
+
+void retract_filament(const xyz_pos_t &where) {
+  if (!g26_retracted) { // Only retract if we are not already retracted!
+    g26_retracted = true;
+    move_to(where, -1.0f * g26_retraction_multiplier);
+  }
+}
+
+// TODO: Parameterize the Z lift with a define
+void retract_lift_move(const xyz_pos_t &s) {
+  retract_filament(destination);
+  move_to(current_position.x, current_position.y, current_position.z + 0.5f, 0.0);  // Z lift to minimize scraping
+  move_to(s.x, s.y, s.z + 0.5f, 0.0);  // Get to the starting point with no extrusion while lifted
+}
+
+void recover_filament(const xyz_pos_t &where) {
+  if (g26_retracted) { // Only un-retract if we are retracted.
+    move_to(where, 1.2f * g26_retraction_multiplier);
+    g26_retracted = false;
+  }
+}
+
+/**
+ * print_line_from_here_to_there() takes two cartesian coordinates and draws a line from one
+ * to the other. But there are really three sets of coordinates involved. The first coordinate
+ * is the present location of the nozzle. We don't necessarily want to print from this location.
+ * We first need to move the nozzle to the start of line segment where we want to print. Once
+ * there, we can use the two coordinates supplied to draw the line.
+ *
+ * Note:  Although we assume the first set of coordinates is the start of the line and the second
+ * set of coordinates is the end of the line, it does not always work out that way. This function
+ * optimizes the movement to minimize the travel distance before it can start printing. This saves
+ * a lot of time and eliminates a lot of nonsensical movement of the nozzle. However, it does
+ * cause a lot of very little short retracement of th nozzle when it draws the very first line
+ * segment of a 'circle'. The time this requires is very short and is easily saved by the other
+ * cases where the optimization comes into play.
+ */
+void print_line_from_here_to_there(const xyz_pos_t &s, const xyz_pos_t &e) {
+
+  // Distances to the start / end of the line
+  xy_float_t svec = current_position - s, evec = current_position - e;
+
+  const float dist_start = HYPOT2(svec.x, svec.y),
+              dist_end = HYPOT2(evec.x, evec.y),
+              line_length = HYPOT(e.x - s.x, e.y - s.y);
+
+  // If the end point of the line is closer to the nozzle, flip the direction,
+  // moving from the end to the start. On very small lines the optimization isn't worth it.
+  if (dist_end < dist_start && (INTERSECTION_CIRCLE_RADIUS) < ABS(line_length))
+    return print_line_from_here_to_there(e, s);
+
+  // Decide whether to retract & lift
+  if (dist_start > 2.0) retract_lift_move(s);
+
+  move_to(s, 0.0); // Get to the starting point with no extrusion / un-Z lift
+
+  const float e_pos_delta = line_length * g26_e_axis_feedrate * g26_extrusion_multiplier;
+
+  recover_filament(destination);
+  move_to(e, e_pos_delta);  // Get to the ending point with an appropriate amount of extrusion
+}
+
+inline bool look_for_lines_to_connect() {
+  xyz_pos_t s, e;
+  s.z = e.z = g26_layer_height;
+
+  GRID_LOOP(i, j) {
+
+    if (TERN0(HAS_LCD_MENU, user_canceled())) return true;
+
+    if (i < (GRID_MAX_POINTS_X)) {  // Can't connect to anything farther to the right than GRID_MAX_POINTS_X.
+                                    // Already a half circle at the edge of the bed.
+
+      if (circle_flags.marked(i, j) && circle_flags.marked(i + 1, j)) {   // Test whether a leftward line can be done
+        if (!horizontal_mesh_line_flags.marked(i, j)) {
+          // Two circles need a horizontal line to connect them
+          s.x = _GET_MESH_X(  i  ) + (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)); // right edge
+          e.x = _GET_MESH_X(i + 1) - (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)); // left edge
+
+          LIMIT(s.x, X_MIN_POS + 1, X_MAX_POS - 1);
+          s.y = e.y = constrain(_GET_MESH_Y(j), Y_MIN_POS + 1, Y_MAX_POS - 1);
+          LIMIT(e.x, X_MIN_POS + 1, X_MAX_POS - 1);
+
+          if (position_is_reachable(s.x, s.y) && position_is_reachable(e.x, e.y))
+            print_line_from_here_to_there(s, e);
+
+          horizontal_mesh_line_flags.mark(i, j); // Mark done, even if skipped
+        }
+      }
+
+      if (j < (GRID_MAX_POINTS_Y)) {  // Can't connect to anything further back than GRID_MAX_POINTS_Y.
+                                      // Already a half circle at the edge of the bed.
+
+        if (circle_flags.marked(i, j) && circle_flags.marked(i, j + 1)) {   // Test whether a downward line can be done
+          if (!vertical_mesh_line_flags.marked(i, j)) {
+            // Two circles that need a vertical line to connect them
+            s.y = _GET_MESH_Y(  j  ) + (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)); // top edge
+            e.y = _GET_MESH_Y(j + 1) - (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)); // bottom edge
+
+            s.x = e.x = constrain(_GET_MESH_X(i), X_MIN_POS + 1, X_MAX_POS - 1);
+            LIMIT(s.y, Y_MIN_POS + 1, Y_MAX_POS - 1);
+            LIMIT(e.y, Y_MIN_POS + 1, Y_MAX_POS - 1);
+
+            if (position_is_reachable(s.x, s.y) && position_is_reachable(e.x, e.y))
+              print_line_from_here_to_there(s, e);
+
+            vertical_mesh_line_flags.mark(i, j); // Mark done, even if skipped
+          }
+        }
+      }
+    }
+  }
+  return false;
+}
+
+/**
+ * Turn on the bed and nozzle heat and
+ * wait for them to get up to temperature.
+ */
+inline bool turn_on_heaters() {
+
+  SERIAL_ECHOLNPGM("Waiting for heatup.");
+
+  #if HAS_HEATED_BED
+
+    if (g26_bed_temp > 25) {
+      #if HAS_WIRED_LCD
+        ui.set_status_P(GET_TEXT(MSG_G26_HEATING_BED), 99);
+        ui.quick_feedback();
+        TERN_(HAS_LCD_MENU, ui.capture());
+      #endif
+      thermalManager.setTargetBed(g26_bed_temp);
+
+      // Wait for the temperature to stabilize
+      if (!thermalManager.wait_for_bed(true
+          #if G26_CLICK_CAN_CANCEL
+            , true
+          #endif
+        )
+      ) return G26_ERR;
+    }
+
+  #endif // HAS_HEATED_BED
+
+  // Start heating the active nozzle
+  #if HAS_WIRED_LCD
+    ui.set_status_P(GET_TEXT(MSG_G26_HEATING_NOZZLE), 99);
+    ui.quick_feedback();
+  #endif
+  thermalManager.setTargetHotend(g26_hotend_temp, active_extruder);
+
+  // Wait for the temperature to stabilize
+  if (!thermalManager.wait_for_hotend(active_extruder, true
+    #if G26_CLICK_CAN_CANCEL
+      , true
+    #endif
+  )) return G26_ERR;
+
+  #if HAS_WIRED_LCD
+    ui.reset_status();
+    ui.quick_feedback();
+  #endif
+
+  return G26_OK;
+}
+
+/**
+ * Prime the nozzle if needed. Return true on error.
+ */
+inline bool prime_nozzle() {
+
+  const feedRate_t fr_slow_e = planner.settings.max_feedrate_mm_s[E_AXIS] / 15.0f;
+  #if HAS_LCD_MENU && !HAS_TOUCH_BUTTONS // ui.button_pressed issue with touchscreen
+    #if ENABLED(PREVENT_LENGTHY_EXTRUDE)
+      float Total_Prime = 0.0;
+    #endif
+
+    if (g26_prime_flag == -1) {  // The user wants to control how much filament gets purged
+      ui.capture();
+      ui.set_status_P(GET_TEXT(MSG_G26_MANUAL_PRIME), 99);
+      ui.chirp();
+
+      destination = current_position;
+
+      recover_filament(destination); // Make sure G26 doesn't think the filament is retracted().
+
+      while (!ui.button_pressed()) {
+        ui.chirp();
+        destination.e += 0.25;
+        #if ENABLED(PREVENT_LENGTHY_EXTRUDE)
+          Total_Prime += 0.25;
+          if (Total_Prime >= EXTRUDE_MAXLENGTH) {
+            ui.release();
+            return G26_ERR;
+          }
+        #endif
+        prepare_internal_move_to_destination(fr_slow_e);
+        destination = current_position;
+        planner.synchronize();    // Without this synchronize, the purge is more consistent,
+                                  // but because the planner has a buffer, we won't be able
+                                  // to stop as quickly. So we put up with the less smooth
+                                  // action to give the user a more responsive 'Stop'.
+      }
+
+      ui.wait_for_release();
+
+      ui.set_status_P(GET_TEXT(MSG_G26_PRIME_DONE), 99);
+      ui.quick_feedback();
+      ui.release();
+    }
+    else
+  #endif
+  {
+    #if HAS_WIRED_LCD
+      ui.set_status_P(GET_TEXT(MSG_G26_FIXED_LENGTH), 99);
+      ui.quick_feedback();
+    #endif
+    destination = current_position;
+    destination.e += g26_prime_length;
+    prepare_internal_move_to_destination(fr_slow_e);
+    destination.e -= g26_prime_length;
+    retract_filament(destination);
+  }
+
+  return G26_OK;
+}
+
+/**
+ * G26: Mesh Validation Pattern generation.
+ *
+ * Used to interactively edit the mesh by placing the
+ * nozzle in a problem area and doing a G29 P4 R command.
+ *
+ * Parameters:
+ *
+ *  B  Bed Temperature
+ *  C  Continue from the Closest mesh point
+ *  D  Disable leveling before starting
+ *  F  Filament diameter
+ *  H  Hotend Temperature
+ *  K  Keep heaters on when completed
+ *  L  Layer Height
+ *  O  Ooze extrusion length
+ *  P  Prime length
+ *  Q  Retraction multiplier
+ *  R  Repetitions (number of grid points)
+ *  S  Nozzle Size (diameter) in mm
+ *  T  Tool index to change to, if included
+ *  U  Random deviation (50 if no value given)
+ *  X  X position
+ *  Y  Y position
+ */
+void GcodeSuite::G26() {
+  SERIAL_ECHOLNPGM("G26 starting...");
+
+  // Don't allow Mesh Validation without homing first,
+  // or if the parameter parsing did not go OK, abort
+  if (homing_needed_error()) return;
+
+  // Change the tool first, if specified
+  if (parser.seenval('T')) tool_change(parser.value_int());
+
+  g26_extrusion_multiplier    = EXTRUSION_MULTIPLIER;
+  g26_retraction_multiplier   = G26_RETRACT_MULTIPLIER;
+  g26_layer_height            = MESH_TEST_LAYER_HEIGHT;
+  g26_prime_length            = PRIME_LENGTH;
+  g26_bed_temp                = MESH_TEST_BED_TEMP;
+  g26_hotend_temp             = MESH_TEST_HOTEND_TEMP;
+  g26_prime_flag              = 0;
+
+  float g26_nozzle            = MESH_TEST_NOZZLE_SIZE,
+        g26_filament_diameter = DEFAULT_NOMINAL_FILAMENT_DIA,
+        g26_ooze_amount       = parser.linearval('O', OOZE_AMOUNT);
+
+  bool g26_continue_with_closest = parser.boolval('C'),
+       g26_keep_heaters_on       = parser.boolval('K');
+
+  // Accept 'I' if temperature presets are defined
+  #if PREHEAT_COUNT
+    const uint8_t preset_index = parser.seenval('I') ? _MIN(parser.value_byte(), PREHEAT_COUNT - 1) + 1 : 0;
+  #endif
+
+  #if HAS_HEATED_BED
+
+    // Get a temperature from 'I' or 'B'
+    int16_t bedtemp = 0;
+
+    // Use the 'I' index if temperature presets are defined
+    #if PREHEAT_COUNT
+      if (preset_index) bedtemp = ui.material_preset[preset_index - 1].bed_temp;
+    #endif
+
+    // Look for 'B' Bed Temperature
+    if (parser.seenval('B')) bedtemp = parser.value_celsius();
+
+    if (bedtemp) {
+      if (!WITHIN(bedtemp, 40, BED_MAX_TARGET)) {
+        SERIAL_ECHOLNPAIR("?Specified bed temperature not plausible (40-", int(BED_MAX_TARGET), "C).");
+        return;
+      }
+      g26_bed_temp = bedtemp;
+    }
+
+  #endif // HAS_HEATED_BED
+
+  if (parser.seenval('L')) {
+    g26_layer_height = parser.value_linear_units();
+    if (!WITHIN(g26_layer_height, 0.0, 2.0)) {
+      SERIAL_ECHOLNPGM("?Specified layer height not plausible.");
+      return;
+    }
+  }
+
+  if (parser.seen('Q')) {
+    if (parser.has_value()) {
+      g26_retraction_multiplier = parser.value_float();
+      if (!WITHIN(g26_retraction_multiplier, 0.05, 15.0)) {
+        SERIAL_ECHOLNPGM("?Specified Retraction Multiplier not plausible.");
+        return;
+      }
+    }
+    else {
+      SERIAL_ECHOLNPGM("?Retraction Multiplier must be specified.");
+      return;
+    }
+  }
+
+  if (parser.seenval('S')) {
+    g26_nozzle = parser.value_float();
+    if (!WITHIN(g26_nozzle, 0.1, 2.0)) {
+      SERIAL_ECHOLNPGM("?Specified nozzle size not plausible.");
+      return;
+    }
+  }
+
+  if (parser.seen('P')) {
+    if (!parser.has_value()) {
+      #if HAS_LCD_MENU
+        g26_prime_flag = -1;
+      #else
+        SERIAL_ECHOLNPGM("?Prime length must be specified when not using an LCD.");
+        return;
+      #endif
+    }
+    else {
+      g26_prime_flag++;
+      g26_prime_length = parser.value_linear_units();
+      if (!WITHIN(g26_prime_length, 0.0, 25.0)) {
+        SERIAL_ECHOLNPGM("?Specified prime length not plausible.");
+        return;
+      }
+    }
+  }
+
+  if (parser.seenval('F')) {
+    g26_filament_diameter = parser.value_linear_units();
+    if (!WITHIN(g26_filament_diameter, 1.0, 4.0)) {
+      SERIAL_ECHOLNPGM("?Specified filament size not plausible.");
+      return;
+    }
+  }
+  g26_extrusion_multiplier *= sq(1.75) / sq(g26_filament_diameter); // If we aren't using 1.75mm filament, we need to
+                                                                    // scale up or down the length needed to get the
+                                                                    // same volume of filament
+
+  g26_extrusion_multiplier *= g26_filament_diameter * sq(g26_nozzle) / sq(0.3); // Scale up by nozzle size
+
+  // Get a temperature from 'I' or 'H'
+  int16_t noztemp = 0;
+
+  // Accept 'I' if temperature presets are defined
+  #if PREHEAT_COUNT
+    if (preset_index) noztemp = ui.material_preset[preset_index - 1].hotend_temp;
+  #endif
+
+  // Look for 'H' Hotend Temperature
+  if (parser.seenval('H')) noztemp = parser.value_celsius();
+
+  // If any preset or temperature was specified
+  if (noztemp) {
+    if (!WITHIN(noztemp, 165, (HEATER_0_MAXTEMP) - (HOTEND_OVERSHOOT))) {
+      SERIAL_ECHOLNPGM("?Specified nozzle temperature not plausible.");
+      return;
+    }
+    g26_hotend_temp = noztemp;
+  }
+
+  // 'U' to Randomize and optionally set circle deviation
+  if (parser.seen('U')) {
+    randomSeed(millis());
+    // This setting will persist for the next G26
+    g26_random_deviation = parser.has_value() ? parser.value_float() : 50.0;
+  }
+
+  // Get repeat from 'R', otherwise do one full circuit
+  int16_t g26_repeats;
+  #if HAS_LCD_MENU
+    g26_repeats = parser.intval('R', GRID_MAX_POINTS + 1);
+  #else
+    if (!parser.seen('R')) {
+      SERIAL_ECHOLNPGM("?(R)epeat must be specified when not using an LCD.");
+      return;
+    }
+    else
+      g26_repeats = parser.has_value() ? parser.value_int() : GRID_MAX_POINTS + 1;
+  #endif
+  if (g26_repeats < 1) {
+    SERIAL_ECHOLNPGM("?(R)epeat value not plausible; must be at least 1.");
+    return;
+  }
+
+  // Set a position with 'X' and/or 'Y'. Default: current_position
+  g26_xy_pos.set(parser.seenval('X') ? RAW_X_POSITION(parser.value_linear_units()) : current_position.x,
+                 parser.seenval('Y') ? RAW_Y_POSITION(parser.value_linear_units()) : current_position.y);
+  if (!position_is_reachable(g26_xy_pos)) {
+    SERIAL_ECHOLNPGM("?Specified X,Y coordinate out of bounds.");
+    return;
+  }
+
+  /**
+   * Wait until all parameters are verified before altering the state!
+   */
+  set_bed_leveling_enabled(!parser.seen('D'));
+
+  do_z_clearance(Z_CLEARANCE_BETWEEN_PROBES);
+
+  #if DISABLED(NO_VOLUMETRICS)
+    bool volumetric_was_enabled = parser.volumetric_enabled;
+    parser.volumetric_enabled = false;
+    planner.calculate_volumetric_multipliers();
+  #endif
+
+  if (turn_on_heaters() != G26_OK) goto LEAVE;
+
+  current_position.e = 0.0;
+  sync_plan_position_e();
+
+  if (g26_prime_flag && prime_nozzle() != G26_OK) goto LEAVE;
+
+  /**
+   *  Bed is preheated
+   *
+   *  Nozzle is at temperature
+   *
+   *  Filament is primed!
+   *
+   *  It's  "Show Time" !!!
+   */
+
+  circle_flags.reset();
+  horizontal_mesh_line_flags.reset();
+  vertical_mesh_line_flags.reset();
+
+  // Move nozzle to the specified height for the first layer
+  destination = current_position;
+  destination.z = g26_layer_height;
+  move_to(destination, 0.0);
+  move_to(destination, g26_ooze_amount);
+
+  TERN_(HAS_LCD_MENU, ui.capture());
+
+  #if DISABLED(ARC_SUPPORT)
+
+    /**
+     * Pre-generate radius offset values at 30 degree intervals to reduce CPU load.
+     */
+    #define A_INT 30
+    #define _ANGS (360 / A_INT)
+    #define A_CNT (_ANGS / 2)
+    #define _IND(A) ((A + _ANGS * 8) % _ANGS)
+    #define _COS(A) (trig_table[_IND(A) % A_CNT] * (_IND(A) >= A_CNT ? -1 : 1))
+    #define _SIN(A) (-_COS((A + A_CNT / 2) % _ANGS))
+    #if A_CNT & 1
+      #error "A_CNT must be a positive value. Please change A_INT."
+    #endif
+    float trig_table[A_CNT];
+    LOOP_L_N(i, A_CNT)
+      trig_table[i] = INTERSECTION_CIRCLE_RADIUS * cos(RADIANS(i * A_INT));
+
+  #endif // !ARC_SUPPORT
+
+  mesh_index_pair location;
+  do {
+    // Find the nearest confluence
+    location = find_closest_circle_to_print(g26_continue_with_closest ? xy_pos_t(current_position) : g26_xy_pos);
+
+    if (location.valid()) {
+      const xy_pos_t circle = _GET_MESH_POS(location.pos);
+
+      // If this mesh location is outside the printable radius, skip it.
+      if (!position_is_reachable(circle)) continue;
+
+      // Determine where to start and end the circle,
+      // which is always drawn counter-clockwise.
+      const xy_int8_t st = location;
+      const bool f = st.y == 0,
+                 r = st.x >= GRID_MAX_POINTS_X - 1,
+                 b = st.y >= GRID_MAX_POINTS_Y - 1;
+
+      #if ENABLED(ARC_SUPPORT)
+
+        #define ARC_LENGTH(quarters)  (INTERSECTION_CIRCLE_RADIUS * M_PI * (quarters) / 2)
+        #define INTERSECTION_CIRCLE_DIAM  ((INTERSECTION_CIRCLE_RADIUS) * 2)
+
+        xy_float_t e = { circle.x + INTERSECTION_CIRCLE_RADIUS, circle.y };
+        xyz_float_t s = e;
+
+        // Figure out where to start and end the arc - we always print counterclockwise
+        float arc_length = ARC_LENGTH(4);
+        if (st.x == 0) {                             // left edge
+          if (!f) { s.x = circle.x; s.y -= INTERSECTION_CIRCLE_RADIUS; }
+          if (!b) { e.x = circle.x; e.y += INTERSECTION_CIRCLE_RADIUS; }
+          arc_length = (f || b) ? ARC_LENGTH(1) : ARC_LENGTH(2);
+        }
+        else if (r) {                             // right edge
+          if (b) s.set(circle.x - (INTERSECTION_CIRCLE_RADIUS), circle.y);
+          else   s.set(circle.x, circle.y + INTERSECTION_CIRCLE_RADIUS);
+          if (f) e.set(circle.x - (INTERSECTION_CIRCLE_RADIUS), circle.y);
+          else   e.set(circle.x, circle.y - (INTERSECTION_CIRCLE_RADIUS));
+          arc_length = (f || b) ? ARC_LENGTH(1) : ARC_LENGTH(2);
+        }
+        else if (f) {
+          e.x -= INTERSECTION_CIRCLE_DIAM;
+          arc_length = ARC_LENGTH(2);
+        }
+        else if (b) {
+          s.x -= INTERSECTION_CIRCLE_DIAM;
+          arc_length = ARC_LENGTH(2);
+        }
+
+        const ab_float_t arc_offset = circle - s;
+        const xy_float_t dist = current_position - s;   // Distance from the start of the actual circle
+        const float dist_start = HYPOT2(dist.x, dist.y);
+        const xyze_pos_t endpoint = {
+          e.x, e.y, g26_layer_height,
+          current_position.e + (arc_length * g26_e_axis_feedrate * g26_extrusion_multiplier)
+        };
+
+        if (dist_start > 2.0) {
+          s.z = g26_layer_height + 0.5f;
+          retract_lift_move(s);
+        }
+
+        s.z = g26_layer_height;
+        move_to(s, 0.0);  // Get to the starting point with no extrusion / un-Z lift
+
+        recover_filament(destination);
+
+        const feedRate_t old_feedrate = feedrate_mm_s;
+        feedrate_mm_s = PLANNER_XY_FEEDRATE() * 0.1f;
+        plan_arc(endpoint, arc_offset, false, 0);  // Draw a counter-clockwise arc
+        feedrate_mm_s = old_feedrate;
+        destination = current_position;
+
+        if (TERN0(HAS_LCD_MENU, user_canceled())) goto LEAVE; // Check if the user wants to stop the Mesh Validation
+
+      #else // !ARC_SUPPORT
+
+        int8_t start_ind = -2, end_ind = 9; // Assume a full circle (from 5:00 to 5:00)
+        if (st.x == 0) {                    // Left edge? Just right half.
+          start_ind = f ? 0 : -3;           //  03:00 to 12:00 for front-left
+          end_ind = b ? 0 : 2;              //  06:00 to 03:00 for back-left
+        }
+        else if (r) {                       // Right edge? Just left half.
+          start_ind = b ? 6 : 3;            //  12:00 to 09:00 for front-right
+          end_ind = f ? 5 : 8;              //  09:00 to 06:00 for back-right
+        }
+        else if (f) {                       // Front edge? Just back half.
+          start_ind = 0;                    //  03:00
+          end_ind = 5;                      //  09:00
+        }
+        else if (b) {                       // Back edge? Just front half.
+          start_ind = 6;                    //  09:00
+          end_ind = 11;                     //  03:00
+        }
+
+        for (int8_t ind = start_ind; ind <= end_ind; ind++) {
+
+          if (TERN0(HAS_LCD_MENU, user_canceled())) goto LEAVE; // Check if the user wants to stop the Mesh Validation
+
+          xyz_float_t p = { circle.x + _COS(ind    ), circle.y + _SIN(ind    ), g26_layer_height },
+                      q = { circle.x + _COS(ind + 1), circle.y + _SIN(ind + 1), g26_layer_height };
+
+          #if IS_KINEMATIC
+            // Check to make sure this segment is entirely on the bed, skip if not.
+            if (!position_is_reachable(p) || !position_is_reachable(q)) continue;
+          #else
+            LIMIT(p.x, X_MIN_POS + 1, X_MAX_POS - 1); // Prevent hitting the endstops
+            LIMIT(p.y, Y_MIN_POS + 1, Y_MAX_POS - 1);
+            LIMIT(q.x, X_MIN_POS + 1, X_MAX_POS - 1);
+            LIMIT(q.y, Y_MIN_POS + 1, Y_MAX_POS - 1);
+          #endif
+
+          print_line_from_here_to_there(p, q);
+          SERIAL_FLUSH();   // Prevent host M105 buffer overrun.
+        }
+
+      #endif // !ARC_SUPPORT
+
+      if (look_for_lines_to_connect()) goto LEAVE;
+    }
+
+    SERIAL_FLUSH(); // Prevent host M105 buffer overrun.
+
+  } while (--g26_repeats && location.valid());
+
+  LEAVE:
+  ui.set_status_P(GET_TEXT(MSG_G26_LEAVING), -1);
+
+  retract_filament(destination);
+  destination.z = Z_CLEARANCE_BETWEEN_PROBES;
+  move_to(destination, 0);                                    // Raise the nozzle
+
+  destination = g26_xy_pos;                                   // Move back to the starting XY position
+  move_to(destination, 0);                                    // Move back to the starting position
+
+  #if DISABLED(NO_VOLUMETRICS)
+    parser.volumetric_enabled = volumetric_was_enabled;
+    planner.calculate_volumetric_multipliers();
+  #endif
+
+  TERN_(HAS_LCD_MENU, ui.release()); // Give back control of the LCD
+
+  if (!g26_keep_heaters_on) {
+    TERN_(HAS_HEATED_BED, thermalManager.setTargetBed(0));
+    thermalManager.setTargetHotend(active_extruder, 0);
+  }
+}
+
+#endif // G26_MESH_VALIDATION
diff --git a/Marlin/src/gcode/bedlevel/G35.cpp b/Marlin/src/gcode/bedlevel/G35.cpp
new file mode 100644
index 0000000..46f75f2
--- /dev/null
+++ b/Marlin/src/gcode/bedlevel/G35.cpp
@@ -0,0 +1,167 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(ASSISTED_TRAMMING)
+
+#include "../gcode.h"
+#include "../../module/planner.h"
+#include "../../module/probe.h"
+#include "../../feature/bedlevel/bedlevel.h"
+
+#if HAS_MULTI_HOTEND
+  #include "../../module/tool_change.h"
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../../core/debug_out.h"
+
+//
+// Define tramming point names.
+//
+
+#include "../../feature/tramming.h"
+
+/**
+ * G35: Read bed corners to help adjust bed screws
+ *
+ *   S<screw_thread>
+ *
+ * Screw thread: 30 - Clockwise M3
+ *               31 - Counter-Clockwise M3
+ *               40 - Clockwise M4
+ *               41 - Counter-Clockwise M4
+ *               50 - Clockwise M5
+ *               51 - Counter-Clockwise M5
+ **/
+void GcodeSuite::G35() {
+  DEBUG_SECTION(log_G35, "G35", DEBUGGING(LEVELING));
+
+  if (DEBUGGING(LEVELING)) log_machine_info();
+
+  float z_measured[G35_PROBE_COUNT] = { 0 };
+
+  const uint8_t screw_thread = parser.byteval('S', TRAMMING_SCREW_THREAD);
+  if (!WITHIN(screw_thread, 30, 51) || screw_thread % 10 > 1) {
+    SERIAL_ECHOLNPGM("?(S)crew thread must be 30, 31, 40, 41, 50, or 51.");
+    return;
+  }
+
+  // Wait for planner moves to finish!
+  planner.synchronize();
+
+  // Disable the leveling matrix before auto-aligning
+  #if HAS_LEVELING
+    TERN_(RESTORE_LEVELING_AFTER_G35, const bool leveling_was_active = planner.leveling_active);
+    set_bed_leveling_enabled(false);
+  #endif
+
+  #if ENABLED(CNC_WORKSPACE_PLANES)
+    workspace_plane = PLANE_XY;
+  #endif
+
+  // Always home with tool 0 active
+  #if HAS_MULTI_HOTEND
+    const uint8_t old_tool_index = active_extruder;
+    tool_change(0, true);
+  #endif
+
+  // Disable duplication mode on homing
+  TERN_(HAS_DUPLICATION_MODE, set_duplication_enabled(false));
+
+  // Home all before this procedure
+  home_all_axes();
+
+  bool err_break = false;
+
+  // Probe all positions
+  LOOP_L_N(i, G35_PROBE_COUNT) {
+
+    // In BLTOUCH HS mode, the probe travels in a deployed state.
+    // Users of G35 might have a badly misaligned bed, so raise Z by the
+    // length of the deployed pin (BLTOUCH stroke < 7mm)
+    do_blocking_move_to_z((Z_CLEARANCE_BETWEEN_PROBES) + TERN0(BLTOUCH_HS_MODE, 7));
+    const float z_probed_height = probe.probe_at_point(screws_tilt_adjust_pos[i], PROBE_PT_RAISE, 0, true);
+
+    if (isnan(z_probed_height)) {
+      SERIAL_ECHOPAIR("G35 failed at point ", int(i), " (");
+      SERIAL_ECHOPGM_P((char *)pgm_read_ptr(&tramming_point_name[i]));
+      SERIAL_CHAR(')');
+      SERIAL_ECHOLNPAIR_P(SP_X_STR, screws_tilt_adjust_pos[i].x, SP_Y_STR, screws_tilt_adjust_pos[i].y);
+      err_break = true;
+      break;
+    }
+
+    if (DEBUGGING(LEVELING)) {
+      DEBUG_ECHOPAIR("Probing point ", int(i), " (");
+      DEBUG_PRINT_P((char *)pgm_read_ptr(&tramming_point_name[i]));
+      DEBUG_CHAR(')');
+      DEBUG_ECHOLNPAIR_P(SP_X_STR, screws_tilt_adjust_pos[i].x, SP_Y_STR, screws_tilt_adjust_pos[i].y, SP_Z_STR, z_probed_height);
+    }
+
+    z_measured[i] = z_probed_height;
+  }
+
+  if (!err_break) {
+    const float threads_factor[] = { 0.5, 0.7, 0.8 };
+
+    // Calculate adjusts
+    LOOP_S_L_N(i, 1, G35_PROBE_COUNT) {
+      const float diff = z_measured[0] - z_measured[i],
+                  adjust = abs(diff) < 0.001f ? 0 : diff / threads_factor[(screw_thread - 30) / 10];
+
+      const int full_turns = trunc(adjust);
+      const float decimal_part = adjust - float(full_turns);
+      const int minutes = trunc(decimal_part * 60.0f);
+
+      SERIAL_ECHOPGM("Turn ");
+      SERIAL_ECHOPGM_P((char *)pgm_read_ptr(&tramming_point_name[i]));
+      SERIAL_ECHOPAIR(" ", (screw_thread & 1) == (adjust > 0) ? "CCW" : "CW", " by ", abs(full_turns), " turns");
+      if (minutes) SERIAL_ECHOPAIR(" and ", abs(minutes), " minutes");
+      if (ENABLED(REPORT_TRAMMING_MM)) SERIAL_ECHOPAIR(" (", -diff, "mm)");
+      SERIAL_EOL();
+    }
+  }
+  else
+    SERIAL_ECHOLNPGM("G35 aborted.");
+
+  // Restore the active tool after homing
+  #if HAS_MULTI_HOTEND
+    tool_change(old_tool_index, DISABLED(PARKING_EXTRUDER)); // Fetch previous toolhead if not PARKING_EXTRUDER
+  #endif
+
+  #if BOTH(HAS_LEVELING, RESTORE_LEVELING_AFTER_G35)
+    set_bed_leveling_enabled(leveling_was_active);
+  #endif
+
+  // Stow the probe, as the last call to probe.probe_at_point(...) left
+  // the probe deployed if it was successful.
+  probe.stow();
+
+  move_to_tramming_wait_pos();
+
+  // After this operation the Z position needs correction
+  set_axis_never_homed(Z_AXIS);
+}
+
+#endif // ASSISTED_TRAMMING
diff --git a/Marlin/src/gcode/bedlevel/G42.cpp b/Marlin/src/gcode/bedlevel/G42.cpp
new file mode 100644
index 0000000..a2896ed
--- /dev/null
+++ b/Marlin/src/gcode/bedlevel/G42.cpp
@@ -0,0 +1,73 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_MESH
+
+#include "../gcode.h"
+#include "../../MarlinCore.h" // for IsRunning()
+#include "../../module/motion.h"
+#include "../../module/probe.h" // for probe.offset
+#include "../../feature/bedlevel/bedlevel.h"
+
+/**
+ * G42: Move X & Y axes to mesh coordinates (I & J)
+ */
+void GcodeSuite::G42() {
+  if (MOTION_CONDITIONS) {
+    const bool hasI = parser.seenval('I');
+    const int8_t ix = hasI ? parser.value_int() : 0;
+    const bool hasJ = parser.seenval('J');
+    const int8_t iy = hasJ ? parser.value_int() : 0;
+
+    if ((hasI && !WITHIN(ix, 0, GRID_MAX_POINTS_X - 1)) || (hasJ && !WITHIN(iy, 0, GRID_MAX_POINTS_Y - 1))) {
+      SERIAL_ECHOLNPGM(STR_ERR_MESH_XY);
+      return;
+    }
+
+    // Move to current_position, as modified by I, J, P parameters
+    destination = current_position;
+
+    if (hasI) destination.x = _GET_MESH_X(ix);
+    if (hasJ) destination.y = _GET_MESH_Y(iy);
+
+    #if HAS_PROBE_XY_OFFSET
+      if (parser.boolval('P')) {
+        if (hasI) destination.x -= probe.offset_xy.x;
+        if (hasJ) destination.y -= probe.offset_xy.y;
+      }
+    #endif
+
+    const feedRate_t fval = parser.linearval('F'),
+                     fr_mm_s = MMM_TO_MMS(fval > 0 ? fval : 0.0f);
+
+    // SCARA kinematic has "safe" XY raw moves
+    #if IS_SCARA
+      prepare_internal_fast_move_to_destination(fr_mm_s);
+    #else
+      prepare_internal_move_to_destination(fr_mm_s);
+    #endif
+  }
+}
+
+#endif // HAS_MESH
diff --git a/Marlin/src/gcode/bedlevel/M420.cpp b/Marlin/src/gcode/bedlevel/M420.cpp
new file mode 100644
index 0000000..96122c1
--- /dev/null
+++ b/Marlin/src/gcode/bedlevel/M420.cpp
@@ -0,0 +1,245 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_LEVELING
+
+#include "../gcode.h"
+#include "../../feature/bedlevel/bedlevel.h"
+#include "../../module/planner.h"
+#include "../../module/probe.h"
+
+#if ENABLED(EEPROM_SETTINGS)
+  #include "../../module/settings.h"
+#endif
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../../lcd/extui/ui_api.h"
+#endif
+
+//#define M420_C_USE_MEAN
+
+/**
+ * M420: Enable/Disable Bed Leveling and/or set the Z fade height.
+ *
+ *   S[bool]   Turns leveling on or off
+ *   Z[height] Sets the Z fade height (0 or none to disable)
+ *   V[bool]   Verbose - Print the leveling grid
+ *
+ * With AUTO_BED_LEVELING_UBL only:
+ *
+ *   L[index]  Load UBL mesh from index (0 is default)
+ *   T[map]    0:Human-readable 1:CSV 2:"LCD" 4:Compact
+ *
+ * With mesh-based leveling only:
+ *
+ *   C         Center mesh on the mean of the lowest and highest
+ *
+ * With MARLIN_DEV_MODE:
+ *   S2        Create a simple random mesh and enable
+ */
+void GcodeSuite::M420() {
+  const bool seen_S = parser.seen('S'),
+             to_enable = seen_S ? parser.value_bool() : planner.leveling_active;
+
+  #if ENABLED(MARLIN_DEV_MODE)
+    if (parser.intval('S') == 2) {
+      const float x_min = probe.min_x(), x_max = probe.max_x(),
+                  y_min = probe.min_y(), y_max = probe.max_y();
+      #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+        bilinear_start.set(x_min, y_min);
+        bilinear_grid_spacing.set((x_max - x_min) / (GRID_MAX_POINTS_X - 1),
+                                  (y_max - y_min) / (GRID_MAX_POINTS_Y - 1));
+      #endif
+      GRID_LOOP(x, y) {
+        Z_VALUES(x, y) = 0.001 * random(-200, 200);
+        TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, Z_VALUES(x, y)));
+      }
+      SERIAL_ECHOPGM("Simulated " STRINGIFY(GRID_MAX_POINTS_X) "x" STRINGIFY(GRID_MAX_POINTS_Y) " mesh ");
+      SERIAL_ECHOPAIR(" (", x_min);
+      SERIAL_CHAR(','); SERIAL_ECHO(y_min);
+      SERIAL_ECHOPAIR(")-(", x_max);
+      SERIAL_CHAR(','); SERIAL_ECHO(y_max);
+      SERIAL_ECHOLNPGM(")");
+    }
+  #endif
+
+  xyz_pos_t oldpos = current_position;
+
+  // If disabling leveling do it right away
+  // (Don't disable for just M420 or M420 V)
+  if (seen_S && !to_enable) set_bed_leveling_enabled(false);
+
+  #if ENABLED(AUTO_BED_LEVELING_UBL)
+
+    // L to load a mesh from the EEPROM
+    if (parser.seen('L')) {
+
+      set_bed_leveling_enabled(false);
+
+      #if ENABLED(EEPROM_SETTINGS)
+        const int8_t storage_slot = parser.has_value() ? parser.value_int() : ubl.storage_slot;
+        const int16_t a = settings.calc_num_meshes();
+
+        if (!a) {
+          SERIAL_ECHOLNPGM("?EEPROM storage not available.");
+          return;
+        }
+
+        if (!WITHIN(storage_slot, 0, a - 1)) {
+          SERIAL_ECHOLNPGM("?Invalid storage slot.");
+          SERIAL_ECHOLNPAIR("?Use 0 to ", a - 1);
+          return;
+        }
+
+        settings.load_mesh(storage_slot);
+        ubl.storage_slot = storage_slot;
+
+      #else
+
+        SERIAL_ECHOLNPGM("?EEPROM storage not available.");
+        return;
+
+      #endif
+    }
+
+    // L or V display the map info
+    if (parser.seen("LV")) {
+      ubl.display_map(parser.byteval('T'));
+      SERIAL_ECHOPGM("Mesh is ");
+      if (!ubl.mesh_is_valid()) SERIAL_ECHOPGM("in");
+      SERIAL_ECHOLNPAIR("valid\nStorage slot: ", ubl.storage_slot);
+    }
+
+  #endif // AUTO_BED_LEVELING_UBL
+
+  const bool seenV = parser.seen('V');
+
+  #if HAS_MESH
+
+    if (leveling_is_valid()) {
+
+      // Subtract the given value or the mean from all mesh values
+      if (parser.seen('C')) {
+        const float cval = parser.value_float();
+        #if ENABLED(AUTO_BED_LEVELING_UBL)
+
+          set_bed_leveling_enabled(false);
+          ubl.adjust_mesh_to_mean(true, cval);
+
+        #else
+
+          #if ENABLED(M420_C_USE_MEAN)
+
+            // Get the sum and average of all mesh values
+            float mesh_sum = 0;
+            GRID_LOOP(x, y) mesh_sum += Z_VALUES(x, y);
+            const float zmean = mesh_sum / float(GRID_MAX_POINTS);
+
+          #else
+
+            // Find the low and high mesh values
+            float lo_val = 100, hi_val = -100;
+            GRID_LOOP(x, y) {
+              const float z = Z_VALUES(x, y);
+              NOMORE(lo_val, z);
+              NOLESS(hi_val, z);
+            }
+            // Take the mean of the lowest and highest
+            const float zmean = (lo_val + hi_val) / 2.0 + cval;
+
+          #endif
+
+          // If not very close to 0, adjust the mesh
+          if (!NEAR_ZERO(zmean)) {
+            set_bed_leveling_enabled(false);
+            // Subtract the mean from all values
+            GRID_LOOP(x, y) {
+              Z_VALUES(x, y) -= zmean;
+              TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, Z_VALUES(x, y)));
+            }
+            TERN_(ABL_BILINEAR_SUBDIVISION, bed_level_virt_interpolate());
+          }
+
+        #endif
+      }
+
+    }
+    else if (to_enable || seenV) {
+      SERIAL_ECHO_MSG("Invalid mesh.");
+      goto EXIT_M420;
+    }
+
+  #endif // HAS_MESH
+
+  // V to print the matrix or mesh
+  if (seenV) {
+    #if ABL_PLANAR
+      planner.bed_level_matrix.debug(PSTR("Bed Level Correction Matrix:"));
+    #else
+      if (leveling_is_valid()) {
+        #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+          print_bilinear_leveling_grid();
+          TERN_(ABL_BILINEAR_SUBDIVISION, print_bilinear_leveling_grid_virt());
+        #elif ENABLED(MESH_BED_LEVELING)
+          SERIAL_ECHOLNPGM("Mesh Bed Level data:");
+          mbl.report_mesh();
+        #endif
+      }
+    #endif
+  }
+
+  #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+    if (parser.seen('Z')) set_z_fade_height(parser.value_linear_units(), false);
+  #endif
+
+  // Enable leveling if specified, or if previously active
+  set_bed_leveling_enabled(to_enable);
+
+  #if HAS_MESH
+    EXIT_M420:
+  #endif
+
+  // Error if leveling failed to enable or reenable
+  if (to_enable && !planner.leveling_active)
+    SERIAL_ERROR_MSG(STR_ERR_M420_FAILED);
+
+  SERIAL_ECHO_START();
+  SERIAL_ECHOPGM("Bed Leveling ");
+  serialprintln_onoff(planner.leveling_active);
+
+  #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+    SERIAL_ECHO_START();
+    SERIAL_ECHOPGM("Fade Height ");
+    if (planner.z_fade_height > 0.0)
+      SERIAL_ECHOLN(planner.z_fade_height);
+    else
+      SERIAL_ECHOLNPGM(STR_OFF);
+  #endif
+
+  // Report change in position
+  if (oldpos != current_position)
+    report_current_position();
+}
+
+#endif // HAS_LEVELING
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
diff --git a/Marlin/src/gcode/bedlevel/abl/M421.cpp b/Marlin/src/gcode/bedlevel/abl/M421.cpp
new file mode 100644
index 0000000..182dc32
--- /dev/null
+++ b/Marlin/src/gcode/bedlevel/abl/M421.cpp
@@ -0,0 +1,74 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * M421.cpp - Auto Bed Leveling
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+
+#include "../../gcode.h"
+#include "../../../feature/bedlevel/bedlevel.h"
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../../../lcd/extui/ui_api.h"
+#endif
+
+/**
+ * M421: Set one or more Mesh Bed Leveling Z coordinates
+ *
+ * Usage:
+ *   M421 I<xindex> J<yindex> Z<linear>
+ *   M421 I<xindex> J<yindex> Q<offset>
+ *
+ *  - If I is omitted, set the entire row
+ *  - If J is omitted, set the entire column
+ *  - If both I and J are omitted, set all
+ */
+void GcodeSuite::M421() {
+  int8_t ix = parser.intval('I', -1), iy = parser.intval('J', -1);
+  const bool hasZ = parser.seenval('Z'),
+             hasQ = !hasZ && parser.seenval('Q');
+
+  if (hasZ || hasQ) {
+    if (WITHIN(ix, -1, GRID_MAX_POINTS_X - 1) && WITHIN(iy, -1, GRID_MAX_POINTS_Y - 1)) {
+      const float zval = parser.value_linear_units();
+      uint8_t sx = ix >= 0 ? ix : 0, ex = ix >= 0 ? ix : GRID_MAX_POINTS_X - 1,
+              sy = iy >= 0 ? iy : 0, ey = iy >= 0 ? iy : GRID_MAX_POINTS_Y - 1;
+      LOOP_S_LE_N(x, sx, ex) {
+        LOOP_S_LE_N(y, sy, ey) {
+          z_values[x][y] = zval + (hasQ ? z_values[x][y] : 0);
+          TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, z_values[x][y]));
+        }
+      }
+      TERN_(ABL_BILINEAR_SUBDIVISION, bed_level_virt_interpolate());
+    }
+    else
+      SERIAL_ERROR_MSG(STR_ERR_MESH_XY);
+  }
+  else
+    SERIAL_ERROR_MSG(STR_ERR_M421_PARAMETERS);
+}
+
+#endif // AUTO_BED_LEVELING_BILINEAR
diff --git a/Marlin/src/gcode/bedlevel/mbl/G29.cpp b/Marlin/src/gcode/bedlevel/mbl/G29.cpp
new file mode 100644
index 0000000..cf27c14
--- /dev/null
+++ b/Marlin/src/gcode/bedlevel/mbl/G29.cpp
@@ -0,0 +1,193 @@
+/**
+ * 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 - Mesh Bed Leveling
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(MESH_BED_LEVELING)
+
+#include "../../../feature/bedlevel/bedlevel.h"
+
+#include "../../gcode.h"
+#include "../../queue.h"
+
+#include "../../../libs/buzzer.h"
+#include "../../../lcd/marlinui.h"
+#include "../../../module/motion.h"
+#include "../../../module/stepper.h"
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../../../lcd/extui/ui_api.h"
+#endif
+
+// Save 130 bytes with non-duplication of PSTR
+inline void echo_not_entered(const char c) { SERIAL_CHAR(c); SERIAL_ECHOLNPGM(" not entered."); }
+
+/**
+ * G29: Mesh-based Z probe, probes a grid and produces a
+ *      mesh to compensate for variable bed height
+ *
+ * Parameters With MESH_BED_LEVELING:
+ *
+ *  S0              Report the current mesh values
+ *  S1              Start probing mesh points
+ *  S2              Probe the next mesh point
+ *  S3 In Jn Zn.nn  Manually modify a single point
+ *  S4 Zn.nn        Set z offset. Positive away from bed, negative closer to bed.
+ *  S5              Reset and disable mesh
+ */
+void GcodeSuite::G29() {
+
+  static int mbl_probe_index = -1;
+
+  MeshLevelingState state = (MeshLevelingState)parser.byteval('S', (int8_t)MeshReport);
+  if (!WITHIN(state, 0, 5)) {
+    SERIAL_ECHOLNPGM("S out of range (0-5).");
+    return;
+  }
+
+  int8_t ix, iy;
+
+  switch (state) {
+    case MeshReport:
+      SERIAL_ECHOPGM("Mesh Bed Leveling ");
+      if (leveling_is_valid()) {
+        serialprintln_onoff(planner.leveling_active);
+        mbl.report_mesh();
+      }
+      else
+        SERIAL_ECHOLNPGM("has no data.");
+      break;
+
+    case MeshStart:
+      mbl.reset();
+      mbl_probe_index = 0;
+      if (!ui.wait_for_move) {
+        queue.inject_P(parser.seen('N') ? PSTR("G28" TERN(G28_L0_ENSURES_LEVELING_OFF, "L0", "") "\nG29S2") : PSTR("G29S2"));
+        return;
+      }
+      state = MeshNext;
+
+    case MeshNext:
+      if (mbl_probe_index < 0) {
+        SERIAL_ECHOLNPGM("Start mesh probing with \"G29 S1\" first.");
+        return;
+      }
+      // For each G29 S2...
+      if (mbl_probe_index == 0) {
+        // Move close to the bed before the first point
+        do_blocking_move_to_z(0);
+      }
+      else {
+        // Save Z for the previous mesh position
+        mbl.set_zigzag_z(mbl_probe_index - 1, current_position.z);
+        SET_SOFT_ENDSTOP_LOOSE(false);
+      }
+      // If there's another point to sample, move there with optional lift.
+      if (mbl_probe_index < GRID_MAX_POINTS) {
+        // Disable software endstops to allow manual adjustment
+        // If G29 is left hanging without completion they won't be re-enabled!
+        SET_SOFT_ENDSTOP_LOOSE(true);
+        mbl.zigzag(mbl_probe_index++, ix, iy);
+        _manual_goto_xy({ mbl.index_to_xpos[ix], mbl.index_to_ypos[iy] });
+      }
+      else {
+        // One last "return to the bed" (as originally coded) at completion
+        current_position.z = MANUAL_PROBE_HEIGHT;
+        line_to_current_position();
+        planner.synchronize();
+
+        // After recording the last point, activate home and activate
+        mbl_probe_index = -1;
+        SERIAL_ECHOLNPGM("Mesh probing done.");
+        BUZZ(100, 659);
+        BUZZ(100, 698);
+
+        home_all_axes();
+        set_bed_leveling_enabled(true);
+
+        #if ENABLED(MESH_G28_REST_ORIGIN)
+          current_position.z = 0;
+          line_to_current_position(homing_feedrate(Z_AXIS));
+          planner.synchronize();
+        #endif
+
+        TERN_(LCD_BED_LEVELING, ui.wait_for_move = false);
+      }
+      break;
+
+    case MeshSet:
+      if (parser.seenval('I')) {
+        ix = parser.value_int();
+        if (!WITHIN(ix, 0, GRID_MAX_POINTS_X - 1)) {
+          SERIAL_ECHOPAIR("I out of range (0-", int(GRID_MAX_POINTS_X - 1));
+          SERIAL_ECHOLNPGM(")");
+          return;
+        }
+      }
+      else
+        return echo_not_entered('J');
+
+      if (parser.seenval('J')) {
+        iy = parser.value_int();
+        if (!WITHIN(iy, 0, GRID_MAX_POINTS_Y - 1)) {
+          SERIAL_ECHOPAIR("J out of range (0-", int(GRID_MAX_POINTS_Y - 1));
+          SERIAL_ECHOLNPGM(")");
+          return;
+        }
+      }
+      else
+        return echo_not_entered('J');
+
+      if (parser.seenval('Z')) {
+        mbl.z_values[ix][iy] = parser.value_linear_units();
+        TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(ix, iy, mbl.z_values[ix][iy]));
+      }
+      else
+        return echo_not_entered('Z');
+      break;
+
+    case MeshSetZOffset:
+      if (parser.seenval('Z'))
+        mbl.z_offset = parser.value_linear_units();
+      else
+        return echo_not_entered('Z');
+      break;
+
+    case MeshReset:
+      reset_bed_level();
+      break;
+
+  } // switch(state)
+
+  if (state == MeshNext) {
+    SERIAL_ECHOPAIR("MBL G29 point ", _MIN(mbl_probe_index, GRID_MAX_POINTS));
+    SERIAL_ECHOLNPAIR(" of ", int(GRID_MAX_POINTS));
+  }
+
+  report_current_position();
+}
+
+#endif // MESH_BED_LEVELING
diff --git a/Marlin/src/gcode/bedlevel/mbl/M421.cpp b/Marlin/src/gcode/bedlevel/mbl/M421.cpp
new file mode 100644
index 0000000..1368ab0
--- /dev/null
+++ b/Marlin/src/gcode/bedlevel/mbl/M421.cpp
@@ -0,0 +1,59 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * M421.cpp - Mesh Bed Leveling
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(MESH_BED_LEVELING)
+
+#include "../../gcode.h"
+#include "../../../module/motion.h"
+#include "../../../feature/bedlevel/mbl/mesh_bed_leveling.h"
+
+/**
+ * M421: Set a single Mesh Bed Leveling Z coordinate
+ *
+ * Usage:
+ *   M421 X<linear> Y<linear> Z<linear>
+ *   M421 X<linear> Y<linear> Q<offset>
+ *   M421 I<xindex> J<yindex> Z<linear>
+ *   M421 I<xindex> J<yindex> Q<offset>
+ */
+void GcodeSuite::M421() {
+  const bool hasX = parser.seen('X'), hasI = parser.seen('I');
+  const int8_t ix = hasI ? parser.value_int() : hasX ? mbl.probe_index_x(RAW_X_POSITION(parser.value_linear_units())) : -1;
+  const bool hasY = parser.seen('Y'), hasJ = parser.seen('J');
+  const int8_t iy = hasJ ? parser.value_int() : hasY ? mbl.probe_index_y(RAW_Y_POSITION(parser.value_linear_units())) : -1;
+  const bool hasZ = parser.seen('Z'), hasQ = !hasZ && parser.seen('Q');
+
+  if (int(hasI && hasJ) + int(hasX && hasY) != 1 || !(hasZ || hasQ))
+    SERIAL_ERROR_MSG(STR_ERR_M421_PARAMETERS);
+  else if (ix < 0 || iy < 0)
+    SERIAL_ERROR_MSG(STR_ERR_MESH_XY);
+  else
+    mbl.set_z(ix, iy, parser.value_linear_units() + (hasQ ? mbl.z_values[ix][iy] : 0));
+}
+
+#endif // MESH_BED_LEVELING
diff --git a/Marlin/src/gcode/bedlevel/ubl/G29.cpp b/Marlin/src/gcode/bedlevel/ubl/G29.cpp
new file mode 100644
index 0000000..2ef3ab4
--- /dev/null
+++ b/Marlin/src/gcode/bedlevel/ubl/G29.cpp
@@ -0,0 +1,36 @@
+/**
+ * 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 - Unified Bed Leveling
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(AUTO_BED_LEVELING_UBL)
+
+#include "../../gcode.h"
+#include "../../../feature/bedlevel/bedlevel.h"
+
+void GcodeSuite::G29() { ubl.G29(); }
+
+#endif // AUTO_BED_LEVELING_UBL
diff --git a/Marlin/src/gcode/bedlevel/ubl/M421.cpp b/Marlin/src/gcode/bedlevel/ubl/M421.cpp
new file mode 100644
index 0000000..600c1fc
--- /dev/null
+++ b/Marlin/src/gcode/bedlevel/ubl/M421.cpp
@@ -0,0 +1,70 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * unified.cpp - Unified Bed Leveling
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(AUTO_BED_LEVELING_UBL)
+
+#include "../../gcode.h"
+#include "../../../feature/bedlevel/bedlevel.h"
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../../../lcd/extui/ui_api.h"
+#endif
+
+/**
+ * M421: Set a single Mesh Bed Leveling Z coordinate
+ *
+ * Usage:
+ *   M421 I<xindex> J<yindex> Z<linear>
+ *   M421 I<xindex> J<yindex> Q<offset>
+ *   M421 I<xindex> J<yindex> N
+ *   M421 C Z<linear>
+ *   M421 C Q<offset>
+ */
+void GcodeSuite::M421() {
+  xy_int8_t ij = { int8_t(parser.intval('I', -1)), int8_t(parser.intval('J', -1)) };
+  const bool hasI = ij.x >= 0,
+             hasJ = ij.y >= 0,
+             hasC = parser.seen('C'),
+             hasN = parser.seen('N'),
+             hasZ = parser.seen('Z'),
+             hasQ = !hasZ && parser.seen('Q');
+
+  if (hasC) ij = ubl.find_closest_mesh_point_of_type(REAL, current_position);
+
+  if (int(hasC) + int(hasI && hasJ) != 1 || !(hasZ || hasQ || hasN))
+    SERIAL_ERROR_MSG(STR_ERR_M421_PARAMETERS);
+  else if (!WITHIN(ij.x, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(ij.y, 0, GRID_MAX_POINTS_Y - 1))
+    SERIAL_ERROR_MSG(STR_ERR_MESH_XY);
+  else {
+    float &zval = ubl.z_values[ij.x][ij.y];
+    zval = hasN ? NAN : parser.value_linear_units() + (hasQ ? zval : 0);
+    TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(ij.x, ij.y, zval));
+  }
+}
+
+#endif // AUTO_BED_LEVELING_UBL