Starting with Stock BTT-Config and Old Configs as Backup

Marlin/src/gcode/bedlevel/G26.cpp

@@ -0,0 +1,877 @@
+/**
+ * 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"
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../../lcd/extui/ui_api.h"
+#endif
+
+#if ENABLED(UBL_HILBERT_CURVE)
+  #include "../../feature/bedlevel/hilbert_curve.h"
+#endif
+
+#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;
+float g26_random_deviation = 0.0;
+
+#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
+
+void move_to(const_float_t rx, const_float_t ry, const_float_t z, const_float_t 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.
+             has_e_component = e_delta != 0.0;
+
+  if (z != last_z) {
+    last_z = z;
+    destination.set(current_position.x, current_position.y, z, current_position.e);
+    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);
+}
+
+void move_to(const xyz_pos_t &where, const_float_t de) { move_to(where.x, where.y, where.z, de); }
+
+typedef struct {
+  float extrusion_multiplier  = EXTRUSION_MULTIPLIER,
+        retraction_multiplier = G26_RETRACT_MULTIPLIER,
+        layer_height          = MESH_TEST_LAYER_HEIGHT,
+        prime_length          = PRIME_LENGTH;
+
+  celsius_t bed_temp          = MESH_TEST_BED_TEMP,
+            hotend_temp       = MESH_TEST_HOTEND_TEMP;
+
+  float nozzle                = MESH_TEST_NOZZLE_SIZE,
+        filament_diameter     = DEFAULT_NOMINAL_FILAMENT_DIA,
+        ooze_amount;            // 'O' ... OOZE_AMOUNT
+
+  bool continue_with_closest,   // 'C'
+       keep_heaters_on;         // 'K'
+
+  xy_pos_t xy_pos; // = { 0, 0 }
+
+  int8_t prime_flag = 0;
+
+  bool g26_retracted = false;  // Track the retracted state during G26 so mismatched
+                               // retracts/recovers don't result in a bad state.
+
+  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 * 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.0f);  // Z lift to minimize scraping
+    move_to(s.x, s.y, s.z + 0.5f, 0.0f);  // 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 * 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 * extrusion_multiplier;
+
+    recover_filament(destination);
+    move_to(e, e_pos_delta);  // Get to the ending point with an appropriate amount of extrusion
+  }
+
+  void connect_neighbor_with_line(const xy_int8_t &p1, int8_t dx, int8_t dy) {
+    xy_int8_t p2;
+    p2.x = p1.x + dx;
+    p2.y = p1.y + dy;
+
+    if (p2.x < 0 || p2.x >= (GRID_MAX_POINTS_X)) return;
+    if (p2.y < 0 || p2.y >= (GRID_MAX_POINTS_Y)) return;
+
+    if (circle_flags.marked(p1.x, p1.y) && circle_flags.marked(p2.x, p2.y)) {
+      xyz_pos_t s, e;
+      s.x = _GET_MESH_X(p1.x) + (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)) * dx;
+      e.x = _GET_MESH_X(p2.x) - (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)) * dx;
+      s.y = _GET_MESH_Y(p1.y) + (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)) * dy;
+      e.y = _GET_MESH_Y(p2.y) - (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)) * dy;
+      s.z = e.z = layer_height;
+
+      #if HAS_ENDSTOPS
+        LIMIT(s.y, Y_MIN_POS + 1, Y_MAX_POS - 1);
+        LIMIT(e.y, Y_MIN_POS + 1, Y_MAX_POS - 1);
+        LIMIT(s.x, X_MIN_POS + 1, X_MAX_POS - 1);
+        LIMIT(e.x, X_MIN_POS + 1, X_MAX_POS - 1);
+      #endif
+
+      if (position_is_reachable(s.x, s.y) && position_is_reachable(e.x, e.y))
+        print_line_from_here_to_there(s, e);
+    }
+  }
+
+  /**
+   * Turn on the bed and nozzle heat and
+   * wait for them to get up to temperature.
+   */
+  bool turn_on_heaters() {
+
+    SERIAL_ECHOLNPGM("Waiting for heatup.");
+
+    #if HAS_HEATED_BED
+
+      if (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(bed_temp);
+
+        // Wait for the temperature to stabilize
+        if (!thermalManager.wait_for_bed(true OPTARG(G26_CLICK_CAN_CANCEL, true)))
+          return G26_ERR;
+      }
+
+    #else
+
+      UNUSED(bed_temp);
+
+    #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(hotend_temp, active_extruder);
+
+    // Wait for the temperature to stabilize
+    if (!thermalManager.wait_for_hotend(active_extruder, true OPTARG(G26_CLICK_CAN_CANCEL, true)))
+      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.
+   */
+  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 (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 += prime_length;
+      prepare_internal_move_to_destination(fr_slow_e);
+      destination.e -= prime_length;
+      retract_filament(destination);
+    }
+
+    return G26_OK;
+  }
+
+  /**
+   * Find the nearest point at which to print a circle
+   */
+  mesh_index_pair find_closest_circle_to_print(const xy_pos_t &pos) {
+
+    mesh_index_pair out_point;
+    out_point.pos = -1;
+
+    #if ENABLED(UBL_HILBERT_CURVE)
+
+      auto test_func = [](uint8_t i, uint8_t j, void *data) -> bool {
+        if (!circle_flags.marked(i, j)) {
+          mesh_index_pair *out_point = (mesh_index_pair*)data;
+          out_point->pos.set(i, j);  // Save its data
+          return true;
+        }
+        return false;
+      };
+
+      hilbert_curve::search_from_closest(pos, test_func, &out_point);
+
+    #else
+
+      float closest = 99999.99;
+
+      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 += (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;
+          }
+        }
+      }
+
+    #endif
+
+    circle_flags.mark(out_point); // Mark this location as done.
+    return out_point;
+  }
+
+} g26_helper_t;
+
+/**
+ * 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_helper_t g26;
+
+  g26.ooze_amount           = parser.linearval('O', OOZE_AMOUNT);
+  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'
+    celsius_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-", 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'
+  celsius_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'))
+      g26_repeats = parser.has_value() ? parser.value_int() : GRID_MAX_POINTS + 1;
+    else {
+      SERIAL_ECHOLNPGM("?(R)epeat must be specified when not using an LCD.");
+      return;
+    }
+  #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_test('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 (g26.turn_on_heaters() != G26_OK) goto LEAVE;
+
+  current_position.e = 0.0;
+  sync_plan_position_e();
+
+  if (g26.prime_flag && g26.prime_nozzle() != G26_OK) goto LEAVE;
+
+  /**
+   *  Bed is preheated
+   *
+   *  Nozzle is at temperature
+   *
+   *  Filament is primed!
+   *
+   *  It's  "Show Time" !!!
+   */
+
+  circle_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;
+  TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(location.pos, ExtUI::G26_START));
+  do {
+    // Find the nearest confluence
+    location = g26.find_closest_circle_to_print(g26.continue_with_closest ? xy_pos_t(current_position) : g26.xy_pos);
+
+    if (location.valid()) {
+      TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(location.pos, ExtUI::G26_POINT_START));
+      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;
+          g26.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
+
+        g26.recover_filament(destination);
+
+        { REMEMBER(fr, feedrate_mm_s, PLANNER_XY_FEEDRATE() * 0.1f);
+          plan_arc(endpoint, arc_offset, false, 0);  // Draw a counter-clockwise arc
+          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;
+          #elif HAS_ENDSTOPS
+            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
+
+          g26.print_line_from_here_to_there(p, q);
+          SERIAL_FLUSH();   // Prevent host M105 buffer overrun.
+        }
+
+      #endif // !ARC_SUPPORT
+
+      g26.connect_neighbor_with_line(location.pos, -1,  0);
+      g26.connect_neighbor_with_line(location.pos,  1,  0);
+      g26.connect_neighbor_with_line(location.pos,  0, -1);
+      g26.connect_neighbor_with_line(location.pos,  0,  1);
+      planner.synchronize();
+      TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(location.pos, ExtUI::G26_POINT_FINISH));
+      if (TERN0(HAS_LCD_MENU, user_canceled())) 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);
+  TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(location, ExtUI::G26_FINISH));
+
+  g26.retract_filament(destination);
+  destination.z = Z_CLEARANCE_BETWEEN_PROBES;
+  move_to(destination, 0);                                   // Raise the nozzle
+
+  #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

Marlin/src/gcode/bedlevel/G35.cpp

@@ -0,0 +1,169 @@
+/**
+ * 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
+    #if ENABLED(RESTORE_LEVELING_AFTER_G35)
+      const bool leveling_was_active = planner.leveling_active;
+    #endif
+    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 only Z axis when X and Y is trusted, otherwise all axes, if needed before this procedure
+  if (!all_axes_trusted()) process_subcommands_now_P(PSTR("G28Z"));
+
+  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(SUM_TERN(BLTOUCH_HS_MODE, Z_CLEARANCE_BETWEEN_PROBES, 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 ", i + 1, " (");
+      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 ", i + 1, " (");
+      DEBUG_ECHOPGM_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

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

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_CELLS_X),
+                                  (y_max - y_min) / (GRID_MAX_CELLS_Y));
+      #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_test('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 // midrange
+
+            // 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);
+            }
+            // Get the midrange plus C value. (The median may be better.)
+            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

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

@@ -0,0 +1,905 @@
+/**
+ * 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_STATUS_MESSAGE
+  #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/e3v2/creality/dwin.h"
+#endif
+
+#if HAS_MULTI_HOTEND
+  #include "../../../module/tool_change.h"
+#endif
+
+#if ABL_USES_GRID
+  #if ENABLED(PROBE_Y_FIRST)
+    #define PR_OUTER_VAR  abl.meshCount.x
+    #define PR_OUTER_SIZE abl.grid_points.x
+    #define PR_INNER_VAR  abl.meshCount.y
+    #define PR_INNER_SIZE abl.grid_points.y
+  #else
+    #define PR_OUTER_VAR  abl.meshCount.y
+    #define PR_OUTER_SIZE abl.grid_points.y
+    #define PR_INNER_VAR  abl.meshCount.x
+    #define PR_INNER_SIZE abl.grid_points.x
+  #endif
+#endif
+
+#define G29_RETURN(b) return TERN_(G29_RETRY_AND_RECOVER, b)
+
+// For manual probing values persist over multiple G29
+class G29_State {
+public:
+  int       verbose_level;
+  xy_pos_t  probePos;
+  float     measured_z;
+  bool      dryrun,
+            reenable;
+
+  #if EITHER(PROBE_MANUALLY, AUTO_BED_LEVELING_LINEAR)
+    int abl_probe_index;
+  #endif
+
+  #if ENABLED(AUTO_BED_LEVELING_LINEAR)
+    int abl_points;
+  #elif ENABLED(AUTO_BED_LEVELING_3POINT)
+    static constexpr int abl_points = 3;
+  #elif ABL_USES_GRID
+    static constexpr int abl_points = GRID_MAX_POINTS;
+  #endif
+
+  #if ABL_USES_GRID
+
+    xy_int8_t meshCount;
+
+    xy_pos_t probe_position_lf,
+             probe_position_rb;
+
+    xy_float_t gridSpacing; // = { 0.0f, 0.0f }
+
+    #if ENABLED(AUTO_BED_LEVELING_LINEAR)
+      bool                topography_map;
+      xy_uint8_t          grid_points;
+    #else // Bilinear
+      static constexpr xy_uint8_t grid_points = { GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y };
+    #endif
+
+    #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+      float Z_offset;
+    #endif
+
+    #if ENABLED(AUTO_BED_LEVELING_LINEAR)
+      int indexIntoAB[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
+      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
+  #endif
+};
+
+#if ABL_USES_GRID && EITHER(AUTO_BED_LEVELING_3POINT, AUTO_BED_LEVELING_BILINEAR)
+  constexpr xy_uint8_t G29_State::grid_points;
+  constexpr int G29_State::abl_points;
+#endif
+
+/**
+ * G29: Detailed Z probe, probes the bed at 3 or more points.
+ *      Will fail if the printer has not been homed with G28.
+ *
+ * Enhanced G29 Auto Bed Leveling Probe Routine
+ *
+ *  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() {
+  DEBUG_SECTION(log_G29, "G29", DEBUGGING(LEVELING));
+
+  TERN_(PROBE_MANUALLY, static) G29_State abl;
+
+  TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_PROBE));
+
+  reset_stepper_timeout();
+
+  const bool seenQ = EITHER(DEBUG_LEVELING_FEATURE, PROBE_MANUALLY) && parser.seen_test('Q');
+
+  // G29 Q is also available if debugging
+  #if ENABLED(DEBUG_LEVELING_FEATURE)
+    if (seenQ || DEBUGGING(LEVELING)) log_machine_info();
+    if (DISABLED(PROBE_MANUALLY) && seenQ) G29_RETURN(false);
+  #endif
+
+  const bool seenA = TERN0(PROBE_MANUALLY, parser.seen_test('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_test('N'))
+    process_subcommands_now_P(TERN(CAN_SET_LEVELING_AFTER_G28, PSTR("G28L0"), G28_STR));
+
+  // Don't allow auto-leveling without homing first
+  if (homing_needed_error()) G29_RETURN(false);
+
+  #if ENABLED(AUTO_BED_LEVELING_3POINT)
+    vector_3 points[3];
+    probe.get_three_points(points);
+  #endif
+
+  #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.abl_probe_index = -1;
+    #endif
+
+    abl.reenable = planner.leveling_active;
+
+    #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+
+      const bool seen_w = parser.seen_test('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 * abl.gridSpacing.x) / abl.gridSpacing.x;
+          j = (ry - bilinear_start.y + 0.5 * abl.gridSpacing.y) / abl.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) - 1)) {
+          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.reenable);
+          if (abl.reenable) report_current_position();
+        }
+        G29_RETURN(false);
+      } // parser.seen_test('W')
+
+    #else
+
+      constexpr bool seen_w = false;
+
+    #endif
+
+    // Jettison bed leveling data
+    if (!seen_w && parser.seen_test('J')) {
+      reset_bed_level();
+      G29_RETURN(false);
+    }
+
+    abl.verbose_level = parser.intval('V');
+    if (!WITHIN(abl.verbose_level, 0, 4)) {
+      SERIAL_ECHOLNPGM("?(V)erbose level implausible (0-4).");
+      G29_RETURN(false);
+    }
+
+    abl.dryrun = parser.boolval('D') || TERN0(PROBE_MANUALLY, no_action);
+
+    #if ENABLED(AUTO_BED_LEVELING_LINEAR)
+
+      incremental_LSF_reset(&lsf_results);
+
+      abl.topography_map = abl.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.abl_points = abl.grid_points.x * abl.grid_points.y;
+      abl.mean = 0;
+
+    #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
+
+      abl.Z_offset = parser.linearval('Z');
+
+    #endif
+
+    #if ABL_USES_GRID
+
+      xy_probe_feedrate_mm_s = MMM_TO_MMS(parser.linearval('S', XY_PROBE_FEEDRATE));
+      if (!xy_probe_feedrate_mm_s) xy_probe_feedrate_mm_s = PLANNER_XY_FEEDRATE();
+      NOLESS(xy_probe_feedrate_mm_s, planner.settings.min_feedrate_mm_s);
+
+      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();
+        abl.probe_position_lf.set(_MAX((X_CENTER) - size / 2, x_min), _MAX((Y_CENTER) - size / 2, y_min));
+        abl.probe_position_rb.set(_MIN(abl.probe_position_lf.x + size, x_max), _MIN(abl.probe_position_lf.y + size, y_max));
+      }
+      else {
+        abl.probe_position_lf.set(parser.linearval('L', x_min), parser.linearval('F', y_min));
+        abl.probe_position_rb.set(parser.linearval('R', x_max), parser.linearval('B', y_max));
+      }
+
+      if (!probe.good_bounds(abl.probe_position_lf, abl.probe_position_rb)) {
+        if (DEBUGGING(LEVELING)) {
+          DEBUG_ECHOLNPAIR("G29 L", abl.probe_position_lf.x, " R", abl.probe_position_rb.x,
+                              " F", abl.probe_position_lf.y, " B", abl.probe_position_rb.y);
+        }
+        SERIAL_ECHOLNPGM("? (L,R,F,B) out of bounds.");
+        G29_RETURN(false);
+      }
+
+      // Probe at the points of a lattice grid
+      abl.gridSpacing.set((abl.probe_position_rb.x - abl.probe_position_lf.x) / (abl.grid_points.x - 1),
+                            (abl.probe_position_rb.y - abl.probe_position_lf.y) / (abl.grid_points.y - 1));
+
+    #endif // ABL_USES_GRID
+
+    if (abl.verbose_level > 0) {
+      SERIAL_ECHOPGM("G29 Auto Bed Leveling");
+      if (abl.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 (!abl.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.reenable);
+        G29_RETURN(false);
+      }
+    #endif
+
+    #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+      if (TERN1(PROBE_MANUALLY, !no_action)
+        && (abl.gridSpacing != bilinear_grid_spacing || abl.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 = abl.gridSpacing;
+        bilinear_start = abl.probe_position_lf;
+
+        // Can't re-enable (on error) until the new grid is written
+        abl.reenable = 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.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.reenable);
+      g29_in_progress = false;
+      TERN_(LCD_BED_LEVELING, ui.wait_for_move = false);
+    }
+
+    // Query G29 status
+    if (abl.verbose_level || seenQ) {
+      SERIAL_ECHOPGM("Manual G29 ");
+      if (g29_in_progress)
+        SERIAL_ECHOLNPAIR("point ", _MIN(abl.abl_probe_index + 1, abl.abl_points), " of ", abl.abl_points);
+      else
+        SERIAL_ECHOLNPGM("idle");
+    }
+
+    if (no_action) G29_RETURN(false);
+
+    if (abl.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.abl_probe_index - 1;
+      #endif
+
+      // For G29 after adjusting Z.
+      // Save the previous Z before going to the next point
+      abl.measured_z = current_position.z;
+
+      #if ENABLED(AUTO_BED_LEVELING_LINEAR)
+
+        abl.mean += abl.measured_z;
+        abl.eqnBVector[index] = abl.measured_z;
+        abl.eqnAMatrix[index + 0 * abl.abl_points] = abl.probePos.x;
+        abl.eqnAMatrix[index + 1 * abl.abl_points] = abl.probePos.y;
+        abl.eqnAMatrix[index + 2 * abl.abl_points] = 1;
+
+        incremental_LSF(&lsf_results, abl.probePos, abl.measured_z);
+
+      #elif ENABLED(AUTO_BED_LEVELING_3POINT)
+
+        points[index].z = abl.measured_z;
+
+      #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
+
+        const float newz = abl.measured_z + abl.Z_offset;
+        z_values[abl.meshCount.x][abl.meshCount.y] = newz;
+        TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(abl.meshCount, newz));
+
+        if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR_P(PSTR("Save X"), abl.meshCount.x, SP_Y_STR, abl.meshCount.y, SP_Z_STR, abl.measured_z + abl.Z_offset);
+
+      #endif
+    }
+
+    //
+    // If there's another point to sample, move there with optional lift.
+    //
+
+    #if ABL_USES_GRID
+
+      // Skip any unreachable points
+      while (abl.abl_probe_index < abl.abl_points) {
+
+        // Set abl.meshCount.x, abl.meshCount.y based on abl.abl_probe_index, with zig-zag
+        PR_OUTER_VAR = abl.abl_probe_index / PR_INNER_SIZE;
+        PR_INNER_VAR = abl.abl_probe_index - (PR_OUTER_VAR * PR_INNER_SIZE);
+
+        // Probe in reverse order for every other row/column
+        const bool zig = (PR_OUTER_VAR & 1); // != ((PR_OUTER_SIZE) & 1);
+        if (zig) PR_INNER_VAR = (PR_INNER_SIZE - 1) - PR_INNER_VAR;
+
+        abl.probePos = abl.probe_position_lf + abl.gridSpacing * abl.meshCount.asFloat();
+
+        TERN_(AUTO_BED_LEVELING_LINEAR, abl.indexIntoAB[abl.meshCount.x][abl.meshCount.y] = abl.abl_probe_index);
+
+        // Keep looping till a reachable point is found
+        if (position_is_reachable(abl.probePos)) break;
+        ++abl.abl_probe_index;
+      }
+
+      // Is there a next point to move to?
+      if (abl.abl_probe_index < abl.abl_points) {
+        _manual_goto_xy(abl.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.abl_probe_index < abl.abl_points) {
+        abl.probePos = xy_pos_t(points[abl.abl_probe_index]);
+        _manual_goto_xy(abl.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 (!abl.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.reenable = false;
+        }
+
+      }
+
+    #endif // AUTO_BED_LEVELING_3POINT
+
+  #else // !PROBE_MANUALLY
+  {
+    const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE;
+
+    abl.measured_z = 0;
+
+    #if ABL_USES_GRID
+
+      bool zig = PR_OUTER_SIZE & 1;  // Always end at RIGHT and BACK_PROBE_BED_POSITION
+
+      abl.measured_z = 0;
+
+      // 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_SIZE && !isnan(abl.measured_z); PR_OUTER_VAR++) {
+
+        int8_t inStart, inStop, inInc;
+
+        if (zig) {                      // Zig away from origin
+          inStart = 0;                  // Left or front
+          inStop = PR_INNER_SIZE;       // Right or back
+          inInc = 1;                    // Zig right
+        }
+        else {                          // Zag towards origin
+          inStart = PR_INNER_SIZE - 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_SIZE) + 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) {
+
+          abl.probePos = abl.probe_position_lf + abl.gridSpacing * abl.meshCount.asFloat();
+
+          TERN_(AUTO_BED_LEVELING_LINEAR, abl.indexIntoAB[abl.meshCount.x][abl.meshCount.y] = ++abl.abl_probe_index); // 0...
+
+          // Avoid probing outside the round or hexagonal area
+          if (TERN0(IS_KINEMATIC, !probe.can_reach(abl.probePos))) continue;
+
+          if (abl.verbose_level) SERIAL_ECHOLNPAIR("Probing mesh point ", pt_index, "/", abl.abl_points, ".");
+          TERN_(HAS_STATUS_MESSAGE, ui.status_printf_P(0, PSTR(S_FMT " %i/%i"), GET_TEXT(MSG_PROBING_MESH), int(pt_index), int(abl.abl_points)));
+
+          abl.measured_z = faux ? 0.001f * random(-100, 101) : probe.probe_at_point(abl.probePos, raise_after, abl.verbose_level);
+
+          if (isnan(abl.measured_z)) {
+            set_bed_leveling_enabled(abl.reenable);
+            break; // Breaks out of both loops
+          }
+
+          #if ENABLED(PROBE_TEMP_COMPENSATION)
+            temp_comp.compensate_measurement(TSI_BED, thermalManager.degBed(), abl.measured_z);
+            temp_comp.compensate_measurement(TSI_PROBE, thermalManager.degProbe(), abl.measured_z);
+            TERN_(USE_TEMP_EXT_COMPENSATION, temp_comp.compensate_measurement(TSI_EXT, thermalManager.degHotend(), abl.measured_z));
+          #endif
+
+          #if ENABLED(AUTO_BED_LEVELING_LINEAR)
+
+            abl.mean += abl.measured_z;
+            abl.eqnBVector[abl.abl_probe_index] = abl.measured_z;
+            abl.eqnAMatrix[abl.abl_probe_index + 0 * abl.abl_points] = abl.probePos.x;
+            abl.eqnAMatrix[abl.abl_probe_index + 1 * abl.abl_points] = abl.probePos.y;
+            abl.eqnAMatrix[abl.abl_probe_index + 2 * abl.abl_points] = 1;
+
+            incremental_LSF(&lsf_results, abl.probePos, abl.measured_z);
+
+          #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
+
+            const float z = abl.measured_z + abl.Z_offset;
+            z_values[abl.meshCount.x][abl.meshCount.y] = z;
+            TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(abl.meshCount, z));
+
+          #endif
+
+          abl.reenable = false;
+          idle_no_sleep();
+
+        } // inner
+      } // outer
+
+    #elif ENABLED(AUTO_BED_LEVELING_3POINT)
+
+      // Probe at 3 arbitrary points
+
+      LOOP_L_N(i, 3) {
+        if (abl.verbose_level) SERIAL_ECHOLNPAIR("Probing point ", i + 1, "/3.");
+        TERN_(HAS_STATUS_MESSAGE, ui.status_printf_P(0, PSTR(S_FMT " %i/3"), GET_TEXT(MSG_PROBING_MESH), int(i + 1)));
+
+        // Retain the last probe position
+        abl.probePos = xy_pos_t(points[i]);
+        abl.measured_z = faux ? 0.001 * random(-100, 101) : probe.probe_at_point(abl.probePos, raise_after, abl.verbose_level);
+        if (isnan(abl.measured_z)) {
+          set_bed_leveling_enabled(abl.reenable);
+          break;
+        }
+        points[i].z = abl.measured_z;
+      }
+
+      if (!abl.dryrun && !isnan(abl.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.reenable = false;
+      }
+
+    #endif // AUTO_BED_LEVELING_3POINT
+
+    TERN_(HAS_STATUS_MESSAGE, ui.reset_status());
+
+    // Stow the probe. No raise for FIX_MOUNTED_PROBE.
+    if (probe.stow()) {
+      set_bed_leveling_enabled(abl.reenable);
+      abl.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(abl.measured_z)) {
+    #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+
+      if (!abl.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;
+
+      abl.mean /= abl.abl_points;
+
+      if (abl.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 (abl.verbose_level > 2)
+          SERIAL_ECHOPAIR_F("\nMean of sampled points: ", abl.mean, 8);
+        SERIAL_EOL();
+      }
+
+      // Create the matrix but don't correct the position yet
+      if (!abl.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 (abl.topography_map) {
+
+        float min_diff = 999;
+
+        auto print_topo_map = [&](PGM_P const title, const bool get_min) {
+          SERIAL_ECHOPGM_P(title);
+          for (int8_t yy = abl.grid_points.y - 1; yy >= 0; yy--) {
+            LOOP_L_N(xx, abl.grid_points.x) {
+              const int ind = abl.indexIntoAB[xx][yy];
+              xyz_float_t tmp = { abl.eqnAMatrix[ind + 0 * abl.abl_points],
+                                  abl.eqnAMatrix[ind + 1 * abl.abl_points], 0 };
+              planner.bed_level_matrix.apply_rotation_xyz(tmp.x, tmp.y, tmp.z);
+              if (get_min) NOMORE(min_diff, abl.eqnBVector[ind] - tmp.z);
+              const float subval = get_min ? abl.mean : tmp.z + min_diff,
+                            diff = abl.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 (abl.verbose_level > 3)
+          print_topo_map(PSTR("\nCorrected Bed Height vs. Bed Topology:\n"), false);
+
+      } // abl.topography_map
+
+    #endif // AUTO_BED_LEVELING_LINEAR
+
+    #if ABL_PLANAR
+
+      // For LINEAR and 3POINT leveling correct the current position
+
+      if (abl.verbose_level > 0)
+        planner.bed_level_matrix.debug(PSTR("\n\nBed Level Correction Matrix:"));
+
+      if (!abl.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, abl.probePos.x - probe.offset_xy.x)
+          && NEAR(current_position.y, abl.probePos.y - probe.offset_xy.y)
+        ) {
+          const float simple_z = current_position.z - abl.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 (!abl.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 = !abl.dryrun || abl.reenable;
+  } // !isnan(abl.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();
+
+  TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_IDLE));
+
+  G29_RETURN(isnan(abl.measured_z));
+
+}
+
+#endif // HAS_ABL_NOT_UBL

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

Marlin/src/gcode/bedlevel/mbl/G29.cpp

@@ -0,0 +1,222 @@
+/**
+ * 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
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../../../core/debug_out.h"
+
+// 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() {
+  DEBUG_SECTION(log_G29, "G29", true);
+
+  // G29 Q is also available if debugging
+  #if ENABLED(DEBUG_LEVELING_FEATURE)
+    const bool seenQ = parser.seen_test('Q');
+    if (seenQ || DEBUGGING(LEVELING)) {
+      log_machine_info();
+      if (seenQ) return;
+    }
+  #endif
+
+  TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_PROBE));
+
+  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;
+  ix = iy = 0;
+
+  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_test('N') ? PSTR("G28" TERN(CAN_SET_LEVELING_AFTER_G28, "L0", "") "\nG29S2") : PSTR("G29S2"));
+        TERN_(EXTENSIBLE_UI, ExtUI::onMeshLevelingStart());
+        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.4f
+          #ifdef MANUAL_PROBE_START_Z
+            + (MANUAL_PROBE_START_Z) - 0.4f
+          #endif
+        );
+      }
+      else {
+        // Save Z for the previous mesh position
+        mbl.set_zigzag_z(mbl_probe_index - 1, current_position.z);
+        TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(ix, iy, 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 {
+        // Move to the after probing position
+        current_position.z = (
+          #ifdef Z_AFTER_PROBING
+            Z_AFTER_PROBING
+          #else
+            Z_CLEARANCE_BETWEEN_MANUAL_PROBES
+          #endif
+        );
+        line_to_current_position();
+        planner.synchronize();
+
+        // After recording the last point, activate home and activate
+        mbl_probe_index = -1;
+        SERIAL_ECHOLNPGM("Mesh probing done.");
+        TERN_(HAS_STATUS_MESSAGE, ui.set_status(GET_TEXT(MSG_MESH_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_ECHOLNPAIR("I out of range (0-", (GRID_MAX_POINTS_X) - 1, ")");
+          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_ECHOLNPAIR("J out of range (0-", (GRID_MAX_POINTS_Y) - 1, ")");
+          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_ECHOLNPAIR("MBL G29 point ", _MIN(mbl_probe_index, GRID_MAX_POINTS), " of ", GRID_MAX_POINTS);
+    if (mbl_probe_index > 0) TERN_(HAS_STATUS_MESSAGE, ui.status_printf_P(0, PSTR(S_FMT " %i/%i"), GET_TEXT(MSG_PROBING_MESH), _MIN(mbl_probe_index, GRID_MAX_POINTS), int(GRID_MAX_POINTS)));
+  }
+
+  report_current_position();
+
+  TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_IDLE));
+}
+
+#endif // MESH_BED_LEVELING

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

Marlin/src/gcode/bedlevel/ubl/G29.cpp

@@ -0,0 +1,47 @@
+/**
+ * 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"
+
+#if ENABLED(FULL_REPORT_TO_HOST_FEATURE)
+  #include "../../../module/motion.h"
+#endif
+
+void GcodeSuite::G29() {
+
+  TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_PROBE));
+
+  ubl.G29();
+
+  TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_IDLE));
+}
+
+#endif // AUTO_BED_LEVELING_UBL

Marlin/src/gcode/bedlevel/ubl/M421.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/>.
+ *
+ */
+
+/**
+ * M421.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>  : Set the Mesh Point IJ to the Z value
+ *   M421 I<xindex> J<yindex> Q<offset>  : Add the Q value to the Mesh Point IJ
+ *   M421 I<xindex> J<yindex> N          : Set the Mesh Point IJ to NAN (not set)
+ *   M421 C Z<linear>                    : Set the closest Mesh Point to the Z value
+ *   M421 C Q<offset>                    : Add the Q value to the closest Mesh Point
+ */
+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_test('C'),
+             hasN = parser.seen_test('N'),
+             hasZ = parser.seen('Z'),
+             hasQ = !hasZ && parser.seen('Q');
+
+  if (hasC) ij = ubl.find_closest_mesh_point_of_type(CLOSEST, current_position);
+
+  // Test for bad parameter combinations
+  if (int(hasC) + int(hasI && hasJ) != 1 || !(hasZ || hasQ || hasN))
+    SERIAL_ERROR_MSG(STR_ERR_M421_PARAMETERS);
+
+  // Test for I J out of range
+  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];                               // Altering this Mesh Point
+    zval = hasN ? NAN : parser.value_linear_units() + (hasQ ? zval : 0);  // N=NAN, Z=NEWVAL, or Q=ADDVAL
+    TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(ij.x, ij.y, zval));          // Ping ExtUI in case it's showing the mesh
+  }
+}
+
+#endif // AUTO_BED_LEVELING_UBL

Marlin/src/gcode/calibrate/G28.cpp

@@ -0,0 +1,551 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#include "../gcode.h"
+
+#include "../../module/stepper.h"
+#include "../../module/endstops.h"
+
+#if HAS_MULTI_HOTEND
+  #include "../../module/tool_change.h"
+#endif
+
+#if HAS_LEVELING
+  #include "../../feature/bedlevel/bedlevel.h"
+#endif
+
+#if ENABLED(SENSORLESS_HOMING)
+  #include "../../feature/tmc_util.h"
+#endif
+
+#include "../../module/probe.h"
+
+#if ENABLED(BLTOUCH)
+  #include "../../feature/bltouch.h"
+#endif
+
+#include "../../lcd/marlinui.h"
+#if ENABLED(DWIN_CREALITY_LCD)
+  #include "../../lcd/e3v2/creality/dwin.h"
+#endif
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../../lcd/extui/ui_api.h"
+#endif
+
+#if HAS_L64XX                         // set L6470 absolute position registers to counts
+  #include "../../libs/L64XX/L64XX_Marlin.h"
+#endif
+
+#if ENABLED(LASER_MOVE_G28_OFF)
+  #include "../../feature/spindle_laser.h"
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../../core/debug_out.h"
+
+#if ENABLED(QUICK_HOME)
+
+  static void quick_home_xy() {
+
+    // Pretend the current position is 0,0
+    current_position.set(0.0, 0.0);
+    sync_plan_position();
+
+    const int x_axis_home_dir = TOOL_X_HOME_DIR(active_extruder);
+
+    const float mlx = max_length(X_AXIS),
+                mly = max_length(Y_AXIS),
+                mlratio = mlx > mly ? mly / mlx : mlx / mly,
+                fr_mm_s = _MIN(homing_feedrate(X_AXIS), homing_feedrate(Y_AXIS)) * SQRT(sq(mlratio) + 1.0);
+
+    #if ENABLED(SENSORLESS_HOMING)
+      sensorless_t stealth_states {
+          tmc_enable_stallguard(stepperX)
+        , tmc_enable_stallguard(stepperY)
+        , false
+        , false
+          #if AXIS_HAS_STALLGUARD(X2)
+            || tmc_enable_stallguard(stepperX2)
+          #endif
+        , false
+          #if AXIS_HAS_STALLGUARD(Y2)
+            || tmc_enable_stallguard(stepperY2)
+          #endif
+      };
+    #endif
+
+    do_blocking_move_to_xy(1.5 * mlx * x_axis_home_dir, 1.5 * mly * Y_HOME_DIR, fr_mm_s);
+
+    endstops.validate_homing_move();
+
+    current_position.set(0.0, 0.0);
+
+    #if ENABLED(SENSORLESS_HOMING) && DISABLED(ENDSTOPS_ALWAYS_ON_DEFAULT)
+      tmc_disable_stallguard(stepperX, stealth_states.x);
+      tmc_disable_stallguard(stepperY, stealth_states.y);
+      #if AXIS_HAS_STALLGUARD(X2)
+        tmc_disable_stallguard(stepperX2, stealth_states.x2);
+      #endif
+      #if AXIS_HAS_STALLGUARD(Y2)
+        tmc_disable_stallguard(stepperY2, stealth_states.y2);
+      #endif
+    #endif
+  }
+
+#endif // QUICK_HOME
+
+#if ENABLED(Z_SAFE_HOMING)
+
+  inline void home_z_safely() {
+    DEBUG_SECTION(log_G28, "home_z_safely", DEBUGGING(LEVELING));
+
+    // Disallow Z homing if X or Y homing is needed
+    if (homing_needed_error(_BV(X_AXIS) | _BV(Y_AXIS))) return;
+
+    sync_plan_position();
+
+    /**
+     * Move the Z probe (or just the nozzle) to the safe homing point
+     * (Z is already at the right height)
+     */
+    constexpr xy_float_t safe_homing_xy = { Z_SAFE_HOMING_X_POINT, Z_SAFE_HOMING_Y_POINT };
+    #if HAS_HOME_OFFSET
+      xy_float_t okay_homing_xy = safe_homing_xy;
+      okay_homing_xy -= home_offset;
+    #else
+      constexpr xy_float_t okay_homing_xy = safe_homing_xy;
+    #endif
+
+    destination.set(okay_homing_xy, current_position.z);
+
+    TERN_(HOMING_Z_WITH_PROBE, destination -= probe.offset_xy);
+
+    if (position_is_reachable(destination)) {
+
+      if (DEBUGGING(LEVELING)) DEBUG_POS("home_z_safely", destination);
+
+      // Free the active extruder for movement
+      TERN_(DUAL_X_CARRIAGE, idex_set_parked(false));
+
+      TERN_(SENSORLESS_HOMING, safe_delay(500)); // Short delay needed to settle
+
+      do_blocking_move_to_xy(destination);
+      homeaxis(Z_AXIS);
+    }
+    else {
+      LCD_MESSAGEPGM(MSG_ZPROBE_OUT);
+      SERIAL_ECHO_MSG(STR_ZPROBE_OUT_SER);
+    }
+  }
+
+#endif // Z_SAFE_HOMING
+
+#if ENABLED(IMPROVE_HOMING_RELIABILITY)
+
+  motion_state_t begin_slow_homing() {
+    motion_state_t motion_state{0};
+    motion_state.acceleration.set(planner.settings.max_acceleration_mm_per_s2[X_AXIS],
+                                 planner.settings.max_acceleration_mm_per_s2[Y_AXIS]
+                                 OPTARG(DELTA, planner.settings.max_acceleration_mm_per_s2[Z_AXIS])
+                               );
+    planner.settings.max_acceleration_mm_per_s2[X_AXIS] = 100;
+    planner.settings.max_acceleration_mm_per_s2[Y_AXIS] = 100;
+    TERN_(DELTA, planner.settings.max_acceleration_mm_per_s2[Z_AXIS] = 100);
+    #if HAS_CLASSIC_JERK
+      motion_state.jerk_state = planner.max_jerk;
+      planner.max_jerk.set(0, 0 OPTARG(DELTA, 0));
+    #endif
+    planner.reset_acceleration_rates();
+    return motion_state;
+  }
+
+  void end_slow_homing(const motion_state_t &motion_state) {
+    planner.settings.max_acceleration_mm_per_s2[X_AXIS] = motion_state.acceleration.x;
+    planner.settings.max_acceleration_mm_per_s2[Y_AXIS] = motion_state.acceleration.y;
+    TERN_(DELTA, planner.settings.max_acceleration_mm_per_s2[Z_AXIS] = motion_state.acceleration.z);
+    TERN_(HAS_CLASSIC_JERK, planner.max_jerk = motion_state.jerk_state);
+    planner.reset_acceleration_rates();
+  }
+
+#endif // IMPROVE_HOMING_RELIABILITY
+
+/**
+ * G28: Home all axes according to settings
+ *
+ * Parameters
+ *
+ *  None  Home to all axes with no parameters.
+ *        With QUICK_HOME enabled XY will home together, then Z.
+ *
+ *  L<bool>   Force leveling state ON (if possible) or OFF after homing (Requires RESTORE_LEVELING_AFTER_G28 or ENABLE_LEVELING_AFTER_G28)
+ *  O         Home only if the position is not known and trusted
+ *  R<linear> Raise by n mm/inches before homing
+ *
+ * Cartesian/SCARA parameters
+ *
+ *  X   Home to the X endstop
+ *  Y   Home to the Y endstop
+ *  Z   Home to the Z endstop
+ */
+void GcodeSuite::G28() {
+  DEBUG_SECTION(log_G28, "G28", DEBUGGING(LEVELING));
+  if (DEBUGGING(LEVELING)) log_machine_info();
+
+  TERN_(LASER_MOVE_G28_OFF, cutter.set_inline_enabled(false));  // turn off laser
+
+  TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_HOMING));
+
+  #if ENABLED(DUAL_X_CARRIAGE)
+    bool IDEX_saved_duplication_state = extruder_duplication_enabled;
+    DualXMode IDEX_saved_mode = dual_x_carriage_mode;
+  #endif
+
+  #if ENABLED(MARLIN_DEV_MODE)
+    if (parser.seen_test('S')) {
+      LOOP_LINEAR_AXES(a) set_axis_is_at_home((AxisEnum)a);
+      sync_plan_position();
+      SERIAL_ECHOLNPGM("Simulated Homing");
+      report_current_position();
+      return;
+    }
+  #endif
+
+  // Home (O)nly if position is unknown
+  if (!axes_should_home() && parser.seen_test('O')) {
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> homing not needed, skip");
+    return;
+  }
+
+  TERN_(DWIN_CREALITY_LCD, DWIN_StartHoming());
+  TERN_(EXTENSIBLE_UI, ExtUI::onHomingStart());
+
+  planner.synchronize();          // Wait for planner moves to finish!
+
+  SET_SOFT_ENDSTOP_LOOSE(false);  // Reset a leftover 'loose' motion state
+
+  // Disable the leveling matrix before homing
+  #if CAN_SET_LEVELING_AFTER_G28
+    const bool leveling_restore_state = parser.boolval('L', TERN1(RESTORE_LEVELING_AFTER_G28, planner.leveling_active));
+  #endif
+
+  // Cancel any prior G29 session
+  TERN_(PROBE_MANUALLY, g29_in_progress = false);
+
+  // Disable leveling before homing
+  TERN_(HAS_LEVELING, set_bed_leveling_enabled(false));
+
+  // Reset to the XY plane
+  TERN_(CNC_WORKSPACE_PLANES, workspace_plane = PLANE_XY);
+
+  // Count this command as movement / activity
+  reset_stepper_timeout();
+
+  #define HAS_CURRENT_HOME(N) (defined(N##_CURRENT_HOME) && N##_CURRENT_HOME != N##_CURRENT)
+  #if HAS_CURRENT_HOME(X) || HAS_CURRENT_HOME(X2) || HAS_CURRENT_HOME(Y) || HAS_CURRENT_HOME(Y2) || (ENABLED(DELTA) && HAS_CURRENT_HOME(Z))
+    #define HAS_HOMING_CURRENT 1
+  #endif
+
+  #if HAS_HOMING_CURRENT
+    auto debug_current = [](PGM_P const s, const int16_t a, const int16_t b) {
+      DEBUG_ECHOPGM_P(s); DEBUG_ECHOLNPAIR(" current: ", a, " -> ", b);
+    };
+    #if HAS_CURRENT_HOME(X)
+      const int16_t tmc_save_current_X = stepperX.getMilliamps();
+      stepperX.rms_current(X_CURRENT_HOME);
+      if (DEBUGGING(LEVELING)) debug_current(PSTR("X"), tmc_save_current_X, X_CURRENT_HOME);
+    #endif
+    #if HAS_CURRENT_HOME(X2)
+      const int16_t tmc_save_current_X2 = stepperX2.getMilliamps();
+      stepperX2.rms_current(X2_CURRENT_HOME);
+      if (DEBUGGING(LEVELING)) debug_current(PSTR("X2"), tmc_save_current_X2, X2_CURRENT_HOME);
+    #endif
+    #if HAS_CURRENT_HOME(Y)
+      const int16_t tmc_save_current_Y = stepperY.getMilliamps();
+      stepperY.rms_current(Y_CURRENT_HOME);
+      if (DEBUGGING(LEVELING)) debug_current(PSTR("Y"), tmc_save_current_Y, Y_CURRENT_HOME);
+    #endif
+    #if HAS_CURRENT_HOME(Y2)
+      const int16_t tmc_save_current_Y2 = stepperY2.getMilliamps();
+      stepperY2.rms_current(Y2_CURRENT_HOME);
+      if (DEBUGGING(LEVELING)) debug_current(PSTR("Y2"), tmc_save_current_Y2, Y2_CURRENT_HOME);
+    #endif
+    #if HAS_CURRENT_HOME(Z) && ENABLED(DELTA)
+      const int16_t tmc_save_current_Z = stepperZ.getMilliamps();
+      stepperZ.rms_current(Z_CURRENT_HOME);
+      if (DEBUGGING(LEVELING)) debug_current(PSTR("Z"), tmc_save_current_Z, Z_CURRENT_HOME);
+    #endif
+  #endif
+
+  #if ENABLED(IMPROVE_HOMING_RELIABILITY)
+    motion_state_t saved_motion_state = begin_slow_homing();
+  #endif
+
+  // Always home with tool 0 active
+  #if HAS_MULTI_HOTEND
+    #if DISABLED(DELTA) || ENABLED(DELTA_HOME_TO_SAFE_ZONE)
+      const uint8_t old_tool_index = active_extruder;
+    #endif
+    // PARKING_EXTRUDER homing requires different handling of movement / solenoid activation, depending on the side of homing
+    #if ENABLED(PARKING_EXTRUDER)
+      const bool pe_final_change_must_unpark = parking_extruder_unpark_after_homing(old_tool_index, X_HOME_DIR + 1 == old_tool_index * 2);
+    #endif
+    tool_change(0, true);
+  #endif
+
+  TERN_(HAS_DUPLICATION_MODE, set_duplication_enabled(false));
+
+  remember_feedrate_scaling_off();
+
+  endstops.enable(true); // Enable endstops for next homing move
+
+  #if ENABLED(DELTA)
+
+    constexpr bool doZ = true; // for NANODLP_Z_SYNC if your DLP is on a DELTA
+
+    home_delta();
+
+    TERN_(IMPROVE_HOMING_RELIABILITY, end_slow_homing(saved_motion_state));
+
+  #elif ENABLED(AXEL_TPARA)
+
+    constexpr bool doZ = true; // for NANODLP_Z_SYNC if your DLP is on a TPARA
+
+    home_TPARA();
+
+  #else
+
+    #define _UNSAFE(A) (homeZ && TERN0(Z_SAFE_HOMING, axes_should_home(_BV(A##_AXIS))))
+
+    const bool homeZ = TERN0(HAS_Z_AXIS, parser.seen_test('Z')),
+               LINEAR_AXIS_LIST(              // Other axes should be homed before Z safe-homing
+                 needX = _UNSAFE(X), needY = _UNSAFE(Y), needZ = false, // UNUSED
+                 needI = _UNSAFE(I), needJ = _UNSAFE(J), needK = _UNSAFE(K)
+               ),
+               LINEAR_AXIS_LIST(              // Home each axis if needed or flagged
+                 homeX = needX || parser.seen_test('X'),
+                 homeY = needY || parser.seen_test('Y'),
+                 homeZZ = homeZ,
+                 homeI = needI || parser.seen_test(AXIS4_NAME), homeJ = needJ || parser.seen_test(AXIS5_NAME), homeK = needK || parser.seen_test(AXIS6_NAME),
+               ),
+               home_all = LINEAR_AXIS_GANG(   // Home-all if all or none are flagged
+                    homeX == homeX, && homeY == homeX, && homeZ == homeX,
+                 && homeI == homeX, && homeJ == homeX, && homeK == homeX
+               ),
+               LINEAR_AXIS_LIST(
+                 doX = home_all || homeX, doY = home_all || homeY, doZ = home_all || homeZ,
+                 doI = home_all || homeI, doJ = home_all || homeJ, doK = home_all || homeK
+               );
+
+    #if HAS_Z_AXIS
+      UNUSED(needZ); UNUSED(homeZZ);
+    #else
+      constexpr bool doZ = false;
+    #endif
+
+    TERN_(HOME_Z_FIRST, if (doZ) homeaxis(Z_AXIS));
+
+    const float z_homing_height = parser.seenval('R') ? parser.value_linear_units() : Z_HOMING_HEIGHT;
+
+    if (z_homing_height && (LINEAR_AXIS_GANG(doX, || doY, || TERN0(Z_SAFE_HOMING, doZ), || doI, || doJ, || doK))) {
+      // Raise Z before homing any other axes and z is not already high enough (never lower z)
+      if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Raise Z (before homing) by ", z_homing_height);
+      do_z_clearance(z_homing_height);
+      TERN_(BLTOUCH, bltouch.init());
+    }
+
+    // Diagonal move first if both are homing
+    TERN_(QUICK_HOME, if (doX && doY) quick_home_xy());
+
+    // Home Y (before X)
+    if (ENABLED(HOME_Y_BEFORE_X) && (doY || TERN0(CODEPENDENT_XY_HOMING, doX)))
+      homeaxis(Y_AXIS);
+
+    // Home X
+    if (doX || (doY && ENABLED(CODEPENDENT_XY_HOMING) && DISABLED(HOME_Y_BEFORE_X))) {
+
+      #if ENABLED(DUAL_X_CARRIAGE)
+
+        // Always home the 2nd (right) extruder first
+        active_extruder = 1;
+        homeaxis(X_AXIS);
+
+        // Remember this extruder's position for later tool change
+        inactive_extruder_x = current_position.x;
+
+        // Home the 1st (left) extruder
+        active_extruder = 0;
+        homeaxis(X_AXIS);
+
+        // Consider the active extruder to be in its "parked" position
+        idex_set_parked();
+
+      #else
+
+        homeaxis(X_AXIS);
+
+      #endif
+    }
+
+    // Home Y (after X)
+    if (DISABLED(HOME_Y_BEFORE_X) && doY)
+      homeaxis(Y_AXIS);
+
+    TERN_(IMPROVE_HOMING_RELIABILITY, end_slow_homing(saved_motion_state));
+
+    // Home Z last if homing towards the bed
+    #if HAS_Z_AXIS && DISABLED(HOME_Z_FIRST)
+      if (doZ) {
+        #if EITHER(Z_MULTI_ENDSTOPS, Z_STEPPER_AUTO_ALIGN)
+          stepper.set_all_z_lock(false);
+          stepper.set_separate_multi_axis(false);
+        #endif
+
+        TERN(Z_SAFE_HOMING, home_z_safely(), homeaxis(Z_AXIS));
+        probe.move_z_after_homing();
+      }
+    #endif
+
+    #if LINEAR_AXES >= 4
+      if (doI) homeaxis(I_AXIS);
+    #endif
+    #if LINEAR_AXES >= 5
+      if (doJ) homeaxis(J_AXIS);
+    #endif
+    #if LINEAR_AXES >= 6
+      if (doK) homeaxis(K_AXIS);
+    #endif
+
+    sync_plan_position();
+
+  #endif
+
+  /**
+   * Preserve DXC mode across a G28 for IDEX printers in DXC_DUPLICATION_MODE.
+   * This is important because it lets a user use the LCD Panel to set an IDEX Duplication mode, and
+   * then print a standard GCode file that contains a single print that does a G28 and has no other
+   * IDEX specific commands in it.
+   */
+  #if ENABLED(DUAL_X_CARRIAGE)
+
+    if (idex_is_duplicating()) {
+
+      TERN_(IMPROVE_HOMING_RELIABILITY, saved_motion_state = begin_slow_homing());
+
+      // Always home the 2nd (right) extruder first
+      active_extruder = 1;
+      homeaxis(X_AXIS);
+
+      // Remember this extruder's position for later tool change
+      inactive_extruder_x = current_position.x;
+
+      // Home the 1st (left) extruder
+      active_extruder = 0;
+      homeaxis(X_AXIS);
+
+      // Consider the active extruder to be parked
+      idex_set_parked();
+
+      dual_x_carriage_mode = IDEX_saved_mode;
+      set_duplication_enabled(IDEX_saved_duplication_state);
+
+      TERN_(IMPROVE_HOMING_RELIABILITY, end_slow_homing(saved_motion_state));
+    }
+
+  #endif // DUAL_X_CARRIAGE
+
+  endstops.not_homing();
+
+  // Clear endstop state for polled stallGuard endstops
+  TERN_(SPI_ENDSTOPS, endstops.clear_endstop_state());
+
+  // Move to a height where we can use the full xy-area
+  TERN_(DELTA_HOME_TO_SAFE_ZONE, do_blocking_move_to_z(delta_clip_start_height));
+
+  TERN_(CAN_SET_LEVELING_AFTER_G28, if (leveling_restore_state) set_bed_leveling_enabled());
+
+  restore_feedrate_and_scaling();
+
+  // Restore the active tool after homing
+  #if HAS_MULTI_HOTEND && (DISABLED(DELTA) || ENABLED(DELTA_HOME_TO_SAFE_ZONE))
+    tool_change(old_tool_index, TERN(PARKING_EXTRUDER, !pe_final_change_must_unpark, DISABLED(DUAL_X_CARRIAGE)));   // Do move if one of these
+  #endif
+
+  #if HAS_HOMING_CURRENT
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Restore driver current...");
+    #if HAS_CURRENT_HOME(X)
+      stepperX.rms_current(tmc_save_current_X);
+    #endif
+    #if HAS_CURRENT_HOME(X2)
+      stepperX2.rms_current(tmc_save_current_X2);
+    #endif
+    #if HAS_CURRENT_HOME(Y)
+      stepperY.rms_current(tmc_save_current_Y);
+    #endif
+    #if HAS_CURRENT_HOME(Y2)
+      stepperY2.rms_current(tmc_save_current_Y2);
+    #endif
+    #if HAS_CURRENT_HOME(Z) && ENABLED(DELTA)
+      stepperZ.rms_current(tmc_save_current_Z);
+    #endif
+    #if HAS_CURRENT_HOME(I)
+      stepperI.rms_current(tmc_save_current_I);
+    #endif
+    #if HAS_CURRENT_HOME(J)
+      stepperJ.rms_current(tmc_save_current_J);
+    #endif
+    #if HAS_CURRENT_HOME(K)
+      stepperK.rms_current(tmc_save_current_K);
+    #endif
+  #endif // HAS_HOMING_CURRENT
+
+  ui.refresh();
+
+  TERN_(DWIN_CREALITY_LCD, DWIN_CompletedHoming());
+  TERN_(EXTENSIBLE_UI, ExtUI::onHomingComplete());
+
+  report_current_position();
+
+  if (ENABLED(NANODLP_Z_SYNC) && (doZ || ENABLED(NANODLP_ALL_AXIS)))
+    SERIAL_ECHOLNPGM(STR_Z_MOVE_COMP);
+
+  TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_IDLE));
+
+  #if HAS_L64XX
+    // Set L6470 absolute position registers to counts
+    // constexpr *might* move this to PROGMEM.
+    // If not, this will need a PROGMEM directive and an accessor.
+    #define _EN_ITEM(N) , E_AXIS
+    static constexpr AxisEnum L64XX_axis_xref[MAX_L64XX] = {
+      LINEAR_AXIS_LIST(X_AXIS, Y_AXIS, Z_AXIS, I_AXIS, J_AXIS, K_AXIS),
+      X_AXIS, Y_AXIS, Z_AXIS, Z_AXIS, Z_AXIS
+      REPEAT(E_STEPPERS, _EN_ITEM)
+    };
+    #undef _EN_ITEM
+    for (uint8_t j = 1; j <= L64XX::chain[0]; j++) {
+      const uint8_t cv = L64XX::chain[j];
+      L64xxManager.set_param((L64XX_axis_t)cv, L6470_ABS_POS, stepper.position(L64XX_axis_xref[cv]));
+    }
+  #endif
+}

Marlin/src/gcode/calibrate/G33.cpp

@@ -0,0 +1,668 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(DELTA_AUTO_CALIBRATION)
+
+#include "../gcode.h"
+#include "../../module/delta.h"
+#include "../../module/motion.h"
+#include "../../module/stepper.h"
+#include "../../module/endstops.h"
+#include "../../lcd/marlinui.h"
+
+#if HAS_BED_PROBE
+  #include "../../module/probe.h"
+#endif
+
+#if HAS_MULTI_HOTEND
+  #include "../../module/tool_change.h"
+#endif
+
+#if HAS_LEVELING
+  #include "../../feature/bedlevel/bedlevel.h"
+#endif
+
+constexpr uint8_t _7P_STEP = 1,              // 7-point step - to change number of calibration points
+                  _4P_STEP = _7P_STEP * 2,   // 4-point step
+                  NPP      = _7P_STEP * 6;   // number of calibration points on the radius
+enum CalEnum : char {                        // the 7 main calibration points - add definitions if needed
+  CEN      = 0,
+  __A      = 1,
+  _AB      = __A + _7P_STEP,
+  __B      = _AB + _7P_STEP,
+  _BC      = __B + _7P_STEP,
+  __C      = _BC + _7P_STEP,
+  _CA      = __C + _7P_STEP,
+};
+
+#define LOOP_CAL_PT(VAR, S, N) for (uint8_t VAR=S; VAR<=NPP; VAR+=N)
+#define F_LOOP_CAL_PT(VAR, S, N) for (float VAR=S; VAR<NPP+0.9999; VAR+=N)
+#define I_LOOP_CAL_PT(VAR, S, N) for (float VAR=S; VAR>CEN+0.9999; VAR-=N)
+#define LOOP_CAL_ALL(VAR) LOOP_CAL_PT(VAR, CEN, 1)
+#define LOOP_CAL_RAD(VAR) LOOP_CAL_PT(VAR, __A, _7P_STEP)
+#define LOOP_CAL_ACT(VAR, _4P, _OP) LOOP_CAL_PT(VAR, _OP ? _AB : __A, _4P ? _4P_STEP : _7P_STEP)
+
+#if HAS_MULTI_HOTEND
+  const uint8_t old_tool_index = active_extruder;
+#endif
+
+float lcd_probe_pt(const xy_pos_t &xy);
+
+void ac_home() {
+  endstops.enable(true);
+  TERN_(SENSORLESS_HOMING, probe.set_homing_current(true));
+  home_delta();
+  TERN_(SENSORLESS_HOMING, probe.set_homing_current(false));
+  endstops.not_homing();
+}
+
+void ac_setup(const bool reset_bed) {
+  TERN_(HAS_MULTI_HOTEND, tool_change(0, true));
+
+  planner.synchronize();
+  remember_feedrate_scaling_off();
+
+  #if HAS_LEVELING
+    if (reset_bed) reset_bed_level(); // After full calibration bed-level data is no longer valid
+  #endif
+}
+
+void ac_cleanup(TERN_(HAS_MULTI_HOTEND, const uint8_t old_tool_index)) {
+  TERN_(DELTA_HOME_TO_SAFE_ZONE, do_blocking_move_to_z(delta_clip_start_height));
+  TERN_(HAS_BED_PROBE, probe.stow());
+  restore_feedrate_and_scaling();
+  TERN_(HAS_MULTI_HOTEND, tool_change(old_tool_index, true));
+}
+
+void print_signed_float(PGM_P const prefix, const_float_t f) {
+  SERIAL_ECHOPGM("  ");
+  SERIAL_ECHOPGM_P(prefix);
+  SERIAL_CHAR(':');
+  if (f >= 0) SERIAL_CHAR('+');
+  SERIAL_ECHO_F(f, 2);
+}
+
+/**
+ *  - Print the delta settings
+ */
+static void print_calibration_settings(const bool end_stops, const bool tower_angles) {
+  SERIAL_ECHOPAIR(".Height:", delta_height);
+  if (end_stops) {
+    print_signed_float(PSTR("Ex"), delta_endstop_adj.a);
+    print_signed_float(PSTR("Ey"), delta_endstop_adj.b);
+    print_signed_float(PSTR("Ez"), delta_endstop_adj.c);
+  }
+  if (end_stops && tower_angles) {
+    SERIAL_ECHOPAIR("  Radius:", delta_radius);
+    SERIAL_EOL();
+    SERIAL_CHAR('.');
+    SERIAL_ECHO_SP(13);
+  }
+  if (tower_angles) {
+    print_signed_float(PSTR("Tx"), delta_tower_angle_trim.a);
+    print_signed_float(PSTR("Ty"), delta_tower_angle_trim.b);
+    print_signed_float(PSTR("Tz"), delta_tower_angle_trim.c);
+  }
+  if ((!end_stops && tower_angles) || (end_stops && !tower_angles)) { // XOR
+    SERIAL_ECHOPAIR("  Radius:", delta_radius);
+  }
+  SERIAL_EOL();
+}
+
+/**
+ *  - Print the probe results
+ */
+static void print_calibration_results(const float z_pt[NPP + 1], const bool tower_points, const bool opposite_points) {
+  SERIAL_ECHOPGM(".    ");
+  print_signed_float(PSTR("c"), z_pt[CEN]);
+  if (tower_points) {
+    print_signed_float(PSTR(" x"), z_pt[__A]);
+    print_signed_float(PSTR(" y"), z_pt[__B]);
+    print_signed_float(PSTR(" z"), z_pt[__C]);
+  }
+  if (tower_points && opposite_points) {
+    SERIAL_EOL();
+    SERIAL_CHAR('.');
+    SERIAL_ECHO_SP(13);
+  }
+  if (opposite_points) {
+    print_signed_float(PSTR("yz"), z_pt[_BC]);
+    print_signed_float(PSTR("zx"), z_pt[_CA]);
+    print_signed_float(PSTR("xy"), z_pt[_AB]);
+  }
+  SERIAL_EOL();
+}
+
+/**
+ *  - Calculate the standard deviation from the zero plane
+ */
+static float std_dev_points(float z_pt[NPP + 1], const bool _0p_cal, const bool _1p_cal, const bool _4p_cal, const bool _4p_opp) {
+  if (!_0p_cal) {
+    float S2 = sq(z_pt[CEN]);
+    int16_t N = 1;
+    if (!_1p_cal) { // std dev from zero plane
+      LOOP_CAL_ACT(rad, _4p_cal, _4p_opp) {
+        S2 += sq(z_pt[rad]);
+        N++;
+      }
+      return LROUND(SQRT(S2 / N) * 1000.0f) / 1000.0f + 0.00001f;
+    }
+  }
+  return 0.00001f;
+}
+
+/**
+ *  - Probe a point
+ */
+static float calibration_probe(const xy_pos_t &xy, const bool stow) {
+  #if HAS_BED_PROBE
+    return probe.probe_at_point(xy, stow ? PROBE_PT_STOW : PROBE_PT_RAISE, 0, true, false);
+  #else
+    UNUSED(stow);
+    return lcd_probe_pt(xy);
+  #endif
+}
+
+/**
+ *  - Probe a grid
+ */
+static bool probe_calibration_points(float z_pt[NPP + 1], const int8_t probe_points, const bool towers_set, const bool stow_after_each) {
+  const bool _0p_calibration      = probe_points == 0,
+             _1p_calibration      = probe_points == 1 || probe_points == -1,
+             _4p_calibration      = probe_points == 2,
+             _4p_opposite_points  = _4p_calibration && !towers_set,
+             _7p_calibration      = probe_points >= 3,
+             _7p_no_intermediates = probe_points == 3,
+             _7p_1_intermediates  = probe_points == 4,
+             _7p_2_intermediates  = probe_points == 5,
+             _7p_4_intermediates  = probe_points == 6,
+             _7p_6_intermediates  = probe_points == 7,
+             _7p_8_intermediates  = probe_points == 8,
+             _7p_11_intermediates = probe_points == 9,
+             _7p_14_intermediates = probe_points == 10,
+             _7p_intermed_points  = probe_points >= 4,
+             _7p_6_center         = probe_points >= 5 && probe_points <= 7,
+             _7p_9_center         = probe_points >= 8;
+
+  LOOP_CAL_ALL(rad) z_pt[rad] = 0.0f;
+
+  if (!_0p_calibration) {
+
+    const float dcr = delta_calibration_radius();
+
+    if (!_7p_no_intermediates && !_7p_4_intermediates && !_7p_11_intermediates) { // probe the center
+      const xy_pos_t center{0};
+      z_pt[CEN] += calibration_probe(center, stow_after_each);
+      if (isnan(z_pt[CEN])) return false;
+    }
+
+    if (_7p_calibration) { // probe extra center points
+      const float start  = _7p_9_center ? float(_CA) + _7P_STEP / 3.0f : _7p_6_center ? float(_CA) : float(__C),
+                  steps  = _7p_9_center ? _4P_STEP / 3.0f : _7p_6_center ? _7P_STEP : _4P_STEP;
+      I_LOOP_CAL_PT(rad, start, steps) {
+        const float a = RADIANS(210 + (360 / NPP) *  (rad - 1)),
+                    r = dcr * 0.1;
+        const xy_pos_t vec = { cos(a), sin(a) };
+        z_pt[CEN] += calibration_probe(vec * r, stow_after_each);
+        if (isnan(z_pt[CEN])) return false;
+     }
+      z_pt[CEN] /= float(_7p_2_intermediates ? 7 : probe_points);
+    }
+
+    if (!_1p_calibration) {  // probe the radius
+      const CalEnum start  = _4p_opposite_points ? _AB : __A;
+      const float   steps  = _7p_14_intermediates ? _7P_STEP / 15.0f : // 15r * 6 + 10c = 100
+                             _7p_11_intermediates ? _7P_STEP / 12.0f : // 12r * 6 +  9c = 81
+                             _7p_8_intermediates  ? _7P_STEP /  9.0f : //  9r * 6 + 10c = 64
+                             _7p_6_intermediates  ? _7P_STEP /  7.0f : //  7r * 6 +  7c = 49
+                             _7p_4_intermediates  ? _7P_STEP /  5.0f : //  5r * 6 +  6c = 36
+                             _7p_2_intermediates  ? _7P_STEP /  3.0f : //  3r * 6 +  7c = 25
+                             _7p_1_intermediates  ? _7P_STEP /  2.0f : //  2r * 6 +  4c = 16
+                             _7p_no_intermediates ? _7P_STEP :        //  1r * 6 +  3c = 9
+                             _4P_STEP;                                // .5r * 6 +  1c = 4
+      bool zig_zag = true;
+      F_LOOP_CAL_PT(rad, start, _7p_9_center ? steps * 3 : steps) {
+        const int8_t offset = _7p_9_center ? 2 : 0;
+        for (int8_t circle = 0; circle <= offset; circle++) {
+          const float a = RADIANS(210 + (360 / NPP) *  (rad - 1)),
+                      r = dcr * (1 - 0.1 * (zig_zag ? offset - circle : circle)),
+                      interpol = FMOD(rad, 1);
+          const xy_pos_t vec = { cos(a), sin(a) };
+          const float z_temp = calibration_probe(vec * r, stow_after_each);
+          if (isnan(z_temp)) return false;
+          // split probe point to neighbouring calibration points
+          z_pt[uint8_t(LROUND(rad - interpol + NPP - 1)) % NPP + 1] += z_temp * sq(cos(RADIANS(interpol * 90)));
+          z_pt[uint8_t(LROUND(rad - interpol))           % NPP + 1] += z_temp * sq(sin(RADIANS(interpol * 90)));
+        }
+        zig_zag = !zig_zag;
+      }
+      if (_7p_intermed_points)
+        LOOP_CAL_RAD(rad)
+          z_pt[rad] /= _7P_STEP / steps;
+
+      do_blocking_move_to_xy(0.0f, 0.0f);
+    }
+  }
+  return true;
+}
+
+/**
+ * kinematics routines and auto tune matrix scaling parameters:
+ * see https://github.com/LVD-AC/Marlin-AC/tree/1.1.x-AC/documentation for
+ *  - formulae for approximative forward kinematics in the end-stop displacement matrix
+ *  - definition of the matrix scaling parameters
+ */
+static void reverse_kinematics_probe_points(float z_pt[NPP + 1], abc_float_t mm_at_pt_axis[NPP + 1]) {
+  xyz_pos_t pos{0};
+
+  const float dcr = delta_calibration_radius();
+  LOOP_CAL_ALL(rad) {
+    const float a = RADIANS(210 + (360 / NPP) *  (rad - 1)),
+                r = (rad == CEN ? 0.0f : dcr);
+    pos.set(cos(a) * r, sin(a) * r, z_pt[rad]);
+    inverse_kinematics(pos);
+    mm_at_pt_axis[rad] = delta;
+  }
+}
+
+static void forward_kinematics_probe_points(abc_float_t mm_at_pt_axis[NPP + 1], float z_pt[NPP + 1]) {
+  const float r_quot = delta_calibration_radius() / delta_radius;
+
+  #define ZPP(N,I,A) (((1.0f + r_quot * (N)) / 3.0f) * mm_at_pt_axis[I].A)
+  #define Z00(I, A) ZPP( 0, I, A)
+  #define Zp1(I, A) ZPP(+1, I, A)
+  #define Zm1(I, A) ZPP(-1, I, A)
+  #define Zp2(I, A) ZPP(+2, I, A)
+  #define Zm2(I, A) ZPP(-2, I, A)
+
+  z_pt[CEN] = Z00(CEN, a) + Z00(CEN, b) + Z00(CEN, c);
+  z_pt[__A] = Zp2(__A, a) + Zm1(__A, b) + Zm1(__A, c);
+  z_pt[__B] = Zm1(__B, a) + Zp2(__B, b) + Zm1(__B, c);
+  z_pt[__C] = Zm1(__C, a) + Zm1(__C, b) + Zp2(__C, c);
+  z_pt[_BC] = Zm2(_BC, a) + Zp1(_BC, b) + Zp1(_BC, c);
+  z_pt[_CA] = Zp1(_CA, a) + Zm2(_CA, b) + Zp1(_CA, c);
+  z_pt[_AB] = Zp1(_AB, a) + Zp1(_AB, b) + Zm2(_AB, c);
+}
+
+static void calc_kinematics_diff_probe_points(float z_pt[NPP + 1], abc_float_t delta_e, const float delta_r, abc_float_t delta_t) {
+  const float z_center = z_pt[CEN];
+  abc_float_t diff_mm_at_pt_axis[NPP + 1], new_mm_at_pt_axis[NPP + 1];
+
+  reverse_kinematics_probe_points(z_pt, diff_mm_at_pt_axis);
+
+  delta_radius += delta_r;
+  delta_tower_angle_trim += delta_t;
+  recalc_delta_settings();
+  reverse_kinematics_probe_points(z_pt, new_mm_at_pt_axis);
+
+  LOOP_CAL_ALL(rad) diff_mm_at_pt_axis[rad] -= new_mm_at_pt_axis[rad] + delta_e;
+  forward_kinematics_probe_points(diff_mm_at_pt_axis, z_pt);
+
+  LOOP_CAL_RAD(rad) z_pt[rad] -= z_pt[CEN] - z_center;
+  z_pt[CEN] = z_center;
+
+  delta_radius -= delta_r;
+  delta_tower_angle_trim -= delta_t;
+  recalc_delta_settings();
+}
+
+static float auto_tune_h() {
+  const float r_quot = delta_calibration_radius() / delta_radius;
+  return RECIPROCAL(r_quot / (2.0f / 3.0f));  // (2/3)/CR
+}
+
+static float auto_tune_r() {
+  constexpr float diff = 0.01f, delta_r = diff;
+  float r_fac = 0.0f, z_pt[NPP + 1] = { 0.0f };
+  abc_float_t delta_e = { 0.0f }, delta_t = { 0.0f };
+
+  calc_kinematics_diff_probe_points(z_pt, delta_e, delta_r, delta_t);
+  r_fac = -(z_pt[__A] + z_pt[__B] + z_pt[__C] + z_pt[_BC] + z_pt[_CA] + z_pt[_AB]) / 6.0f;
+  r_fac = diff / r_fac / 3.0f; // 1/(3*delta_Z)
+  return r_fac;
+}
+
+static float auto_tune_a() {
+  constexpr float diff = 0.01f, delta_r = 0.0f;
+  float a_fac = 0.0f, z_pt[NPP + 1] = { 0.0f };
+  abc_float_t delta_e = { 0.0f }, delta_t = { 0.0f };
+
+  delta_t.reset();
+  LOOP_LINEAR_AXES(axis) {
+    delta_t[axis] = diff;
+    calc_kinematics_diff_probe_points(z_pt, delta_e, delta_r, delta_t);
+    delta_t[axis] = 0;
+    a_fac += z_pt[uint8_t((axis * _4P_STEP) - _7P_STEP + NPP) % NPP + 1] / 6.0f;
+    a_fac -= z_pt[uint8_t((axis * _4P_STEP) + 1 + _7P_STEP)] / 6.0f;
+  }
+  a_fac = diff / a_fac / 3.0f; // 1/(3*delta_Z)
+  return a_fac;
+}
+
+/**
+ * G33 - Delta '1-4-7-point' Auto-Calibration
+ *       Calibrate height, z_offset, endstops, delta radius, and tower angles.
+ *
+ * Parameters:
+ *
+ *   Pn  Number of probe points:
+ *      P0       Normalizes calibration.
+ *      P1       Calibrates height only with center probe.
+ *      P2       Probe center and towers. Calibrate height, endstops and delta radius.
+ *      P3       Probe all positions: center, towers and opposite towers. Calibrate all.
+ *      P4-P10   Probe all positions at different intermediate locations and average them.
+ *
+ *   T   Don't calibrate tower angle corrections
+ *
+ *   Cn.nn  Calibration precision; when omitted calibrates to maximum precision
+ *
+ *   Fn  Force to run at least n iterations and take the best result
+ *
+ *   Vn  Verbose level:
+ *      V0  Dry-run mode. Report settings and probe results. No calibration.
+ *      V1  Report start and end settings only
+ *      V2  Report settings at each iteration
+ *      V3  Report settings and probe results
+ *
+ *   E   Engage the probe for each point
+ *
+ * With SENSORLESS_PROBING:
+ *   Use these flags to calibrate stall sensitivity: (e.g., `G33 P1 Y Z` to calibrate X only.)
+ *   X   Don't activate stallguard on X.
+ *   Y   Don't activate stallguard on Y.
+ *   Z   Don't activate stallguard on Z.
+ */
+void GcodeSuite::G33() {
+
+  TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_PROBE));
+
+  const int8_t probe_points = parser.intval('P', DELTA_CALIBRATION_DEFAULT_POINTS);
+  if (!WITHIN(probe_points, 0, 10)) {
+    SERIAL_ECHOLNPGM("?(P)oints implausible (0-10).");
+    return;
+  }
+
+  const bool towers_set = !parser.seen_test('T');
+
+  const float calibration_precision = parser.floatval('C', 0.0f);
+  if (calibration_precision < 0) {
+    SERIAL_ECHOLNPGM("?(C)alibration precision implausible (>=0).");
+    return;
+  }
+
+  const int8_t force_iterations = parser.intval('F', 0);
+  if (!WITHIN(force_iterations, 0, 30)) {
+    SERIAL_ECHOLNPGM("?(F)orce iteration implausible (0-30).");
+    return;
+  }
+
+  const int8_t verbose_level = parser.byteval('V', 1);
+  if (!WITHIN(verbose_level, 0, 3)) {
+    SERIAL_ECHOLNPGM("?(V)erbose level implausible (0-3).");
+    return;
+  }
+
+  const bool stow_after_each = parser.seen_test('E');
+
+  #if ENABLED(SENSORLESS_PROBING)
+    probe.test_sensitivity.x = !parser.seen_test('X');
+    TERN_(HAS_Y_AXIS, probe.test_sensitivity.y = !parser.seen_test('Y'));
+    TERN_(HAS_Z_AXIS, probe.test_sensitivity.z = !parser.seen_test('Z'));
+  #endif
+
+  const bool _0p_calibration      = probe_points == 0,
+             _1p_calibration      = probe_points == 1 || probe_points == -1,
+             _4p_calibration      = probe_points == 2,
+             _4p_opposite_points  = _4p_calibration && !towers_set,
+             _7p_9_center         = probe_points >= 8,
+             _tower_results       = (_4p_calibration && towers_set) || probe_points >= 3,
+             _opposite_results    = (_4p_calibration && !towers_set) || probe_points >= 3,
+             _endstop_results     = probe_points != 1 && probe_points != -1 && probe_points != 0,
+             _angle_results       = probe_points >= 3 && towers_set;
+  int8_t iterations = 0;
+  float test_precision,
+        zero_std_dev = (verbose_level ? 999.0f : 0.0f), // 0.0 in dry-run mode : forced end
+        zero_std_dev_min = zero_std_dev,
+        zero_std_dev_old = zero_std_dev,
+        h_factor, r_factor, a_factor,
+        r_old = delta_radius,
+        h_old = delta_height;
+
+  abc_pos_t e_old = delta_endstop_adj, a_old = delta_tower_angle_trim;
+
+  SERIAL_ECHOLNPGM("G33 Auto Calibrate");
+
+  const float dcr = delta_calibration_radius();
+
+  if (!_1p_calibration && !_0p_calibration) { // test if the outer radius is reachable
+    LOOP_CAL_RAD(axis) {
+      const float a = RADIANS(210 + (360 / NPP) *  (axis - 1));
+      if (!position_is_reachable(cos(a) * dcr, sin(a) * dcr)) {
+        SERIAL_ECHOLNPGM("?Bed calibration radius implausible.");
+        return;
+      }
+    }
+  }
+
+  // Report settings
+  PGM_P const checkingac = PSTR("Checking... AC");
+  SERIAL_ECHOPGM_P(checkingac);
+  if (verbose_level == 0) SERIAL_ECHOPGM(" (DRY-RUN)");
+  SERIAL_EOL();
+  ui.set_status_P(checkingac);
+
+  print_calibration_settings(_endstop_results, _angle_results);
+
+  ac_setup(!_0p_calibration && !_1p_calibration);
+
+  if (!_0p_calibration) ac_home();
+
+  do { // start iterations
+
+    float z_at_pt[NPP + 1] = { 0.0f };
+
+    test_precision = zero_std_dev_old != 999.0f ? (zero_std_dev + zero_std_dev_old) / 2.0f : zero_std_dev;
+    iterations++;
+
+    // Probe the points
+    zero_std_dev_old = zero_std_dev;
+    if (!probe_calibration_points(z_at_pt, probe_points, towers_set, stow_after_each)) {
+      SERIAL_ECHOLNPGM("Correct delta settings with M665 and M666");
+      return ac_cleanup(TERN_(HAS_MULTI_HOTEND, old_tool_index));
+    }
+    zero_std_dev = std_dev_points(z_at_pt, _0p_calibration, _1p_calibration, _4p_calibration, _4p_opposite_points);
+
+    // Solve matrices
+
+    if ((zero_std_dev < test_precision || iterations <= force_iterations) && zero_std_dev > calibration_precision) {
+
+      #if !HAS_BED_PROBE
+        test_precision = 0.0f; // forced end
+      #endif
+
+      if (zero_std_dev < zero_std_dev_min) {
+        // set roll-back point
+        e_old = delta_endstop_adj;
+        r_old = delta_radius;
+        h_old = delta_height;
+        a_old = delta_tower_angle_trim;
+      }
+
+      abc_float_t e_delta = { 0.0f }, t_delta = { 0.0f };
+      float r_delta = 0.0f;
+
+      /**
+       * convergence matrices:
+       * see https://github.com/LVD-AC/Marlin-AC/tree/1.1.x-AC/documentation for
+       *  - definition of the matrix scaling parameters
+       *  - matrices for 4 and 7 point calibration
+       */
+      #define ZP(N,I) ((N) * z_at_pt[I] / 4.0f) // 4.0 = divider to normalize to integers
+      #define Z12(I) ZP(12, I)
+      #define Z4(I) ZP(4, I)
+      #define Z2(I) ZP(2, I)
+      #define Z1(I) ZP(1, I)
+      #define Z0(I) ZP(0, I)
+
+      // calculate factors
+      if (_7p_9_center) calibration_radius_factor = 0.9f;
+      h_factor = auto_tune_h();
+      r_factor = auto_tune_r();
+      a_factor = auto_tune_a();
+      calibration_radius_factor = 1.0f;
+
+      switch (probe_points) {
+        case 0:
+          test_precision = 0.0f; // forced end
+          break;
+
+        case 1:
+          test_precision = 0.0f; // forced end
+          LOOP_LINEAR_AXES(axis) e_delta[axis] = +Z4(CEN);
+          break;
+
+        case 2:
+          if (towers_set) { // see 4 point calibration (towers) matrix
+            e_delta.set((+Z4(__A) -Z2(__B) -Z2(__C)) * h_factor  +Z4(CEN),
+                        (-Z2(__A) +Z4(__B) -Z2(__C)) * h_factor  +Z4(CEN),
+                        (-Z2(__A) -Z2(__B) +Z4(__C)) * h_factor  +Z4(CEN));
+            r_delta   = (+Z4(__A) +Z4(__B) +Z4(__C) -Z12(CEN)) * r_factor;
+          }
+          else { // see 4 point calibration (opposites) matrix
+            e_delta.set((-Z4(_BC) +Z2(_CA) +Z2(_AB)) * h_factor  +Z4(CEN),
+                        (+Z2(_BC) -Z4(_CA) +Z2(_AB)) * h_factor  +Z4(CEN),
+                        (+Z2(_BC) +Z2(_CA) -Z4(_AB)) * h_factor  +Z4(CEN));
+            r_delta   = (+Z4(_BC) +Z4(_CA) +Z4(_AB) -Z12(CEN)) * r_factor;
+          }
+          break;
+
+        default: // see 7 point calibration (towers & opposites) matrix
+          e_delta.set((+Z2(__A) -Z1(__B) -Z1(__C) -Z2(_BC) +Z1(_CA) +Z1(_AB)) * h_factor  +Z4(CEN),
+                      (-Z1(__A) +Z2(__B) -Z1(__C) +Z1(_BC) -Z2(_CA) +Z1(_AB)) * h_factor  +Z4(CEN),
+                      (-Z1(__A) -Z1(__B) +Z2(__C) +Z1(_BC) +Z1(_CA) -Z2(_AB)) * h_factor  +Z4(CEN));
+          r_delta   = (+Z2(__A) +Z2(__B) +Z2(__C) +Z2(_BC) +Z2(_CA) +Z2(_AB) -Z12(CEN)) * r_factor;
+
+          if (towers_set) { // see 7 point tower angle calibration (towers & opposites) matrix
+            t_delta.set((+Z0(__A) -Z4(__B) +Z4(__C) +Z0(_BC) -Z4(_CA) +Z4(_AB) +Z0(CEN)) * a_factor,
+                        (+Z4(__A) +Z0(__B) -Z4(__C) +Z4(_BC) +Z0(_CA) -Z4(_AB) +Z0(CEN)) * a_factor,
+                        (-Z4(__A) +Z4(__B) +Z0(__C) -Z4(_BC) +Z4(_CA) +Z0(_AB) +Z0(CEN)) * a_factor);
+          }
+          break;
+      }
+      delta_endstop_adj += e_delta;
+      delta_radius += r_delta;
+      delta_tower_angle_trim += t_delta;
+    }
+    else if (zero_std_dev >= test_precision) {
+      // roll back
+      delta_endstop_adj = e_old;
+      delta_radius = r_old;
+      delta_height = h_old;
+      delta_tower_angle_trim = a_old;
+    }
+
+    if (verbose_level != 0) {                                    // !dry run
+
+      // Normalize angles to least-squares
+      if (_angle_results) {
+        float a_sum = 0.0f;
+        LOOP_LINEAR_AXES(axis) a_sum += delta_tower_angle_trim[axis];
+        LOOP_LINEAR_AXES(axis) delta_tower_angle_trim[axis] -= a_sum / 3.0f;
+      }
+
+      // adjust delta_height and endstops by the max amount
+      const float z_temp = _MAX(delta_endstop_adj.a, delta_endstop_adj.b, delta_endstop_adj.c);
+      delta_height -= z_temp;
+      LOOP_LINEAR_AXES(axis) delta_endstop_adj[axis] -= z_temp;
+    }
+    recalc_delta_settings();
+    NOMORE(zero_std_dev_min, zero_std_dev);
+
+    // print report
+
+    if (verbose_level == 3 || verbose_level == 0)
+      print_calibration_results(z_at_pt, _tower_results, _opposite_results);
+
+    if (verbose_level != 0) { // !dry run
+      if ((zero_std_dev >= test_precision && iterations > force_iterations) || zero_std_dev <= calibration_precision) { // end iterations
+        SERIAL_ECHOPGM("Calibration OK");
+        SERIAL_ECHO_SP(32);
+        #if HAS_BED_PROBE
+          if (zero_std_dev >= test_precision && !_1p_calibration && !_0p_calibration)
+            SERIAL_ECHOPGM("rolling back.");
+          else
+        #endif
+          {
+            SERIAL_ECHOPAIR_F("std dev:", zero_std_dev_min, 3);
+          }
+        SERIAL_EOL();
+        char mess[21];
+        strcpy_P(mess, PSTR("Calibration sd:"));
+        if (zero_std_dev_min < 1)
+          sprintf_P(&mess[15], PSTR("0.%03i"), (int)LROUND(zero_std_dev_min * 1000.0f));
+        else
+          sprintf_P(&mess[15], PSTR("%03i.x"), (int)LROUND(zero_std_dev_min));
+        ui.set_status(mess);
+        print_calibration_settings(_endstop_results, _angle_results);
+        SERIAL_ECHOLNPGM("Save with M500 and/or copy to Configuration.h");
+      }
+      else { // !end iterations
+        char mess[15];
+        if (iterations < 31)
+          sprintf_P(mess, PSTR("Iteration : %02i"), (unsigned int)iterations);
+        else
+          strcpy_P(mess, PSTR("No convergence"));
+        SERIAL_ECHO(mess);
+        SERIAL_ECHO_SP(32);
+        SERIAL_ECHOLNPAIR_F("std dev:", zero_std_dev, 3);
+        ui.set_status(mess);
+        if (verbose_level > 1)
+          print_calibration_settings(_endstop_results, _angle_results);
+      }
+    }
+    else { // dry run
+      PGM_P const enddryrun = PSTR("End DRY-RUN");
+      SERIAL_ECHOPGM_P(enddryrun);
+      SERIAL_ECHO_SP(35);
+      SERIAL_ECHOLNPAIR_F("std dev:", zero_std_dev, 3);
+
+      char mess[21];
+      strcpy_P(mess, enddryrun);
+      strcpy_P(&mess[11], PSTR(" sd:"));
+      if (zero_std_dev < 1)
+        sprintf_P(&mess[15], PSTR("0.%03i"), (int)LROUND(zero_std_dev * 1000.0f));
+      else
+        sprintf_P(&mess[15], PSTR("%03i.x"), (int)LROUND(zero_std_dev));
+      ui.set_status(mess);
+    }
+    ac_home();
+  }
+  while (((zero_std_dev < test_precision && iterations < 31) || iterations <= force_iterations) && zero_std_dev > calibration_precision);
+
+  ac_cleanup(TERN_(HAS_MULTI_HOTEND, old_tool_index));
+
+  TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_IDLE));
+}
+
+#endif // DELTA_AUTO_CALIBRATION

Marlin/src/gcode/calibrate/G34.cpp

@@ -0,0 +1,157 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if ENABLED(MECHANICAL_GANTRY_CALIBRATION)
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+#include "../../module/stepper.h"
+#include "../../module/endstops.h"
+
+#if HAS_LEVELING
+  #include "../../feature/bedlevel/bedlevel.h"
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../../core/debug_out.h"
+
+void GcodeSuite::G34() {
+
+  // Home before the alignment procedure
+  home_if_needed();
+
+  TERN_(HAS_LEVELING, TEMPORARY_BED_LEVELING_STATE(false));
+
+  SET_SOFT_ENDSTOP_LOOSE(true);
+  TemporaryGlobalEndstopsState unlock_z(false);
+
+  #ifdef GANTRY_CALIBRATION_COMMANDS_PRE
+    gcode.process_subcommands_now_P(PSTR(GANTRY_CALIBRATION_COMMANDS_PRE));
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Sub Commands Processed");
+  #endif
+
+  #ifdef GANTRY_CALIBRATION_SAFE_POSITION
+    // Move XY to safe position
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Parking XY");
+    const xy_pos_t safe_pos = GANTRY_CALIBRATION_SAFE_POSITION;
+    do_blocking_move_to(safe_pos, MMM_TO_MMS(GANTRY_CALIBRATION_XY_PARK_FEEDRATE));
+  #endif
+
+  const float move_distance = parser.intval('Z', GANTRY_CALIBRATION_EXTRA_HEIGHT),
+              zbase = ENABLED(GANTRY_CALIBRATION_TO_MIN) ? Z_MIN_POS : Z_MAX_POS,
+              zpounce = zbase - move_distance, zgrind = zbase + move_distance;
+
+  // Move Z to pounce position
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Setting Z Pounce");
+  do_blocking_move_to_z(zpounce, homing_feedrate(Z_AXIS));
+
+  // Store current motor settings, then apply reduced value
+
+  #define _REDUCE_CURRENT ANY(HAS_MOTOR_CURRENT_SPI, HAS_MOTOR_CURRENT_PWM, HAS_MOTOR_CURRENT_DAC, HAS_MOTOR_CURRENT_I2C, HAS_TRINAMIC_CONFIG)
+  #if _REDUCE_CURRENT
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Reducing Current");
+  #endif
+
+  #if HAS_MOTOR_CURRENT_SPI
+    const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT);
+    const uint32_t previous_current = stepper.motor_current_setting[Z_AXIS];
+    stepper.set_digipot_current(Z_AXIS, target_current);
+  #elif HAS_MOTOR_CURRENT_PWM
+    const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT);
+    const uint32_t previous_current = stepper.motor_current_setting[Z_AXIS];
+    stepper.set_digipot_current(1, target_current);
+  #elif HAS_MOTOR_CURRENT_DAC
+    const float target_current = parser.floatval('S', GANTRY_CALIBRATION_CURRENT);
+    const float previous_current = dac_amps(Z_AXIS, target_current);
+    stepper_dac.set_current_value(Z_AXIS, target_current);
+  #elif HAS_MOTOR_CURRENT_I2C
+    const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT);
+    previous_current = dac_amps(Z_AXIS);
+    digipot_i2c.set_current(Z_AXIS, target_current)
+  #elif HAS_TRINAMIC_CONFIG
+    const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT);
+    static uint16_t previous_current_arr[NUM_Z_STEPPER_DRIVERS];
+    #if AXIS_IS_TMC(Z)
+      previous_current_arr[0] = stepperZ.getMilliamps();
+      stepperZ.rms_current(target_current);
+    #endif
+    #if AXIS_IS_TMC(Z2)
+      previous_current_arr[1] = stepperZ2.getMilliamps();
+      stepperZ2.rms_current(target_current);
+    #endif
+    #if AXIS_IS_TMC(Z3)
+      previous_current_arr[2] = stepperZ3.getMilliamps();
+      stepperZ3.rms_current(target_current);
+    #endif
+    #if AXIS_IS_TMC(Z4)
+      previous_current_arr[3] = stepperZ4.getMilliamps();
+      stepperZ4.rms_current(target_current);
+    #endif
+  #endif
+
+  // Do Final Z move to adjust
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Final Z Move");
+  do_blocking_move_to_z(zgrind, MMM_TO_MMS(GANTRY_CALIBRATION_FEEDRATE));
+
+  // Back off end plate, back to normal motion range
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Z Backoff");
+  do_blocking_move_to_z(zpounce, MMM_TO_MMS(GANTRY_CALIBRATION_FEEDRATE));
+
+  #if _REDUCE_CURRENT
+    // Reset current to original values
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Restore Current");
+  #endif
+
+  #if HAS_MOTOR_CURRENT_SPI
+    stepper.set_digipot_current(Z_AXIS, previous_current);
+  #elif HAS_MOTOR_CURRENT_PWM
+    stepper.set_digipot_current(1, previous_current);
+  #elif HAS_MOTOR_CURRENT_DAC
+    stepper_dac.set_current_value(Z_AXIS, previous_current);
+  #elif HAS_MOTOR_CURRENT_I2C
+    digipot_i2c.set_current(Z_AXIS, previous_current)
+  #elif HAS_TRINAMIC_CONFIG
+    #if AXIS_IS_TMC(Z)
+      stepperZ.rms_current(previous_current_arr[0]);
+    #endif
+    #if AXIS_IS_TMC(Z2)
+      stepperZ2.rms_current(previous_current_arr[1]);
+    #endif
+    #if AXIS_IS_TMC(Z3)
+      stepperZ3.rms_current(previous_current_arr[2]);
+    #endif
+    #if AXIS_IS_TMC(Z4)
+      stepperZ4.rms_current(previous_current_arr[3]);
+    #endif
+  #endif
+
+  #ifdef GANTRY_CALIBRATION_COMMANDS_POST
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Running Post Commands");
+    gcode.process_subcommands_now_P(PSTR(GANTRY_CALIBRATION_COMMANDS_POST));
+  #endif
+
+  SET_SOFT_ENDSTOP_LOOSE(false);
+}
+
+#endif // MECHANICAL_GANTRY_CALIBRATION

Marlin/src/gcode/calibrate/G34_M422.cpp

@@ -0,0 +1,548 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if EITHER(Z_MULTI_ENDSTOPS, Z_STEPPER_AUTO_ALIGN)
+
+#include "../../feature/z_stepper_align.h"
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+#include "../../module/stepper.h"
+#include "../../module/planner.h"
+#include "../../module/probe.h"
+#include "../../lcd/marlinui.h" // for LCD_MESSAGEPGM
+
+#if HAS_LEVELING
+  #include "../../feature/bedlevel/bedlevel.h"
+#endif
+
+#if HAS_MULTI_HOTEND
+  #include "../../module/tool_change.h"
+#endif
+
+#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+  #include "../../libs/least_squares_fit.h"
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../../core/debug_out.h"
+
+#if NUM_Z_STEPPER_DRIVERS >= 3
+  #define TRIPLE_Z 1
+  #if NUM_Z_STEPPER_DRIVERS >= 4
+    #define QUAD_Z 1
+  #endif
+#endif
+
+/**
+ * G34: Z-Stepper automatic alignment
+ *
+ * Manual stepper lock controls (reset by G28):
+ *   L                 Unlock all steppers
+ *   Z<1-4>            Z stepper to lock / unlock
+ *   S<state>          0=UNLOCKED 1=LOCKED. If omitted, assume LOCKED.
+ *
+ *   Examples:
+ *     G34 Z1     ; Lock Z1
+ *     G34 L Z2   ; Unlock all, then lock Z2
+ *     G34 Z2 S0  ; Unlock Z2
+ *
+ * With Z_STEPPER_AUTO_ALIGN:
+ *   I<iterations>     Number of tests. If omitted, Z_STEPPER_ALIGN_ITERATIONS.
+ *   T<accuracy>       Target Accuracy factor. If omitted, Z_STEPPER_ALIGN_ACC.
+ *   A<amplification>  Provide an Amplification value. If omitted, Z_STEPPER_ALIGN_AMP.
+ *   R                 Flag to recalculate points based on current probe offsets
+ */
+void GcodeSuite::G34() {
+  DEBUG_SECTION(log_G34, "G34", DEBUGGING(LEVELING));
+  if (DEBUGGING(LEVELING)) log_machine_info();
+
+  planner.synchronize();  // Prevent damage
+
+  const bool seenL = parser.seen('L');
+  if (seenL) stepper.set_all_z_lock(false);
+
+  const bool seenZ = parser.seenval('Z');
+  if (seenZ) {
+    const bool state = parser.boolval('S', true);
+    switch (parser.intval('Z')) {
+      case 1: stepper.set_z1_lock(state); break;
+      case 2: stepper.set_z2_lock(state); break;
+      #if TRIPLE_Z
+        case 3: stepper.set_z3_lock(state); break;
+        #if QUAD_Z
+          case 4: stepper.set_z4_lock(state); break;
+        #endif
+      #endif
+    }
+  }
+
+  if (seenL || seenZ) {
+    stepper.set_separate_multi_axis(seenZ);
+    return;
+  }
+
+  #if ENABLED(Z_STEPPER_AUTO_ALIGN)
+    do { // break out on error
+
+      const int8_t z_auto_align_iterations = parser.intval('I', Z_STEPPER_ALIGN_ITERATIONS);
+      if (!WITHIN(z_auto_align_iterations, 1, 30)) {
+        SERIAL_ECHOLNPGM("?(I)teration out of bounds (1-30).");
+        break;
+      }
+
+      const float z_auto_align_accuracy = parser.floatval('T', Z_STEPPER_ALIGN_ACC);
+      if (!WITHIN(z_auto_align_accuracy, 0.01f, 1.0f)) {
+        SERIAL_ECHOLNPGM("?(T)arget accuracy out of bounds (0.01-1.0).");
+        break;
+      }
+
+      const float z_auto_align_amplification = TERN(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS, Z_STEPPER_ALIGN_AMP, parser.floatval('A', Z_STEPPER_ALIGN_AMP));
+      if (!WITHIN(ABS(z_auto_align_amplification), 0.5f, 2.0f)) {
+        SERIAL_ECHOLNPGM("?(A)mplification out of bounds (0.5-2.0).");
+        break;
+      }
+
+      if (parser.seen('R')) z_stepper_align.reset_to_default();
+
+      const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE;
+
+      // Disable the leveling matrix before auto-aligning
+      #if HAS_LEVELING
+        #if ENABLED(RESTORE_LEVELING_AFTER_G34)
+          const bool leveling_was_active = planner.leveling_active;
+        #endif
+        set_bed_leveling_enabled(false);
+      #endif
+
+      TERN_(CNC_WORKSPACE_PLANES, workspace_plane = PLANE_XY);
+
+      // Always home with tool 0 active
+      #if HAS_MULTI_HOTEND
+        const uint8_t old_tool_index = active_extruder;
+        tool_change(0, true);
+      #endif
+
+      TERN_(HAS_DUPLICATION_MODE, set_duplication_enabled(false));
+
+      // In BLTOUCH HS mode, the probe travels in a deployed state.
+      // Users of G34 might have a badly misaligned bed, so raise Z by the
+      // length of the deployed pin (BLTOUCH stroke < 7mm)
+      #define Z_BASIC_CLEARANCE (Z_CLEARANCE_BETWEEN_PROBES + 7.0f * BOTH(BLTOUCH, BLTOUCH_HS_MODE))
+
+      // Compute a worst-case clearance height to probe from. After the first
+      // iteration this will be re-calculated based on the actual bed position
+      auto magnitude2 = [&](const uint8_t i, const uint8_t j) {
+        const xy_pos_t diff = z_stepper_align.xy[i] - z_stepper_align.xy[j];
+        return HYPOT2(diff.x, diff.y);
+      };
+      float z_probe = Z_BASIC_CLEARANCE + (G34_MAX_GRADE) * 0.01f * SQRT(_MAX(0, magnitude2(0, 1)
+        #if TRIPLE_Z
+          , magnitude2(2, 1), magnitude2(2, 0)
+          #if QUAD_Z
+            , magnitude2(3, 2), magnitude2(3, 1), magnitude2(3, 0)
+          #endif
+        #endif
+      ));
+
+      // Home before the alignment procedure
+      home_if_needed();
+
+      // Move the Z coordinate realm towards the positive - dirty trick
+      current_position.z += z_probe * 0.5f;
+      sync_plan_position();
+      // Now, the Z origin lies below the build plate. That allows to probe deeper, before run_z_probe throws an error.
+      // This hack is un-done at the end of G34 - either by re-homing, or by using the probed heights of the last iteration.
+
+      #if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+        float last_z_align_move[NUM_Z_STEPPER_DRIVERS] = ARRAY_N_1(NUM_Z_STEPPER_DRIVERS, 10000.0f);
+      #else
+        float last_z_align_level_indicator = 10000.0f;
+      #endif
+      float z_measured[NUM_Z_STEPPER_DRIVERS] = { 0 },
+            z_maxdiff = 0.0f,
+            amplification = z_auto_align_amplification;
+
+      #if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+        bool adjustment_reverse = false;
+      #endif
+
+      #if HAS_STATUS_MESSAGE
+        PGM_P const msg_iteration = GET_TEXT(MSG_ITERATION);
+        const uint8_t iter_str_len = strlen_P(msg_iteration);
+      #endif
+
+      // Final z and iteration values will be used after breaking the loop
+      float z_measured_min;
+      uint8_t iteration = 0;
+      bool err_break = false; // To break out of nested loops
+      while (iteration < z_auto_align_iterations) {
+        if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> probing all positions.");
+
+        const int iter = iteration + 1;
+        SERIAL_ECHOLNPAIR("\nG34 Iteration: ", iter);
+        #if HAS_STATUS_MESSAGE
+          char str[iter_str_len + 2 + 1];
+          sprintf_P(str, msg_iteration, iter);
+          ui.set_status(str);
+        #endif
+
+        // Initialize minimum value
+        z_measured_min =  100000.0f;
+        float z_measured_max = -100000.0f;
+
+        // Probe all positions (one per Z-Stepper)
+        LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) {
+          // iteration odd/even --> downward / upward stepper sequence
+          const uint8_t iprobe = (iteration & 1) ? NUM_Z_STEPPER_DRIVERS - 1 - i : i;
+
+          // Safe clearance even on an incline
+          if ((iteration == 0 || i > 0) && z_probe > current_position.z) do_blocking_move_to_z(z_probe);
+
+          if (DEBUGGING(LEVELING))
+            DEBUG_ECHOLNPAIR_P(PSTR("Probing X"), z_stepper_align.xy[iprobe].x, SP_Y_STR, z_stepper_align.xy[iprobe].y);
+
+          // Probe a Z height for each stepper.
+          // Probing sanity check is disabled, as it would trigger even in normal cases because
+          // current_position.z has been manually altered in the "dirty trick" above.
+          const float z_probed_height = probe.probe_at_point(z_stepper_align.xy[iprobe], raise_after, 0, true, false);
+          if (isnan(z_probed_height)) {
+            SERIAL_ECHOLNPGM("Probing failed");
+            LCD_MESSAGEPGM(MSG_LCD_PROBING_FAILED);
+            err_break = true;
+            break;
+          }
+
+          // Add height to each value, to provide a more useful target height for
+          // the next iteration of probing. This allows adjustments to be made away from the bed.
+          z_measured[iprobe] = z_probed_height + Z_CLEARANCE_BETWEEN_PROBES;
+
+          if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", iprobe + 1, " measured position is ", z_measured[iprobe]);
+
+          // Remember the minimum measurement to calculate the correction later on
+          z_measured_min = _MIN(z_measured_min, z_measured[iprobe]);
+          z_measured_max = _MAX(z_measured_max, z_measured[iprobe]);
+        } // for (i)
+
+        if (err_break) break;
+
+        // Adapt the next probe clearance height based on the new measurements.
+        // Safe_height = lowest distance to bed (= highest measurement) plus highest measured misalignment.
+        z_maxdiff = z_measured_max - z_measured_min;
+        z_probe = Z_BASIC_CLEARANCE + z_measured_max + z_maxdiff;
+
+        #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+          // Replace the initial values in z_measured with calculated heights at
+          // each stepper position. This allows the adjustment algorithm to be
+          // shared between both possible probing mechanisms.
+
+          // This must be done after the next z_probe height is calculated, so that
+          // the height is calculated from actual print area positions, and not
+          // extrapolated motor movements.
+
+          // Compute the least-squares fit for all probed points.
+          // Calculate the Z position of each stepper and store it in z_measured.
+          // This allows the actual adjustment logic to be shared by both algorithms.
+          linear_fit_data lfd;
+          incremental_LSF_reset(&lfd);
+          LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) {
+            SERIAL_ECHOLNPAIR("PROBEPT_", i, ": ", z_measured[i]);
+            incremental_LSF(&lfd, z_stepper_align.xy[i], z_measured[i]);
+          }
+          finish_incremental_LSF(&lfd);
+
+          z_measured_min = 100000.0f;
+          LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) {
+            z_measured[i] = -(lfd.A * z_stepper_align.stepper_xy[i].x + lfd.B * z_stepper_align.stepper_xy[i].y + lfd.D);
+            z_measured_min = _MIN(z_measured_min, z_measured[i]);
+          }
+
+          SERIAL_ECHOLNPAIR(
+            LIST_N(DOUBLE(NUM_Z_STEPPER_DRIVERS),
+              "Calculated Z1=", z_measured[0],
+                        " Z2=", z_measured[1],
+                        " Z3=", z_measured[2],
+                        " Z4=", z_measured[3]
+            )
+          );
+        #endif
+
+        SERIAL_ECHOLNPAIR("\n"
+          "Z2-Z1=", ABS(z_measured[1] - z_measured[0])
+          #if TRIPLE_Z
+            , " Z3-Z2=", ABS(z_measured[2] - z_measured[1])
+            , " Z3-Z1=", ABS(z_measured[2] - z_measured[0])
+            #if QUAD_Z
+              , " Z4-Z3=", ABS(z_measured[3] - z_measured[2])
+              , " Z4-Z2=", ABS(z_measured[3] - z_measured[1])
+              , " Z4-Z1=", ABS(z_measured[3] - z_measured[0])
+            #endif
+          #endif
+        );
+
+        #if HAS_STATUS_MESSAGE
+          char fstr1[10];
+          char msg[6 + (6 + 5) * NUM_Z_STEPPER_DRIVERS + 1]
+            #if TRIPLE_Z
+              , fstr2[10], fstr3[10]
+              #if QUAD_Z
+                , fstr4[10], fstr5[10], fstr6[10]
+              #endif
+            #endif
+          ;
+          sprintf_P(msg,
+            PSTR("1:2=%s" TERN_(TRIPLE_Z, " 3-2=%s 3-1=%s") TERN_(QUAD_Z, " 4-3=%s 4-2=%s 4-1=%s")),
+            dtostrf(ABS(z_measured[1] - z_measured[0]), 1, 3, fstr1)
+            OPTARG(TRIPLE_Z,
+              dtostrf(ABS(z_measured[2] - z_measured[1]), 1, 3, fstr2),
+              dtostrf(ABS(z_measured[2] - z_measured[0]), 1, 3, fstr3))
+            OPTARG(QUAD_Z,
+              dtostrf(ABS(z_measured[3] - z_measured[2]), 1, 3, fstr4),
+              dtostrf(ABS(z_measured[3] - z_measured[1]), 1, 3, fstr5),
+              dtostrf(ABS(z_measured[3] - z_measured[0]), 1, 3, fstr6))
+          );
+          ui.set_status(msg);
+        #endif
+
+        auto decreasing_accuracy = [](const_float_t v1, const_float_t v2) {
+          if (v1 < v2 * 0.7f) {
+            SERIAL_ECHOLNPGM("Decreasing Accuracy Detected.");
+            LCD_MESSAGEPGM(MSG_DECREASING_ACCURACY);
+            return true;
+          }
+          return false;
+        };
+
+        #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+          // Check if the applied corrections go in the correct direction.
+          // Calculate the sum of the absolute deviations from the mean of the probe measurements.
+          // Compare to the last iteration to ensure it's getting better.
+
+          // Calculate mean value as a reference
+          float z_measured_mean = 0.0f;
+          LOOP_L_N(zstepper, NUM_Z_STEPPER_DRIVERS) z_measured_mean += z_measured[zstepper];
+          z_measured_mean /= NUM_Z_STEPPER_DRIVERS;
+
+          // Calculate the sum of the absolute deviations from the mean value
+          float z_align_level_indicator = 0.0f;
+          LOOP_L_N(zstepper, NUM_Z_STEPPER_DRIVERS)
+            z_align_level_indicator += ABS(z_measured[zstepper] - z_measured_mean);
+
+          // If it's getting worse, stop and throw an error
+          err_break = decreasing_accuracy(last_z_align_level_indicator, z_align_level_indicator);
+          if (err_break) break;
+
+          last_z_align_level_indicator = z_align_level_indicator;
+        #endif
+
+        // The following correction actions are to be enabled for select Z-steppers only
+        stepper.set_separate_multi_axis(true);
+
+        bool success_break = true;
+        // Correct the individual stepper offsets
+        LOOP_L_N(zstepper, NUM_Z_STEPPER_DRIVERS) {
+          // Calculate current stepper move
+          float z_align_move = z_measured[zstepper] - z_measured_min;
+          const float z_align_abs = ABS(z_align_move);
+
+          #if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+            // Optimize one iteration's correction based on the first measurements
+            if (z_align_abs) amplification = (iteration == 1) ? _MIN(last_z_align_move[zstepper] / z_align_abs, 2.0f) : z_auto_align_amplification;
+
+            // Check for less accuracy compared to last move
+            if (decreasing_accuracy(last_z_align_move[zstepper], z_align_abs)) {
+              if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", zstepper + 1, " last_z_align_move = ", last_z_align_move[zstepper]);
+              if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", zstepper + 1, " z_align_abs = ", z_align_abs);
+              adjustment_reverse = !adjustment_reverse;
+            }
+
+            // Remember the alignment for the next iteration, but only if steppers move,
+            // otherwise it would be just zero (in case this stepper was at z_measured_min already)
+            if (z_align_abs > 0) last_z_align_move[zstepper] = z_align_abs;
+          #endif
+
+          // Stop early if all measured points achieve accuracy target
+          if (z_align_abs > z_auto_align_accuracy) success_break = false;
+
+          if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", zstepper + 1, " corrected by ", z_align_move);
+
+          // Lock all steppers except one
+          stepper.set_all_z_lock(true, zstepper);
+
+          #if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+            // Decreasing accuracy was detected so move was inverted.
+            // Will match reversed Z steppers on dual steppers. Triple will need more work to map.
+            if (adjustment_reverse) {
+              z_align_move = -z_align_move;
+              if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", zstepper + 1, " correction reversed to ", z_align_move);
+            }
+          #endif
+
+          // Do a move to correct part of the misalignment for the current stepper
+          do_blocking_move_to_z(amplification * z_align_move + current_position.z);
+        } // for (zstepper)
+
+        // Back to normal stepper operations
+        stepper.set_all_z_lock(false);
+        stepper.set_separate_multi_axis(false);
+
+        if (err_break) break;
+
+        if (success_break) {
+          SERIAL_ECHOLNPGM("Target accuracy achieved.");
+          LCD_MESSAGEPGM(MSG_ACCURACY_ACHIEVED);
+          break;
+        }
+
+        iteration++;
+      } // while (iteration < z_auto_align_iterations)
+
+      if (err_break)
+        SERIAL_ECHOLNPGM("G34 aborted.");
+      else {
+        SERIAL_ECHOLNPAIR("Did ", iteration + (iteration != z_auto_align_iterations), " of ", z_auto_align_iterations);
+        SERIAL_ECHOLNPAIR_F("Accuracy: ", z_maxdiff);
+      }
+
+      // Stow the probe because the last call to probe.probe_at_point(...)
+      // leaves the probe deployed when it's successful.
+      IF_DISABLED(TOUCH_MI_PROBE, probe.stow());
+
+      #if ENABLED(HOME_AFTER_G34)
+        // After this operation the z position needs correction
+        set_axis_never_homed(Z_AXIS);
+        // Home Z after the alignment procedure
+        process_subcommands_now_P(PSTR("G28Z"));
+      #else
+        // Use the probed height from the last iteration to determine the Z height.
+        // z_measured_min is used, because all steppers are aligned to z_measured_min.
+        // Ideally, this would be equal to the 'z_probe * 0.5f' which was added earlier.
+        current_position.z -= z_measured_min - (float)Z_CLEARANCE_BETWEEN_PROBES;
+        sync_plan_position();
+      #endif
+
+      // Restore the active tool after homing
+      TERN_(HAS_MULTI_HOTEND, tool_change(old_tool_index, DISABLED(PARKING_EXTRUDER))); // Fetch previous tool for parking extruder
+
+      #if BOTH(HAS_LEVELING, RESTORE_LEVELING_AFTER_G34)
+        set_bed_leveling_enabled(leveling_was_active);
+      #endif
+
+    }while(0);
+  #endif // Z_STEPPER_AUTO_ALIGN
+}
+
+#endif // Z_MULTI_ENDSTOPS || Z_STEPPER_AUTO_ALIGN
+
+#if ENABLED(Z_STEPPER_AUTO_ALIGN)
+
+/**
+ * M422: Set a Z-Stepper automatic alignment XY point.
+ *       Use repeatedly to set multiple points.
+ *
+ *   S<index> : Index of the probe point to set
+ *
+ * With Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS:
+ *   W<index> : Index of the Z stepper position to set
+ *              The W and S parameters may not be combined.
+ *
+ * S and W require an X and/or Y parameter
+ *   X<pos>   : X position to set (Unchanged if omitted)
+ *   Y<pos>   : Y position to set (Unchanged if omitted)
+ *
+ * R : Recalculate points based on current probe offsets
+ */
+void GcodeSuite::M422() {
+
+  if (parser.seen('R')) {
+    z_stepper_align.reset_to_default();
+    return;
+  }
+
+  if (!parser.seen_any()) {
+    LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS)
+      SERIAL_ECHOLNPAIR_P(PSTR("M422 S"), i + 1, SP_X_STR, z_stepper_align.xy[i].x, SP_Y_STR, z_stepper_align.xy[i].y);
+    #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+      LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS)
+        SERIAL_ECHOLNPAIR_P(PSTR("M422 W"), i + 1, SP_X_STR, z_stepper_align.stepper_xy[i].x, SP_Y_STR, z_stepper_align.stepper_xy[i].y);
+    #endif
+    return;
+  }
+
+  const bool is_probe_point = parser.seen('S');
+
+  if (TERN0(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS, is_probe_point && parser.seen('W'))) {
+    SERIAL_ECHOLNPGM("?(S) and (W) may not be combined.");
+    return;
+  }
+
+  xy_pos_t *pos_dest = (
+    TERN_(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS, !is_probe_point ? z_stepper_align.stepper_xy :)
+    z_stepper_align.xy
+  );
+
+  if (!is_probe_point && TERN1(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS, !parser.seen('W'))) {
+    SERIAL_ECHOLNPGM("?(S)" TERN_(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS, " or (W)") " is required.");
+    return;
+  }
+
+  // Get the Probe Position Index or Z Stepper Index
+  int8_t position_index;
+  if (is_probe_point) {
+    position_index = parser.intval('S') - 1;
+    if (!WITHIN(position_index, 0, int8_t(NUM_Z_STEPPER_DRIVERS) - 1)) {
+      SERIAL_ECHOLNPGM("?(S) Probe-position index invalid.");
+      return;
+    }
+  }
+  else {
+    #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+      position_index = parser.intval('W') - 1;
+      if (!WITHIN(position_index, 0, NUM_Z_STEPPER_DRIVERS - 1)) {
+        SERIAL_ECHOLNPGM("?(W) Z-stepper index invalid.");
+        return;
+      }
+    #endif
+  }
+
+  const xy_pos_t pos = {
+    parser.floatval('X', pos_dest[position_index].x),
+    parser.floatval('Y', pos_dest[position_index].y)
+  };
+
+  if (is_probe_point) {
+    if (!probe.can_reach(pos.x, Y_CENTER)) {
+      SERIAL_ECHOLNPGM("?(X) out of bounds.");
+      return;
+    }
+    if (!probe.can_reach(pos)) {
+      SERIAL_ECHOLNPGM("?(Y) out of bounds.");
+      return;
+    }
+  }
+
+  pos_dest[position_index] = pos;
+}
+
+#endif // Z_STEPPER_AUTO_ALIGN

Marlin/src/gcode/calibrate/G425.cpp

@@ -0,0 +1,752 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../MarlinCore.h"
+
+#if ENABLED(CALIBRATION_GCODE)
+
+#include "../gcode.h"
+
+#if ENABLED(BACKLASH_GCODE)
+  #include "../../feature/backlash.h"
+#endif
+
+#include "../../lcd/marlinui.h"
+#include "../../module/motion.h"
+#include "../../module/planner.h"
+#include "../../module/tool_change.h"
+#include "../../module/endstops.h"
+#include "../../feature/bedlevel/bedlevel.h"
+
+#if !AXIS_CAN_CALIBRATE(X)
+  #undef CALIBRATION_MEASURE_LEFT
+  #undef CALIBRATION_MEASURE_RIGHT
+#endif
+
+#if !AXIS_CAN_CALIBRATE(Y)
+  #undef CALIBRATION_MEASURE_FRONT
+  #undef CALIBRATION_MEASURE_BACK
+#endif
+
+#if !AXIS_CAN_CALIBRATE(Z)
+  #undef CALIBRATION_MEASURE_AT_TOP_EDGES
+#endif
+
+/**
+ * G425 backs away from the calibration object by various distances
+ * depending on the confidence level:
+ *
+ *   UNKNOWN   - No real notion on where the calibration object is on the bed
+ *   UNCERTAIN - Measurement may be uncertain due to backlash
+ *   CERTAIN   - Measurement obtained with backlash compensation
+ */
+
+#ifndef CALIBRATION_MEASUREMENT_UNKNOWN
+  #define CALIBRATION_MEASUREMENT_UNKNOWN   5.0 // mm
+#endif
+#ifndef CALIBRATION_MEASUREMENT_UNCERTAIN
+  #define CALIBRATION_MEASUREMENT_UNCERTAIN 1.0 // mm
+#endif
+#ifndef CALIBRATION_MEASUREMENT_CERTAIN
+  #define CALIBRATION_MEASUREMENT_CERTAIN   0.5 // mm
+#endif
+
+#if BOTH(CALIBRATION_MEASURE_LEFT, CALIBRATION_MEASURE_RIGHT)
+  #define HAS_X_CENTER 1
+#endif
+#if HAS_Y_AXIS && BOTH(CALIBRATION_MEASURE_FRONT, CALIBRATION_MEASURE_BACK)
+  #define HAS_Y_CENTER 1
+#endif
+#if LINEAR_AXES >= 4 && BOTH(CALIBRATION_MEASURE_IMIN, CALIBRATION_MEASURE_IMAX)
+  #define HAS_I_CENTER 1
+#endif
+#if LINEAR_AXES >= 5 && BOTH(CALIBRATION_MEASURE_JMIN, CALIBRATION_MEASURE_JMAX)
+  #define HAS_J_CENTER 1
+#endif
+#if LINEAR_AXES >= 6 && BOTH(CALIBRATION_MEASURE_KMIN, CALIBRATION_MEASURE_KMAX)
+  #define HAS_K_CENTER 1
+#endif
+
+enum side_t : uint8_t {
+  TOP, RIGHT, FRONT, LEFT, BACK, NUM_SIDES,
+  LIST_N(DOUBLE(SUB3(LINEAR_AXES)), IMINIMUM, IMAXIMUM, JMINIMUM, JMAXIMUM, KMINIMUM, KMAXIMUM)
+};
+
+static constexpr xyz_pos_t true_center CALIBRATION_OBJECT_CENTER;
+static constexpr xyz_float_t dimensions CALIBRATION_OBJECT_DIMENSIONS;
+static constexpr xy_float_t nod = { CALIBRATION_NOZZLE_OUTER_DIAMETER, CALIBRATION_NOZZLE_OUTER_DIAMETER };
+
+struct measurements_t {
+  xyz_pos_t obj_center = true_center; // Non-static must be assigned from xyz_pos_t
+
+  float obj_side[NUM_SIDES], backlash[NUM_SIDES];
+  xyz_float_t pos_error;
+
+  xy_float_t nozzle_outer_dimension = nod;
+};
+
+#if ENABLED(BACKLASH_GCODE)
+  #define TEMPORARY_BACKLASH_CORRECTION(value) REMEMBER(tbst, backlash.correction, value)
+#else
+  #define TEMPORARY_BACKLASH_CORRECTION(value)
+#endif
+
+#if ENABLED(BACKLASH_GCODE) && defined(BACKLASH_SMOOTHING_MM)
+  #define TEMPORARY_BACKLASH_SMOOTHING(value) REMEMBER(tbsm, backlash.smoothing_mm, value)
+#else
+  #define TEMPORARY_BACKLASH_SMOOTHING(value)
+#endif
+
+inline void calibration_move() {
+  do_blocking_move_to((xyz_pos_t)current_position, MMM_TO_MMS(CALIBRATION_FEEDRATE_TRAVEL));
+}
+
+/**
+ * Move to the exact center above the calibration object
+ *
+ *   m                  in     - Measurement record
+ *   uncertainty        in     - How far away from the object top to park
+ */
+inline void park_above_object(measurements_t &m, const float uncertainty) {
+  // Move to safe distance above calibration object
+  current_position.z = m.obj_center.z + dimensions.z / 2 + uncertainty;
+  calibration_move();
+
+  // Move to center of calibration object in XY
+  current_position = xy_pos_t(m.obj_center);
+  calibration_move();
+}
+
+#if HAS_MULTI_HOTEND
+  inline void set_nozzle(measurements_t &m, const uint8_t extruder) {
+    if (extruder != active_extruder) {
+      park_above_object(m, CALIBRATION_MEASUREMENT_UNKNOWN);
+      tool_change(extruder);
+    }
+  }
+#endif
+
+#if HAS_HOTEND_OFFSET
+
+  inline void normalize_hotend_offsets() {
+    LOOP_S_L_N(e, 1, HOTENDS)
+      hotend_offset[e] -= hotend_offset[0];
+    hotend_offset[0].reset();
+  }
+
+#endif
+
+#if !PIN_EXISTS(CALIBRATION)
+  #include "../../module/probe.h"
+#endif
+
+inline bool read_calibration_pin() {
+  return (
+    #if PIN_EXISTS(CALIBRATION)
+      READ(CALIBRATION_PIN) != CALIBRATION_PIN_INVERTING
+    #else
+      PROBE_TRIGGERED()
+    #endif
+  );
+}
+
+/**
+ * Move along axis in the specified dir until the probe value becomes stop_state,
+ * then return the axis value.
+ *
+ *   axis         in - Axis along which the measurement will take place
+ *   dir          in - Direction along that axis (-1 or 1)
+ *   stop_state   in - Move until probe pin becomes this value
+ *   fast         in - Fast vs. precise measurement
+ */
+float measuring_movement(const AxisEnum axis, const int dir, const bool stop_state, const bool fast) {
+  const float step     = fast ? 0.25 : CALIBRATION_MEASUREMENT_RESOLUTION;
+  const feedRate_t mms = fast ? MMM_TO_MMS(CALIBRATION_FEEDRATE_FAST) : MMM_TO_MMS(CALIBRATION_FEEDRATE_SLOW);
+  const float limit    = fast ? 50 : 5;
+
+  destination = current_position;
+  for (float travel = 0; travel < limit; travel += step) {
+    destination[axis] += dir * step;
+    do_blocking_move_to((xyz_pos_t)destination, mms);
+    planner.synchronize();
+    if (read_calibration_pin() == stop_state) break;
+  }
+  return destination[axis];
+}
+
+/**
+ * Move along axis until the probe is triggered. Move toolhead to its starting
+ * point and return the measured value.
+ *
+ *   axis               in     - Axis along which the measurement will take place
+ *   dir                in     - Direction along that axis (-1 or 1)
+ *   stop_state         in     - Move until probe pin becomes this value
+ *   backlash_ptr       in/out - When not nullptr, measure and record axis backlash
+ *   uncertainty        in     - If uncertainty is CALIBRATION_MEASUREMENT_UNKNOWN, do a fast probe.
+ */
+inline float measure(const AxisEnum axis, const int dir, const bool stop_state, float * const backlash_ptr, const float uncertainty) {
+  const bool fast = uncertainty == CALIBRATION_MEASUREMENT_UNKNOWN;
+
+  // Save the current position of the specified axis
+  const float start_pos = current_position[axis];
+
+  // Take a measurement. Only the specified axis will be affected.
+  const float measured_pos = measuring_movement(axis, dir, stop_state, fast);
+
+  // Measure backlash
+  if (backlash_ptr && !fast) {
+    const float release_pos = measuring_movement(axis, -dir, !stop_state, fast);
+    *backlash_ptr = ABS(release_pos - measured_pos);
+  }
+
+  // Move back to the starting position
+  destination = current_position;
+  destination[axis] = start_pos;
+  do_blocking_move_to((xyz_pos_t)destination, MMM_TO_MMS(CALIBRATION_FEEDRATE_TRAVEL));
+  return measured_pos;
+}
+
+/**
+ * Probe one side of the calibration object
+ *
+ *   m                  in/out - Measurement record, m.obj_center and m.obj_side will be updated.
+ *   uncertainty        in     - How far away from the calibration object to begin probing
+ *   side               in     - Side of probe where probe will occur
+ *   probe_top_at_edge  in     - When probing sides, probe top of calibration object nearest edge
+ *                               to find out height of edge
+ */
+inline void probe_side(measurements_t &m, const float uncertainty, const side_t side, const bool probe_top_at_edge=false) {
+  const xyz_float_t dimensions = CALIBRATION_OBJECT_DIMENSIONS;
+  AxisEnum axis;
+  float dir = 1;
+
+  park_above_object(m, uncertainty);
+
+  switch (side) {
+    #if AXIS_CAN_CALIBRATE(X)
+      case RIGHT: dir = -1;
+      case LEFT:  axis = X_AXIS; break;
+    #endif
+    #if LINEAR_AXES >= 2 && AXIS_CAN_CALIBRATE(Y)
+      case BACK:  dir = -1;
+      case FRONT: axis = Y_AXIS; break;
+    #endif
+    #if HAS_Z_AXIS && AXIS_CAN_CALIBRATE(Z)
+      case TOP: {
+        const float measurement = measure(Z_AXIS, -1, true, &m.backlash[TOP], uncertainty);
+        m.obj_center.z = measurement - dimensions.z / 2;
+        m.obj_side[TOP] = measurement;
+        return;
+      }
+    #endif
+    #if LINEAR_AXES >= 4 && AXIS_CAN_CALIBRATE(I)
+      case IMINIMUM: dir = -1;
+      case IMAXIMUM: axis = I_AXIS; break;
+    #endif
+    #if LINEAR_AXES >= 5 && AXIS_CAN_CALIBRATE(J)
+      case JMINIMUM: dir = -1;
+      case JMAXIMUM: axis = J_AXIS; break;
+    #endif
+    #if LINEAR_AXES >= 6 && AXIS_CAN_CALIBRATE(K)
+      case KMINIMUM: dir = -1;
+      case KMAXIMUM: axis = K_AXIS; break;
+    #endif
+    default: return;
+  }
+
+  if (probe_top_at_edge) {
+    #if AXIS_CAN_CALIBRATE(Z)
+      // Probe top nearest the side we are probing
+      current_position[axis] = m.obj_center[axis] + (-dir) * (dimensions[axis] / 2 - m.nozzle_outer_dimension[axis]);
+      calibration_move();
+      m.obj_side[TOP] = measure(Z_AXIS, -1, true, &m.backlash[TOP], uncertainty);
+      m.obj_center.z = m.obj_side[TOP] - dimensions.z / 2;
+    #endif
+  }
+
+  if ((AXIS_CAN_CALIBRATE(X) && axis == X_AXIS) || (AXIS_CAN_CALIBRATE(Y) && axis == Y_AXIS)) {
+    // Move to safe distance to the side of the calibration object
+    current_position[axis] = m.obj_center[axis] + (-dir) * (dimensions[axis] / 2 + m.nozzle_outer_dimension[axis] / 2 + uncertainty);
+    calibration_move();
+
+    // Plunge below the side of the calibration object and measure
+    current_position.z = m.obj_side[TOP] - (CALIBRATION_NOZZLE_TIP_HEIGHT) * 0.7f;
+    calibration_move();
+    const float measurement = measure(axis, dir, true, &m.backlash[side], uncertainty);
+    m.obj_center[axis] = measurement + dir * (dimensions[axis] / 2 + m.nozzle_outer_dimension[axis] / 2);
+    m.obj_side[side] = measurement;
+  }
+}
+
+/**
+ * Probe all sides of the calibration calibration object
+ *
+ *   m                  in/out - Measurement record: center, backlash and error values be updated.
+ *   uncertainty        in     - How far away from the calibration object to begin probing
+ */
+inline void probe_sides(measurements_t &m, const float uncertainty) {
+  #if ENABLED(CALIBRATION_MEASURE_AT_TOP_EDGES)
+    constexpr bool probe_top_at_edge = true;
+  #else
+    // Probing at the exact center only works if the center is flat. Probing on a washer
+    // or bolt will require probing the top near the side edges, away from the center.
+    constexpr bool probe_top_at_edge = false;
+    probe_side(m, uncertainty, TOP);
+  #endif
+
+  TERN_(CALIBRATION_MEASURE_RIGHT, probe_side(m, uncertainty, RIGHT,    probe_top_at_edge));
+  TERN_(CALIBRATION_MEASURE_FRONT, probe_side(m, uncertainty, FRONT,    probe_top_at_edge));
+  TERN_(CALIBRATION_MEASURE_LEFT,  probe_side(m, uncertainty, LEFT,     probe_top_at_edge));
+  TERN_(CALIBRATION_MEASURE_BACK,  probe_side(m, uncertainty, BACK,     probe_top_at_edge));
+  TERN_(CALIBRATION_MEASURE_IMIN,  probe_side(m, uncertainty, IMINIMUM, probe_top_at_edge));
+  TERN_(CALIBRATION_MEASURE_IMAX,  probe_side(m, uncertainty, IMAXIMUM, probe_top_at_edge));
+  TERN_(CALIBRATION_MEASURE_JMIN,  probe_side(m, uncertainty, JMINIMUM, probe_top_at_edge));
+  TERN_(CALIBRATION_MEASURE_JMAX,  probe_side(m, uncertainty, JMAXIMUM, probe_top_at_edge));
+  TERN_(CALIBRATION_MEASURE_KMIN,  probe_side(m, uncertainty, KMINIMUM, probe_top_at_edge));
+  TERN_(CALIBRATION_MEASURE_KMAX,  probe_side(m, uncertainty, KMAXIMUM, probe_top_at_edge));
+
+  // Compute the measured center of the calibration object.
+  TERN_(HAS_X_CENTER, m.obj_center.x = (m.obj_side[LEFT]     + m.obj_side[RIGHT])    / 2);
+  TERN_(HAS_Y_CENTER, m.obj_center.y = (m.obj_side[FRONT]    + m.obj_side[BACK])     / 2);
+  TERN_(HAS_I_CENTER, m.obj_center.i = (m.obj_side[IMINIMUM] + m.obj_side[IMAXIMUM]) / 2);
+  TERN_(HAS_J_CENTER, m.obj_center.j = (m.obj_side[JMINIMUM] + m.obj_side[JMAXIMUM]) / 2);
+  TERN_(HAS_K_CENTER, m.obj_center.k = (m.obj_side[KMINIMUM] + m.obj_side[KMAXIMUM]) / 2);
+
+  // Compute the outside diameter of the nozzle at the height
+  // at which it makes contact with the calibration object
+  TERN_(HAS_X_CENTER, m.nozzle_outer_dimension.x = m.obj_side[RIGHT] - m.obj_side[LEFT] - dimensions.x);
+  TERN_(HAS_Y_CENTER, m.nozzle_outer_dimension.y = m.obj_side[BACK]  - m.obj_side[FRONT] - dimensions.y);
+
+  park_above_object(m, uncertainty);
+
+  // The difference between the known and the measured location
+  // of the calibration object is the positional error
+  LINEAR_AXIS_CODE(
+    m.pos_error.x = TERN0(HAS_X_CENTER, true_center.x - m.obj_center.x),
+    m.pos_error.y = TERN0(HAS_Y_CENTER, true_center.y - m.obj_center.y),
+    m.pos_error.z = true_center.z - m.obj_center.z,
+    m.pos_error.i = TERN0(HAS_I_CENTER, true_center.i - m.obj_center.i),
+    m.pos_error.j = TERN0(HAS_J_CENTER, true_center.j - m.obj_center.j),
+    m.pos_error.k = TERN0(HAS_K_CENTER, true_center.k - m.obj_center.k)
+  );
+}
+
+#if ENABLED(CALIBRATION_REPORTING)
+  inline void report_measured_faces(const measurements_t &m) {
+    SERIAL_ECHOLNPGM("Sides:");
+    #if HAS_Z_AXIS && AXIS_CAN_CALIBRATE(Z)
+      SERIAL_ECHOLNPAIR("  Top: ", m.obj_side[TOP]);
+    #endif
+    #if ENABLED(CALIBRATION_MEASURE_LEFT)
+      SERIAL_ECHOLNPAIR("  Left: ", m.obj_side[LEFT]);
+    #endif
+    #if ENABLED(CALIBRATION_MEASURE_RIGHT)
+      SERIAL_ECHOLNPAIR("  Right: ", m.obj_side[RIGHT]);
+    #endif
+    #if HAS_Y_AXIS
+      #if ENABLED(CALIBRATION_MEASURE_FRONT)
+        SERIAL_ECHOLNPAIR("  Front: ", m.obj_side[FRONT]);
+      #endif
+      #if ENABLED(CALIBRATION_MEASURE_BACK)
+        SERIAL_ECHOLNPAIR("  Back: ", m.obj_side[BACK]);
+      #endif
+    #endif
+    #if LINEAR_AXES >= 4
+      #if ENABLED(CALIBRATION_MEASURE_IMIN)
+        SERIAL_ECHOLNPAIR("  " STR_I_MIN ": ", m.obj_side[IMINIMUM]);
+      #endif
+      #if ENABLED(CALIBRATION_MEASURE_IMAX)
+        SERIAL_ECHOLNPAIR("  " STR_I_MAX ": ", m.obj_side[IMAXIMUM]);
+      #endif
+    #endif
+    #if LINEAR_AXES >= 5
+      #if ENABLED(CALIBRATION_MEASURE_JMIN)
+        SERIAL_ECHOLNPAIR("  " STR_J_MIN ": ", m.obj_side[JMINIMUM]);
+      #endif
+      #if ENABLED(CALIBRATION_MEASURE_JMAX)
+        SERIAL_ECHOLNPAIR("  " STR_J_MAX ": ", m.obj_side[JMAXIMUM]);
+      #endif
+    #endif
+    #if LINEAR_AXES >= 6
+      #if ENABLED(CALIBRATION_MEASURE_KMIN)
+        SERIAL_ECHOLNPAIR("  " STR_K_MIN ": ", m.obj_side[KMINIMUM]);
+      #endif
+      #if ENABLED(CALIBRATION_MEASURE_KMAX)
+        SERIAL_ECHOLNPAIR("  " STR_K_MAX ": ", m.obj_side[KMAXIMUM]);
+      #endif
+    #endif
+    SERIAL_EOL();
+  }
+
+  inline void report_measured_center(const measurements_t &m) {
+    SERIAL_ECHOLNPGM("Center:");
+    #if HAS_X_CENTER
+      SERIAL_ECHOLNPAIR_P(SP_X_STR, m.obj_center.x);
+    #endif
+    #if HAS_Y_CENTER
+      SERIAL_ECHOLNPAIR_P(SP_Y_STR, m.obj_center.y);
+    #endif
+    SERIAL_ECHOLNPAIR_P(SP_Z_STR, m.obj_center.z);
+    #if HAS_I_CENTER
+      SERIAL_ECHOLNPAIR_P(SP_I_STR, m.obj_center.i);
+    #endif
+    #if HAS_J_CENTER
+      SERIAL_ECHOLNPAIR_P(SP_J_STR, m.obj_center.j);
+    #endif
+    #if HAS_K_CENTER
+      SERIAL_ECHOLNPAIR_P(SP_K_STR, m.obj_center.k);
+    #endif
+    SERIAL_EOL();
+  }
+
+  inline void report_measured_backlash(const measurements_t &m) {
+    SERIAL_ECHOLNPGM("Backlash:");
+    #if AXIS_CAN_CALIBRATE(X)
+      #if ENABLED(CALIBRATION_MEASURE_LEFT)
+        SERIAL_ECHOLNPAIR("  Left: ", m.backlash[LEFT]);
+      #endif
+      #if ENABLED(CALIBRATION_MEASURE_RIGHT)
+        SERIAL_ECHOLNPAIR("  Right: ", m.backlash[RIGHT]);
+      #endif
+    #endif
+    #if HAS_Y_AXIS && AXIS_CAN_CALIBRATE(Y)
+      #if ENABLED(CALIBRATION_MEASURE_FRONT)
+        SERIAL_ECHOLNPAIR("  Front: ", m.backlash[FRONT]);
+      #endif
+      #if ENABLED(CALIBRATION_MEASURE_BACK)
+        SERIAL_ECHOLNPAIR("  Back: ", m.backlash[BACK]);
+      #endif
+    #endif
+    #if HAS_Z_AXIS && AXIS_CAN_CALIBRATE(Z)
+      SERIAL_ECHOLNPAIR("  Top: ", m.backlash[TOP]);
+    #endif
+    #if LINEAR_AXES >= 4 && AXIS_CAN_CALIBRATE(I)
+      #if ENABLED(CALIBRATION_MEASURE_IMIN)
+        SERIAL_ECHOLNPAIR("  " STR_I_MIN ": ", m.backlash[IMINIMUM]);
+      #endif
+      #if ENABLED(CALIBRATION_MEASURE_IMAX)
+        SERIAL_ECHOLNPAIR("  " STR_I_MAX ": ", m.backlash[IMAXIMUM]);
+      #endif
+    #endif
+    #if LINEAR_AXES >= 5 && AXIS_CAN_CALIBRATE(J)
+      #if ENABLED(CALIBRATION_MEASURE_JMIN)
+        SERIAL_ECHOLNPAIR("  " STR_J_MIN ": ", m.backlash[JMINIMUM]);
+      #endif
+      #if ENABLED(CALIBRATION_MEASURE_JMAX)
+        SERIAL_ECHOLNPAIR("  " STR_J_MAX ": ", m.backlash[JMAXIMUM]);
+      #endif
+    #endif
+    #if LINEAR_AXES >= 6 && AXIS_CAN_CALIBRATE(K)
+      #if ENABLED(CALIBRATION_MEASURE_KMIN)
+        SERIAL_ECHOLNPAIR("  " STR_K_MIN ": ", m.backlash[KMINIMUM]);
+      #endif
+      #if ENABLED(CALIBRATION_MEASURE_KMAX)
+        SERIAL_ECHOLNPAIR("  " STR_K_MAX ": ", m.backlash[KMAXIMUM]);
+      #endif
+    #endif
+    SERIAL_EOL();
+  }
+
+  inline void report_measured_positional_error(const measurements_t &m) {
+    SERIAL_CHAR('T');
+    SERIAL_ECHO(active_extruder);
+    SERIAL_ECHOLNPGM(" Positional Error:");
+    #if HAS_X_CENTER && AXIS_CAN_CALIBRATE(X)
+      SERIAL_ECHOLNPAIR_P(SP_X_STR, m.pos_error.x);
+    #endif
+    #if HAS_Y_CENTER && AXIS_CAN_CALIBRATE(Y)
+      SERIAL_ECHOLNPAIR_P(SP_Y_STR, m.pos_error.y);
+    #endif
+    #if HAS_Z_AXIS && AXIS_CAN_CALIBRATE(Z)
+      SERIAL_ECHOLNPAIR_P(SP_Z_STR, m.pos_error.z);
+    #endif
+    #if HAS_I_CENTER && AXIS_CAN_CALIBRATE(I)
+      SERIAL_ECHOLNPAIR_P(SP_I_STR, m.pos_error.i);
+    #endif
+    #if HAS_J_CENTER && AXIS_CAN_CALIBRATE(J)
+      SERIAL_ECHOLNPAIR_P(SP_J_STR, m.pos_error.j);
+    #endif
+    #if HAS_K_CENTER && AXIS_CAN_CALIBRATE(K)
+      SERIAL_ECHOLNPAIR_P(SP_Z_STR, m.pos_error.z);
+    #endif
+    SERIAL_EOL();
+  }
+
+  inline void report_measured_nozzle_dimensions(const measurements_t &m) {
+    SERIAL_ECHOLNPGM("Nozzle Tip Outer Dimensions:");
+    #if HAS_X_CENTER
+      SERIAL_ECHOLNPAIR_P(SP_X_STR, m.nozzle_outer_dimension.x);
+    #endif
+    #if HAS_Y_CENTER
+      SERIAL_ECHOLNPAIR_P(SP_Y_STR, m.nozzle_outer_dimension.y);
+    #endif
+    SERIAL_EOL();
+    UNUSED(m);
+  }
+
+  #if HAS_HOTEND_OFFSET
+    //
+    // This function requires normalize_hotend_offsets() to be called
+    //
+    inline void report_hotend_offsets() {
+      LOOP_S_L_N(e, 1, HOTENDS)
+        SERIAL_ECHOLNPAIR_P(PSTR("T"), e, PSTR(" Hotend Offset X"), hotend_offset[e].x, SP_Y_STR, hotend_offset[e].y, SP_Z_STR, hotend_offset[e].z);
+    }
+  #endif
+
+#endif // CALIBRATION_REPORTING
+
+/**
+ * Probe around the calibration object to measure backlash
+ *
+ *   m              in/out - Measurement record, updated with new readings
+ *   uncertainty    in     - How far away from the object to begin probing
+ */
+inline void calibrate_backlash(measurements_t &m, const float uncertainty) {
+  // Backlash compensation should be off while measuring backlash
+
+  {
+    // New scope for TEMPORARY_BACKLASH_CORRECTION
+    TEMPORARY_BACKLASH_CORRECTION(all_off);
+    TEMPORARY_BACKLASH_SMOOTHING(0.0f);
+
+    probe_sides(m, uncertainty);
+
+    #if ENABLED(BACKLASH_GCODE)
+
+      #if HAS_X_CENTER
+        backlash.distance_mm.x = (m.backlash[LEFT] + m.backlash[RIGHT]) / 2;
+      #elif ENABLED(CALIBRATION_MEASURE_LEFT)
+        backlash.distance_mm.x = m.backlash[LEFT];
+      #elif ENABLED(CALIBRATION_MEASURE_RIGHT)
+        backlash.distance_mm.x = m.backlash[RIGHT];
+      #endif
+
+      #if HAS_Y_CENTER
+        backlash.distance_mm.y = (m.backlash[FRONT] + m.backlash[BACK]) / 2;
+      #elif ENABLED(CALIBRATION_MEASURE_FRONT)
+        backlash.distance_mm.y = m.backlash[FRONT];
+      #elif ENABLED(CALIBRATION_MEASURE_BACK)
+        backlash.distance_mm.y = m.backlash[BACK];
+      #endif
+
+      TERN_(HAS_Z_AXIS, if (AXIS_CAN_CALIBRATE(Z)) backlash.distance_mm.z = m.backlash[TOP]);
+
+      #if HAS_I_CENTER
+        backlash.distance_mm.i = (m.backlash[IMINIMUM] + m.backlash[IMAXIMUM]) / 2;
+      #elif ENABLED(CALIBRATION_MEASURE_IMIN)
+        backlash.distance_mm.i = m.backlash[IMINIMUM];
+      #elif ENABLED(CALIBRATION_MEASURE_IMAX)
+        backlash.distance_mm.i = m.backlash[IMAXIMUM];
+      #endif
+
+      #if HAS_J_CENTER
+        backlash.distance_mm.j = (m.backlash[JMINIMUM] + m.backlash[JMAXIMUM]) / 2;
+      #elif ENABLED(CALIBRATION_MEASURE_JMIN)
+        backlash.distance_mm.j = m.backlash[JMINIMUM];
+      #elif ENABLED(CALIBRATION_MEASURE_JMAX)
+        backlash.distance_mm.j = m.backlash[JMAXIMUM];
+      #endif
+
+      #if HAS_K_CENTER
+        backlash.distance_mm.k = (m.backlash[KMINIMUM] + m.backlash[KMAXIMUM]) / 2;
+      #elif ENABLED(CALIBRATION_MEASURE_KMIN)
+        backlash.distance_mm.k = m.backlash[KMINIMUM];
+      #elif ENABLED(CALIBRATION_MEASURE_KMAX)
+        backlash.distance_mm.k = m.backlash[KMAXIMUM];
+      #endif
+
+    #endif // BACKLASH_GCODE
+  }
+
+  #if ENABLED(BACKLASH_GCODE)
+    // Turn on backlash compensation and move in all
+    // allowed directions to take up any backlash
+    {
+      // New scope for TEMPORARY_BACKLASH_CORRECTION
+      TEMPORARY_BACKLASH_CORRECTION(all_on);
+      TEMPORARY_BACKLASH_SMOOTHING(0.0f);
+      const xyz_float_t move = LINEAR_AXIS_ARRAY(
+        AXIS_CAN_CALIBRATE(X) * 3, AXIS_CAN_CALIBRATE(Y) * 3, AXIS_CAN_CALIBRATE(Z) * 3,
+        AXIS_CAN_CALIBRATE(I) * 3, AXIS_CAN_CALIBRATE(J) * 3, AXIS_CAN_CALIBRATE(K) * 3
+      );
+      current_position += move; calibration_move();
+      current_position -= move; calibration_move();
+    }
+  #endif
+}
+
+inline void update_measurements(measurements_t &m, const AxisEnum axis) {
+  current_position[axis] += m.pos_error[axis];
+  m.obj_center[axis] = true_center[axis];
+  m.pos_error[axis] = 0;
+}
+
+/**
+ * Probe around the calibration object. Adjust the position and toolhead offset
+ * using the deviation from the known position of the calibration object.
+ *
+ *   m              in/out - Measurement record, updated with new readings
+ *   uncertainty    in     - How far away from the object to begin probing
+ *   extruder       in     - What extruder to probe
+ *
+ * Prerequisites:
+ *    - Call calibrate_backlash() beforehand for best accuracy
+ */
+inline void calibrate_toolhead(measurements_t &m, const float uncertainty, const uint8_t extruder) {
+  TEMPORARY_BACKLASH_CORRECTION(all_on);
+  TEMPORARY_BACKLASH_SMOOTHING(0.0f);
+
+  TERN(HAS_MULTI_HOTEND, set_nozzle(m, extruder), UNUSED(extruder));
+
+  probe_sides(m, uncertainty);
+
+  // Adjust the hotend offset
+  #if HAS_HOTEND_OFFSET
+    if (ENABLED(HAS_X_CENTER) && AXIS_CAN_CALIBRATE(X)) hotend_offset[extruder].x += m.pos_error.x;
+    if (ENABLED(HAS_Y_CENTER) && AXIS_CAN_CALIBRATE(Y)) hotend_offset[extruder].y += m.pos_error.y;
+                             if (AXIS_CAN_CALIBRATE(Z)) hotend_offset[extruder].z += m.pos_error.z;
+    normalize_hotend_offsets();
+  #endif
+
+  // Correct for positional error, so the object
+  // is at the known actual spot
+  planner.synchronize();
+  if (ENABLED(HAS_X_CENTER) && AXIS_CAN_CALIBRATE(X)) update_measurements(m, X_AXIS);
+  if (ENABLED(HAS_Y_CENTER) && AXIS_CAN_CALIBRATE(Y)) update_measurements(m, Y_AXIS);
+                           if (AXIS_CAN_CALIBRATE(Z)) update_measurements(m, Z_AXIS);
+
+  TERN_(HAS_I_CENTER, update_measurements(m, I_AXIS));
+  TERN_(HAS_J_CENTER, update_measurements(m, J_AXIS));
+  TERN_(HAS_K_CENTER, update_measurements(m, K_AXIS));
+
+  sync_plan_position();
+}
+
+/**
+ * Probe around the calibration object for all toolheads, adjusting the coordinate
+ * system for the first nozzle and the nozzle offset for subsequent nozzles.
+ *
+ *   m              in/out - Measurement record, updated with new readings
+ *   uncertainty    in     - How far away from the object to begin probing
+ */
+inline void calibrate_all_toolheads(measurements_t &m, const float uncertainty) {
+  TEMPORARY_BACKLASH_CORRECTION(all_on);
+  TEMPORARY_BACKLASH_SMOOTHING(0.0f);
+
+  HOTEND_LOOP() calibrate_toolhead(m, uncertainty, e);
+
+  TERN_(HAS_HOTEND_OFFSET, normalize_hotend_offsets());
+
+  TERN_(HAS_MULTI_HOTEND, set_nozzle(m, 0));
+}
+
+/**
+ * Perform a full auto-calibration routine:
+ *
+ *   1) For each nozzle, touch top and sides of object to determine object position and
+ *      nozzle offsets. Do a fast but rough search over a wider area.
+ *   2) With the first nozzle, touch top and sides of object to determine backlash values
+ *      for all axis (if BACKLASH_GCODE is enabled)
+ *   3) For each nozzle, touch top and sides of object slowly to determine precise
+ *      position of object. Adjust coordinate system and nozzle offsets so probed object
+ *      location corresponds to known object location with a high degree of precision.
+ */
+inline void calibrate_all() {
+  measurements_t m;
+
+  TERN_(HAS_HOTEND_OFFSET, reset_hotend_offsets());
+
+  TEMPORARY_BACKLASH_CORRECTION(all_on);
+  TEMPORARY_BACKLASH_SMOOTHING(0.0f);
+
+  // Do a fast and rough calibration of the toolheads
+  calibrate_all_toolheads(m, CALIBRATION_MEASUREMENT_UNKNOWN);
+
+  TERN_(BACKLASH_GCODE, calibrate_backlash(m, CALIBRATION_MEASUREMENT_UNCERTAIN));
+
+  // Cycle the toolheads so the servos settle into their "natural" positions
+  #if HAS_MULTI_HOTEND
+    HOTEND_LOOP() set_nozzle(m, e);
+  #endif
+
+  // Do a slow and precise calibration of the toolheads
+  calibrate_all_toolheads(m, CALIBRATION_MEASUREMENT_UNCERTAIN);
+
+  current_position.x = X_CENTER;
+  calibration_move();         // Park nozzle away from calibration object
+}
+
+/**
+ * G425: Perform calibration with calibration object.
+ *
+ *   B           - Perform calibration of backlash only.
+ *   T<extruder> - Perform calibration of toolhead only.
+ *   V           - Probe object and print position, error, backlash and hotend offset.
+ *   U           - Uncertainty, how far to start probe away from the object (mm)
+ *
+ *   no args     - Perform entire calibration sequence (backlash + position on all toolheads)
+ */
+void GcodeSuite::G425() {
+
+  #ifdef CALIBRATION_SCRIPT_PRE
+    GcodeSuite::process_subcommands_now_P(PSTR(CALIBRATION_SCRIPT_PRE));
+  #endif
+
+  if (homing_needed_error()) return;
+
+  TEMPORARY_BED_LEVELING_STATE(false);
+  SET_SOFT_ENDSTOP_LOOSE(true);
+
+  measurements_t m;
+  const float uncertainty = parser.floatval('U', CALIBRATION_MEASUREMENT_UNCERTAIN);
+
+  if (parser.seen_test('B'))
+    calibrate_backlash(m, uncertainty);
+  else if (parser.seen_test('T'))
+    calibrate_toolhead(m, uncertainty, parser.intval('T', active_extruder));
+  #if ENABLED(CALIBRATION_REPORTING)
+    else if (parser.seen('V')) {
+      probe_sides(m, uncertainty);
+      SERIAL_EOL();
+      report_measured_faces(m);
+      report_measured_center(m);
+      report_measured_backlash(m);
+      report_measured_nozzle_dimensions(m);
+      report_measured_positional_error(m);
+      #if HAS_HOTEND_OFFSET
+        normalize_hotend_offsets();
+        report_hotend_offsets();
+      #endif
+    }
+  #endif
+  else
+    calibrate_all();
+
+  SET_SOFT_ENDSTOP_LOOSE(false);
+
+  #ifdef CALIBRATION_SCRIPT_POST
+    GcodeSuite::process_subcommands_now_P(PSTR(CALIBRATION_SCRIPT_POST));
+  #endif
+}
+
+#endif // CALIBRATION_GCODE

Marlin/src/gcode/calibrate/G76_M192_M871.cpp

@@ -0,0 +1,358 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * G76_M871.cpp - Temperature calibration/compensation for z-probing
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(PROBE_TEMP_COMPENSATION)
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+#include "../../module/planner.h"
+#include "../../module/probe.h"
+#include "../../feature/bedlevel/bedlevel.h"
+#include "../../module/temperature.h"
+#include "../../module/probe.h"
+#include "../../feature/probe_temp_comp.h"
+#include "../../lcd/marlinui.h"
+
+/**
+ * G76: calibrate probe and/or bed temperature offsets
+ *  Notes:
+ *  - When calibrating probe, bed temperature is held constant.
+ *    Compensation values are deltas to first probe measurement at probe temp. = 30°C.
+ *  - When calibrating bed, probe temperature is held constant.
+ *    Compensation values are deltas to first probe measurement at bed temp. = 60°C.
+ *  - The hotend will not be heated at any time.
+ *  - On my Průša MK3S clone I put a piece of paper between the probe and the hotend
+ *    so the hotend fan would not cool my probe constantly. Alternatively you could just
+ *    make sure the fan is not running while running the calibration process.
+ *
+ *  Probe calibration:
+ *  - Moves probe to cooldown point.
+ *  - Heats up bed to 100°C.
+ *  - Moves probe to probing point (1mm above heatbed).
+ *  - Waits until probe reaches target temperature (30°C).
+ *  - Does a z-probing (=base value) and increases target temperature by 5°C.
+ *  - Waits until probe reaches increased target temperature.
+ *  - Does a z-probing (delta to base value will be a compensation value) and increases target temperature by 5°C.
+ *  - Repeats last two steps until max. temperature reached or timeout (i.e. probe does not heat up any further).
+ *  - Compensation values of higher temperatures will be extrapolated (using linear regression first).
+ *    While this is not exact by any means it is still better than simply using the last compensation value.
+ *
+ *  Bed calibration:
+ *  - Moves probe to cooldown point.
+ *  - Heats up bed to 60°C.
+ *  - Moves probe to probing point (1mm above heatbed).
+ *  - Waits until probe reaches target temperature (30°C).
+ *  - Does a z-probing (=base value) and increases bed temperature by 5°C.
+ *  - Moves probe to cooldown point.
+ *  - Waits until probe is below 30°C and bed has reached target temperature.
+ *  - Moves probe to probing point and waits until it reaches target temperature (30°C).
+ *  - Does a z-probing (delta to base value will be a compensation value) and increases bed temperature by 5°C.
+ *  - Repeats last four points until max. bed temperature reached (110°C) or timeout.
+ *  - Compensation values of higher temperatures will be extrapolated (using linear regression first).
+ *    While this is not exact by any means it is still better than simply using the last compensation value.
+ *
+ *  G76 [B | P]
+ *  - no flag - Both calibration procedures will be run.
+ *  - `B` - Run bed temperature calibration.
+ *  - `P` - Run probe temperature calibration.
+ */
+
+static void say_waiting_for()               { SERIAL_ECHOPGM("Waiting for "); }
+static void say_waiting_for_probe_heating() { say_waiting_for(); SERIAL_ECHOLNPGM("probe heating."); }
+static void say_successfully_calibrated()   { SERIAL_ECHOPGM("Successfully calibrated"); }
+static void say_failed_to_calibrate()       { SERIAL_ECHOPGM("!Failed to calibrate"); }
+
+void GcodeSuite::G76() {
+  // Check if heated bed is available and z-homing is done with probe
+  #if TEMP_SENSOR_BED == 0 || !(HOMING_Z_WITH_PROBE)
+    return;
+  #endif
+
+  auto report_temps = [](millis_t &ntr, millis_t timeout=0) {
+    idle_no_sleep();
+    const millis_t ms = millis();
+    if (ELAPSED(ms, ntr)) {
+      ntr = ms + 1000;
+      thermalManager.print_heater_states(active_extruder);
+    }
+    return (timeout && ELAPSED(ms, timeout));
+  };
+
+  auto wait_for_temps = [&](const celsius_t tb, const celsius_t tp, millis_t &ntr, const millis_t timeout=0) {
+    say_waiting_for(); SERIAL_ECHOLNPGM("bed and probe temperature.");
+    while (thermalManager.wholeDegBed() != tb || thermalManager.wholeDegProbe() > tp)
+      if (report_temps(ntr, timeout)) return true;
+    return false;
+  };
+
+  auto g76_probe = [](const TempSensorID sid, celsius_t &targ, const xy_pos_t &nozpos) {
+    do_z_clearance(5.0); // Raise nozzle before probing
+    const float measured_z = probe.probe_at_point(nozpos, PROBE_PT_STOW, 0, false);  // verbose=0, probe_relative=false
+    if (isnan(measured_z))
+      SERIAL_ECHOLNPGM("!Received NAN. Aborting.");
+    else {
+      SERIAL_ECHOLNPAIR_F("Measured: ", measured_z);
+      if (targ == cali_info_init[sid].start_temp)
+        temp_comp.prepare_new_calibration(measured_z);
+      else
+        temp_comp.push_back_new_measurement(sid, measured_z);
+      targ += cali_info_init[sid].temp_res;
+    }
+    return measured_z;
+  };
+
+  #if ENABLED(BLTOUCH)
+    // Make sure any BLTouch error condition is cleared
+    bltouch_command(BLTOUCH_RESET, BLTOUCH_RESET_DELAY);
+    set_bltouch_deployed(false);
+  #endif
+
+  bool do_bed_cal = parser.boolval('B'), do_probe_cal = parser.boolval('P');
+  if (!do_bed_cal && !do_probe_cal) do_bed_cal = do_probe_cal = true;
+
+  // Synchronize with planner
+  planner.synchronize();
+
+  const xyz_pos_t parkpos = temp_comp.park_point,
+            probe_pos_xyz = xyz_pos_t(temp_comp.measure_point) + xyz_pos_t({ 0.0f, 0.0f, PTC_PROBE_HEATING_OFFSET }),
+              noz_pos_xyz = probe_pos_xyz - probe.offset_xy;  // Nozzle position based on probe position
+
+  if (do_bed_cal || do_probe_cal) {
+    // Ensure park position is reachable
+    bool reachable = position_is_reachable(parkpos) || WITHIN(parkpos.z, Z_MIN_POS - fslop, Z_MAX_POS + fslop);
+    if (!reachable)
+      SERIAL_ECHOLNPGM("!Park");
+    else {
+      // Ensure probe position is reachable
+      reachable = probe.can_reach(probe_pos_xyz);
+      if (!reachable) SERIAL_ECHOLNPGM("!Probe");
+    }
+
+    if (!reachable) {
+      SERIAL_ECHOLNPGM(" position unreachable - aborting.");
+      return;
+    }
+
+    process_subcommands_now_P(G28_STR);
+  }
+
+  remember_feedrate_scaling_off();
+
+  /******************************************
+   * Calibrate bed temperature offsets
+   ******************************************/
+
+  // Report temperatures every second and handle heating timeouts
+  millis_t next_temp_report = millis() + 1000;
+
+  auto report_targets = [&](const celsius_t tb, const celsius_t tp) {
+    SERIAL_ECHOLNPAIR("Target Bed:", tb, " Probe:", tp);
+  };
+
+  if (do_bed_cal) {
+
+    celsius_t target_bed = cali_info_init[TSI_BED].start_temp,
+            target_probe = temp_comp.bed_calib_probe_temp;
+
+    say_waiting_for(); SERIAL_ECHOLNPGM(" cooling.");
+    while (thermalManager.wholeDegBed() > target_bed || thermalManager.wholeDegProbe() > target_probe)
+      report_temps(next_temp_report);
+
+    // Disable leveling so it won't mess with us
+    TERN_(HAS_LEVELING, set_bed_leveling_enabled(false));
+
+    for (;;) {
+      thermalManager.setTargetBed(target_bed);
+
+      report_targets(target_bed, target_probe);
+
+      // Park nozzle
+      do_blocking_move_to(parkpos);
+
+      // Wait for heatbed to reach target temp and probe to cool below target temp
+      if (wait_for_temps(target_bed, target_probe, next_temp_report, millis() + MIN_TO_MS(15))) {
+        SERIAL_ECHOLNPGM("!Bed heating timeout.");
+        break;
+      }
+
+      // Move the nozzle to the probing point and wait for the probe to reach target temp
+      do_blocking_move_to(noz_pos_xyz);
+      say_waiting_for_probe_heating();
+      SERIAL_EOL();
+      while (thermalManager.wholeDegProbe() < target_probe)
+        report_temps(next_temp_report);
+
+      const float measured_z = g76_probe(TSI_BED, target_bed, noz_pos_xyz);
+      if (isnan(measured_z) || target_bed > (BED_MAX_TARGET)) break;
+    }
+
+    SERIAL_ECHOLNPAIR("Retrieved measurements: ", temp_comp.get_index());
+    if (temp_comp.finish_calibration(TSI_BED)) {
+      say_successfully_calibrated();
+      SERIAL_ECHOLNPGM(" bed.");
+    }
+    else {
+      say_failed_to_calibrate();
+      SERIAL_ECHOLNPGM(" bed. Values reset.");
+    }
+
+    // Cleanup
+    thermalManager.setTargetBed(0);
+    TERN_(HAS_LEVELING, set_bed_leveling_enabled(true));
+  } // do_bed_cal
+
+  /********************************************
+   * Calibrate probe temperature offsets
+   ********************************************/
+
+  if (do_probe_cal) {
+
+    // Park nozzle
+    do_blocking_move_to(parkpos);
+
+    // Initialize temperatures
+    const celsius_t target_bed = temp_comp.probe_calib_bed_temp;
+    thermalManager.setTargetBed(target_bed);
+
+    celsius_t target_probe = cali_info_init[TSI_PROBE].start_temp;
+
+    report_targets(target_bed, target_probe);
+
+    // Wait for heatbed to reach target temp and probe to cool below target temp
+    wait_for_temps(target_bed, target_probe, next_temp_report);
+
+    // Disable leveling so it won't mess with us
+    TERN_(HAS_LEVELING, set_bed_leveling_enabled(false));
+
+    bool timeout = false;
+    for (;;) {
+      // Move probe to probing point and wait for it to reach target temperature
+      do_blocking_move_to(noz_pos_xyz);
+
+      say_waiting_for_probe_heating();
+      SERIAL_ECHOLNPAIR(" Bed:", target_bed, " Probe:", target_probe);
+      const millis_t probe_timeout_ms = millis() + SEC_TO_MS(900UL);
+      while (thermalManager.degProbe() < target_probe) {
+        if (report_temps(next_temp_report, probe_timeout_ms)) {
+          SERIAL_ECHOLNPGM("!Probe heating timed out.");
+          timeout = true;
+          break;
+        }
+      }
+      if (timeout) break;
+
+      const float measured_z = g76_probe(TSI_PROBE, target_probe, noz_pos_xyz);
+      if (isnan(measured_z) || target_probe > cali_info_init[TSI_PROBE].end_temp) break;
+    }
+
+    SERIAL_ECHOLNPAIR("Retrieved measurements: ", temp_comp.get_index());
+    if (temp_comp.finish_calibration(TSI_PROBE))
+      say_successfully_calibrated();
+    else
+      say_failed_to_calibrate();
+    SERIAL_ECHOLNPGM(" probe.");
+
+    // Cleanup
+    thermalManager.setTargetBed(0);
+    TERN_(HAS_LEVELING, set_bed_leveling_enabled(true));
+
+    SERIAL_ECHOLNPGM("Final compensation values:");
+    temp_comp.print_offsets();
+  } // do_probe_cal
+
+  restore_feedrate_and_scaling();
+}
+
+/**
+ * M871: Report / reset temperature compensation offsets.
+ *       Note: This does not affect values in EEPROM until M500.
+ *
+ *   M871 [ R | B | P | E ]
+ *
+ *    No Parameters - Print current offset values.
+ *
+ * Select only one of these flags:
+ *    R - Reset all offsets to zero (i.e., disable compensation).
+ *    B - Manually set offset for bed
+ *    P - Manually set offset for probe
+ *    E - Manually set offset for extruder
+ *
+ * With B, P, or E:
+ *    I[index] - Index in the array
+ *    V[value] - Adjustment in µm
+ */
+void GcodeSuite::M871() {
+
+  if (parser.seen('R')) {
+    // Reset z-probe offsets to factory defaults
+    temp_comp.clear_all_offsets();
+    SERIAL_ECHOLNPGM("Offsets reset to default.");
+  }
+  else if (parser.seen("BPE")) {
+    if (!parser.seenval('V')) return;
+    const int16_t offset_val = parser.value_int();
+    if (!parser.seenval('I')) return;
+    const int16_t idx = parser.value_int();
+    const TempSensorID mod = (parser.seen('B') ? TSI_BED :
+                                #if ENABLED(USE_TEMP_EXT_COMPENSATION)
+                                  parser.seen('E') ? TSI_EXT :
+                                #endif
+                                TSI_PROBE
+                              );
+    if (idx > 0 && temp_comp.set_offset(mod, idx - 1, offset_val))
+      SERIAL_ECHOLNPAIR("Set value: ", offset_val);
+    else
+      SERIAL_ECHOLNPGM("!Invalid index. Failed to set value (note: value at index 0 is constant).");
+
+  }
+  else // Print current Z-probe adjustments. Note: Values in EEPROM might differ.
+    temp_comp.print_offsets();
+}
+
+/**
+ * M192: Wait for probe temperature sensor to reach a target
+ *
+ * Select only one of these flags:
+ *    R - Wait for heating or cooling
+ *    S - Wait only for heating
+ */
+void GcodeSuite::M192() {
+  if (DEBUGGING(DRYRUN)) return;
+
+  const bool no_wait_for_cooling = parser.seenval('S');
+  if (!no_wait_for_cooling && ! parser.seenval('R')) {
+    SERIAL_ERROR_MSG("No target temperature set.");
+    return;
+  }
+
+  const celsius_t target_temp = parser.value_celsius();
+  ui.set_status_P(thermalManager.isProbeBelowTemp(target_temp) ? GET_TEXT(MSG_PROBE_HEATING) : GET_TEXT(MSG_PROBE_COOLING));
+  thermalManager.wait_for_probe(target_temp, no_wait_for_cooling);
+}
+
+#endif // PROBE_TEMP_COMPENSATION

Marlin/src/gcode/calibrate/M100.cpp

@@ -0,0 +1,373 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(M100_FREE_MEMORY_WATCHER)
+
+#include "../gcode.h"
+#include "../queue.h"
+#include "../../libs/hex_print.h"
+
+#include "../../MarlinCore.h" // for idle()
+
+/**
+ * M100 Free Memory Watcher
+ *
+ * This code watches the free memory block between the bottom of the heap and the top of the stack.
+ * This memory block is initialized and watched via the M100 command.
+ *
+ * M100 I   Initializes the free memory block and prints vitals statistics about the area
+ *
+ * M100 F   Identifies how much of the free memory block remains free and unused. It also
+ *          detects and reports any corruption within the free memory block that may have
+ *          happened due to errant firmware.
+ *
+ * M100 D   Does a hex display of the free memory block along with a flag for any errant
+ *          data that does not match the expected value.
+ *
+ * M100 C x Corrupts x locations within the free memory block. This is useful to check the
+ *          correctness of the M100 F and M100 D commands.
+ *
+ * Also, there are two support functions that can be called from a developer's C code.
+ *
+ *    uint16_t check_for_free_memory_corruption(PGM_P const free_memory_start);
+ *    void M100_dump_routine(PGM_P const title, const char * const start, const uintptr_t size);
+ *
+ * Initial version by Roxy-3D
+ */
+#define M100_FREE_MEMORY_DUMPER     // Enable for the `M100 D` Dump sub-command
+#define M100_FREE_MEMORY_CORRUPTOR  // Enable for the `M100 C` Corrupt sub-command
+
+#define TEST_BYTE ((char) 0xE5)
+
+#if EITHER(__AVR__, IS_32BIT_TEENSY)
+
+  extern char __bss_end;
+  char *end_bss = &__bss_end,
+       *free_memory_start = end_bss, *free_memory_end = 0,
+       *stacklimit = 0, *heaplimit = 0;
+
+  #define MEMORY_END_CORRECTION 0
+
+#elif defined(TARGET_LPC1768)
+
+  extern char __bss_end__, __StackLimit, __HeapLimit;
+
+  char *end_bss = &__bss_end__,
+       *stacklimit = &__StackLimit,
+       *heaplimit = &__HeapLimit;
+
+  #define MEMORY_END_CORRECTION 0x200
+
+  char *free_memory_start = heaplimit,
+       *free_memory_end = stacklimit - MEMORY_END_CORRECTION;
+
+#elif defined(__SAM3X8E__)
+
+  extern char _ebss;
+
+  char *end_bss = &_ebss,
+       *free_memory_start = end_bss,
+       *free_memory_end = 0,
+       *stacklimit = 0,
+       *heaplimit = 0;
+
+  #define MEMORY_END_CORRECTION 0x10000  // need to stay well below 0x20080000 or M100 F crashes
+
+#elif defined(__SAMD51__)
+
+  extern unsigned int __bss_end__, __StackLimit, __HeapLimit;
+  extern "C" void * _sbrk(int incr);
+
+  void *end_bss = &__bss_end__,
+       *stacklimit = &__StackLimit,
+       *heaplimit = &__HeapLimit;
+
+  #define MEMORY_END_CORRECTION 0x400
+
+  char *free_memory_start = (char *)_sbrk(0) + 0x200,     //  Leave some heap space
+       *free_memory_end = (char *)stacklimit - MEMORY_END_CORRECTION;
+
+#else
+  #error "M100 - unsupported CPU"
+#endif
+
+//
+// Utility functions
+//
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wreturn-local-addr"
+
+// Location of a variable in its stack frame.
+// The returned address will be above the stack (after it returns).
+char *top_of_stack() {
+  char x;
+  return &x + 1; // x is pulled on return;
+}
+
+#pragma GCC diagnostic pop
+
+// Count the number of test bytes at the specified location.
+inline int32_t count_test_bytes(const char * const start_free_memory) {
+  for (uint32_t i = 0; i < 32000; i++)
+    if (char(start_free_memory[i]) != TEST_BYTE)
+      return i - 1;
+
+  return -1;
+}
+
+//
+// M100 sub-commands
+//
+
+#if ENABLED(M100_FREE_MEMORY_DUMPER)
+  /**
+   * M100 D
+   *  Dump the free memory block from brkval to the stack pointer.
+   *  malloc() eats memory from the start of the block and the stack grows
+   *  up from the bottom of the block. Solid test bytes indicate nothing has
+   *  used that memory yet. There should not be anything but test bytes within
+   *  the block. If so, it may indicate memory corruption due to a bad pointer.
+   *  Unexpected bytes are flagged in the right column.
+   */
+  void dump_free_memory(char *start_free_memory, char *end_free_memory) {
+    //
+    // Start and end the dump on a nice 16 byte boundary
+    // (even though the values are not 16-byte aligned).
+    //
+    start_free_memory = (char*)(uintptr_t(uint32_t(start_free_memory) & ~0xFUL)); // Align to 16-byte boundary
+    end_free_memory   = (char*)(uintptr_t(uint32_t(end_free_memory)   |  0xFUL)); // Align end_free_memory to the 15th byte (at or above end_free_memory)
+
+    // Dump command main loop
+    while (start_free_memory < end_free_memory) {
+      print_hex_address(start_free_memory);             // Print the address
+      SERIAL_CHAR(':');
+      LOOP_L_N(i, 16) {  // and 16 data bytes
+        if (i == 8) SERIAL_CHAR('-');
+        print_hex_byte(start_free_memory[i]);
+        SERIAL_CHAR(' ');
+      }
+      serial_delay(25);
+      SERIAL_CHAR('|');                   // Point out non test bytes
+      LOOP_L_N(i, 16) {
+        char ccc = (char)start_free_memory[i]; // cast to char before automatically casting to char on assignment, in case the compiler is broken
+        ccc = (ccc == TEST_BYTE) ? ' ' : '?';
+        SERIAL_CHAR(ccc);
+      }
+      SERIAL_EOL();
+      start_free_memory += 16;
+      serial_delay(25);
+      idle();
+    }
+  }
+
+  void M100_dump_routine(PGM_P const title, const char * const start, const uintptr_t size) {
+    SERIAL_ECHOLNPGM_P(title);
+    //
+    // Round the start and end locations to produce full lines of output
+    //
+    const char * const end = start + size - 1;
+    dump_free_memory(
+      (char*)(uintptr_t(uint32_t(start) & ~0xFUL)), // Align to 16-byte boundary
+      (char*)(uintptr_t(uint32_t(end)   |  0xFUL))  // Align end_free_memory to the 15th byte (at or above end_free_memory)
+    );
+  }
+
+#endif // M100_FREE_MEMORY_DUMPER
+
+inline int check_for_free_memory_corruption(PGM_P const title) {
+  SERIAL_ECHOPGM_P(title);
+
+  char *start_free_memory = free_memory_start, *end_free_memory = free_memory_end;
+  int n = end_free_memory - start_free_memory;
+
+  SERIAL_ECHOLNPAIR("\nfmc() n=", n,
+                    "\nfree_memory_start=", hex_address(free_memory_start),
+                    "  end=", hex_address(end_free_memory));
+
+  if (end_free_memory < start_free_memory)  {
+    SERIAL_ECHOPGM(" end_free_memory < Heap ");
+    //SET_INPUT_PULLUP(63);           // if the developer has a switch wired up to their controller board
+    //safe_delay(5);                  // this code can be enabled to pause the display as soon as the
+    //while ( READ(63))               // malfunction is detected.   It is currently defaulting to a switch
+    //  idle();                       // being on pin-63 which is unassigend and available on most controller
+    //safe_delay(20);                 // boards.
+    //while ( !READ(63))
+    //  idle();
+    serial_delay(20);
+    #if ENABLED(M100_FREE_MEMORY_DUMPER)
+      M100_dump_routine(PSTR("   Memory corruption detected with end_free_memory<Heap\n"), (const char*)0x1B80, 0x0680);
+    #endif
+  }
+
+  // Scan through the range looking for the biggest block of 0xE5's we can find
+  int block_cnt = 0;
+  for (int i = 0; i < n; i++) {
+    if (start_free_memory[i] == TEST_BYTE) {
+      int32_t j = count_test_bytes(start_free_memory + i);
+      if (j > 8) {
+        //SERIAL_ECHOPAIR("Found ", j);
+        //SERIAL_ECHOLNPAIR(" bytes free at ", hex_address(start_free_memory + i));
+        i += j;
+        block_cnt++;
+        SERIAL_ECHOLNPAIR(" (", block_cnt, ") found=", j);
+      }
+    }
+  }
+  SERIAL_ECHOPAIR("  block_found=", block_cnt);
+
+  if (block_cnt != 1)
+    SERIAL_ECHOLNPGM("\nMemory Corruption detected in free memory area.");
+
+  if (block_cnt == 0)       // Make sure the special case of no free blocks shows up as an
+    block_cnt = -1;         //  error to the calling code!
+
+  SERIAL_ECHOPGM(" return=");
+  if (block_cnt == 1) {
+    SERIAL_CHAR('0');       // If the block_cnt is 1, nothing has broken up the free memory
+    SERIAL_EOL();           //  area and it is appropriate to say 'no corruption'.
+    return 0;
+  }
+  SERIAL_ECHOLNPGM("true");
+  return block_cnt;
+}
+
+/**
+ * M100 F
+ *  Return the number of free bytes in the memory pool,
+ *  with other vital statistics defining the pool.
+ */
+inline void free_memory_pool_report(char * const start_free_memory, const int32_t size) {
+  int32_t max_cnt = -1, block_cnt = 0;
+  char *max_addr = nullptr;
+  // Find the longest block of test bytes in the buffer
+  for (int32_t i = 0; i < size; i++) {
+    char *addr = start_free_memory + i;
+    if (*addr == TEST_BYTE) {
+      const int32_t j = count_test_bytes(addr);
+      if (j > 8) {
+        SERIAL_ECHOLNPAIR("Found ", j, " bytes free at ", hex_address(addr));
+        if (j > max_cnt) {
+          max_cnt  = j;
+          max_addr = addr;
+        }
+        i += j;
+        block_cnt++;
+      }
+    }
+  }
+  if (block_cnt > 1) SERIAL_ECHOLNPAIR(
+    "\nMemory Corruption detected in free memory area."
+    "\nLargest free block is ", max_cnt, " bytes at ", hex_address(max_addr)
+  );
+  SERIAL_ECHOLNPAIR("check_for_free_memory_corruption() = ", check_for_free_memory_corruption(PSTR("M100 F ")));
+}
+
+#if ENABLED(M100_FREE_MEMORY_CORRUPTOR)
+  /**
+   * M100 C<num>
+   *  Corrupt <num> locations in the free memory pool and report the corrupt addresses.
+   *  This is useful to check the correctness of the M100 D and the M100 F commands.
+   */
+  inline void corrupt_free_memory(char *start_free_memory, const uintptr_t size) {
+    start_free_memory += 8;
+    const uint32_t near_top = top_of_stack() - start_free_memory - 250, // -250 to avoid interrupt activity that's altered the stack.
+                   j = near_top / (size + 1);
+
+    SERIAL_ECHOLNPGM("Corrupting free memory block.");
+    for (uint32_t i = 1; i <= size; i++) {
+      char * const addr = start_free_memory + i * j;
+      *addr = i;
+      SERIAL_ECHOPAIR("\nCorrupting address: ", hex_address(addr));
+    }
+    SERIAL_EOL();
+  }
+#endif // M100_FREE_MEMORY_CORRUPTOR
+
+/**
+ * M100 I
+ *  Init memory for the M100 tests. (Automatically applied on the first M100.)
+ */
+inline void init_free_memory(char *start_free_memory, int32_t size) {
+  SERIAL_ECHOLNPGM("Initializing free memory block.\n\n");
+
+  size -= 250;    // -250 to avoid interrupt activity that's altered the stack.
+  if (size < 0) {
+    SERIAL_ECHOLNPGM("Unable to initialize.\n");
+    return;
+  }
+
+  start_free_memory += 8; // move a few bytes away from the heap just because we
+                          // don't want to be altering memory that close to it.
+  memset(start_free_memory, TEST_BYTE, size);
+
+  SERIAL_ECHO(size);
+  SERIAL_ECHOLNPGM(" bytes of memory initialized.\n");
+
+  for (int32_t i = 0; i < size; i++) {
+    if (start_free_memory[i] != TEST_BYTE) {
+      SERIAL_ECHOPAIR("? address : ", hex_address(start_free_memory + i));
+      SERIAL_ECHOLNPAIR("=", hex_byte(start_free_memory[i]));
+      SERIAL_EOL();
+    }
+  }
+}
+
+/**
+ * M100: Free Memory Check
+ */
+void GcodeSuite::M100() {
+  char *sp = top_of_stack();
+  if (!free_memory_end) free_memory_end = sp - MEMORY_END_CORRECTION;
+                  SERIAL_ECHOPAIR("\nbss_end               : ", hex_address(end_bss));
+  if (heaplimit)  SERIAL_ECHOPAIR("\n__heaplimit           : ", hex_address(heaplimit));
+                  SERIAL_ECHOPAIR("\nfree_memory_start     : ", hex_address(free_memory_start));
+  if (stacklimit) SERIAL_ECHOPAIR("\n__stacklimit          : ", hex_address(stacklimit));
+                  SERIAL_ECHOPAIR("\nfree_memory_end       : ", hex_address(free_memory_end));
+  if (MEMORY_END_CORRECTION)
+                  SERIAL_ECHOPAIR("\nMEMORY_END_CORRECTION : ", MEMORY_END_CORRECTION);
+                  SERIAL_ECHOLNPAIR("\nStack Pointer       : ", hex_address(sp));
+
+  // Always init on the first invocation of M100
+  static bool m100_not_initialized = true;
+  if (m100_not_initialized || parser.seen('I')) {
+    m100_not_initialized = false;
+    init_free_memory(free_memory_start, free_memory_end - free_memory_start);
+  }
+
+  #if ENABLED(M100_FREE_MEMORY_DUMPER)
+    if (parser.seen('D'))
+      return dump_free_memory(free_memory_start, free_memory_end);
+  #endif
+
+  if (parser.seen('F'))
+    return free_memory_pool_report(free_memory_start, free_memory_end - free_memory_start);
+
+  #if ENABLED(M100_FREE_MEMORY_CORRUPTOR)
+    if (parser.seen('C'))
+      return corrupt_free_memory(free_memory_start, parser.value_int());
+  #endif
+}
+
+#endif // M100_FREE_MEMORY_WATCHER

Marlin/src/gcode/calibrate/M12.cpp

@@ -0,0 +1,39 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#include "../../inc/MarlinConfigPre.h"
+
+#if ENABLED(EXTERNAL_CLOSED_LOOP_CONTROLLER)
+
+#include "../gcode.h"
+#include "../../module/planner.h"
+#include "../../feature/closedloop.h"
+
+void GcodeSuite::M12() {
+
+  planner.synchronize();
+
+  if (parser.seenval('S'))
+    closedloop.set(parser.value_int()); // Force a CLC set
+
+}
+
+#endif

Marlin/src/gcode/calibrate/M425.cpp

@@ -0,0 +1,116 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(BACKLASH_GCODE)
+
+#include "../../feature/backlash.h"
+#include "../../module/planner.h"
+
+#include "../gcode.h"
+
+/**
+ * M425: Enable and tune backlash correction.
+ *
+ *   F<fraction>     Enable/disable/fade-out backlash correction (0.0 to 1.0)
+ *   S<smoothing_mm> Distance over which backlash correction is spread
+ *   X<distance_mm>  Set the backlash distance on X (0 to disable)
+ *   Y<distance_mm>                        ... on Y
+ *   Z<distance_mm>                        ... on Z
+ *   X               If a backlash measurement was done on X, copy that value
+ *   Y                                              ... on Y
+ *   Z                                              ... on Z
+ *
+ * Type M425 without any arguments to show active values.
+ */
+void GcodeSuite::M425() {
+  bool noArgs = true;
+
+  auto axis_can_calibrate = [](const uint8_t a) {
+    switch (a) {
+      default: return false;
+      LINEAR_AXIS_CODE(
+        case X_AXIS: return AXIS_CAN_CALIBRATE(X),
+        case Y_AXIS: return AXIS_CAN_CALIBRATE(Y),
+        case Z_AXIS: return AXIS_CAN_CALIBRATE(Z),
+        case I_AXIS: return AXIS_CAN_CALIBRATE(I),
+        case J_AXIS: return AXIS_CAN_CALIBRATE(J),
+        case K_AXIS: return AXIS_CAN_CALIBRATE(K),
+      );
+    }
+  };
+
+  LOOP_LINEAR_AXES(a) {
+    if (axis_can_calibrate(a) && parser.seen(AXIS_CHAR(a))) {
+      planner.synchronize();
+      backlash.distance_mm[a] = parser.has_value() ? parser.value_linear_units() : backlash.get_measurement(AxisEnum(a));
+      noArgs = false;
+    }
+  }
+
+  if (parser.seen('F')) {
+    planner.synchronize();
+    backlash.set_correction(parser.value_float());
+    noArgs = false;
+  }
+
+  #ifdef BACKLASH_SMOOTHING_MM
+    if (parser.seen('S')) {
+      planner.synchronize();
+      backlash.smoothing_mm = parser.value_linear_units();
+      noArgs = false;
+    }
+  #endif
+
+  if (noArgs) {
+    SERIAL_ECHOPGM("Backlash Correction ");
+    if (!backlash.correction) SERIAL_ECHOPGM("in");
+    SERIAL_ECHOLNPGM("active:");
+    SERIAL_ECHOLNPAIR("  Correction Amount/Fade-out:     F", backlash.get_correction(), " (F1.0 = full, F0.0 = none)");
+    SERIAL_ECHOPGM("  Backlash Distance (mm):        ");
+    LOOP_LINEAR_AXES(a) if (axis_can_calibrate(a)) {
+      SERIAL_CHAR(' ', AXIS_CHAR(a));
+      SERIAL_ECHO(backlash.distance_mm[a]);
+      SERIAL_EOL();
+    }
+
+    #ifdef BACKLASH_SMOOTHING_MM
+      SERIAL_ECHOLNPAIR("  Smoothing (mm):                 S", backlash.smoothing_mm);
+    #endif
+
+    #if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
+      SERIAL_ECHOPGM("  Average measured backlash (mm):");
+      if (backlash.has_any_measurement()) {
+        LOOP_LINEAR_AXES(a) if (axis_can_calibrate(a) && backlash.has_measurement(AxisEnum(a))) {
+          SERIAL_CHAR(' ', AXIS_CHAR(a));
+          SERIAL_ECHO(backlash.get_measurement(AxisEnum(a)));
+        }
+      }
+      else
+        SERIAL_ECHOPGM(" (Not yet measured)");
+      SERIAL_EOL();
+    #endif
+  }
+}
+
+#endif // BACKLASH_GCODE

Marlin/src/gcode/calibrate/M48.cpp

@@ -0,0 +1,275 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST)
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+#include "../../module/probe.h"
+#include "../../lcd/marlinui.h"
+
+#include "../../feature/bedlevel/bedlevel.h"
+
+#if HAS_LEVELING
+  #include "../../module/planner.h"
+#endif
+
+/**
+ * M48: Z probe repeatability measurement function.
+ *
+ * Usage:
+ *   M48 <P#> <X#> <Y#> <V#> <E> <L#> <S>
+ *     P = Number of sampled points (4-50, default 10)
+ *     X = Sample X position
+ *     Y = Sample Y position
+ *     V = Verbose level (0-4, default=1)
+ *     E = Engage Z probe for each reading
+ *     L = Number of legs of movement before probe
+ *     S = Schizoid (Or Star if you prefer)
+ *
+ * This function requires the machine to be homed before invocation.
+ */
+
+void GcodeSuite::M48() {
+
+  if (homing_needed_error()) return;
+
+  const int8_t verbose_level = parser.byteval('V', 1);
+  if (!WITHIN(verbose_level, 0, 4)) {
+    SERIAL_ECHOLNPGM("?(V)erbose level implausible (0-4).");
+    return;
+  }
+
+  if (verbose_level > 0)
+    SERIAL_ECHOLNPGM("M48 Z-Probe Repeatability Test");
+
+  const int8_t n_samples = parser.byteval('P', 10);
+  if (!WITHIN(n_samples, 4, 50)) {
+    SERIAL_ECHOLNPGM("?Sample size not plausible (4-50).");
+    return;
+  }
+
+  const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE;
+
+  // Test at the current position by default, overridden by X and Y
+  const xy_pos_t test_position = {
+    parser.linearval('X', current_position.x + probe.offset_xy.x),  // If no X use the probe's current X position
+    parser.linearval('Y', current_position.y + probe.offset_xy.y)   // If no Y, ditto
+  };
+
+  if (!probe.can_reach(test_position)) {
+    ui.set_status_P(GET_TEXT(MSG_M48_OUT_OF_BOUNDS), 99);
+    SERIAL_ECHOLNPGM("? (X,Y) out of bounds.");
+    return;
+  }
+
+  // Get the number of leg moves per test-point
+  bool seen_L = parser.seen('L');
+  uint8_t n_legs = seen_L ? parser.value_byte() : 0;
+  if (n_legs > 15) {
+    SERIAL_ECHOLNPGM("?Legs of movement implausible (0-15).");
+    return;
+  }
+  if (n_legs == 1) n_legs = 2;
+
+  // Schizoid motion as an optional stress-test
+  const bool schizoid_flag = parser.boolval('S');
+  if (schizoid_flag && !seen_L) n_legs = 7;
+
+  if (verbose_level > 2)
+    SERIAL_ECHOLNPGM("Positioning the probe...");
+
+  // Always disable Bed Level correction before probing...
+
+  #if HAS_LEVELING
+    const bool was_enabled = planner.leveling_active;
+    set_bed_leveling_enabled(false);
+  #endif
+
+  // Work with reasonable feedrates
+  remember_feedrate_scaling_off();
+
+  // Working variables
+  float mean = 0.0,     // The average of all points so far, used to calculate deviation
+        sigma = 0.0,    // Standard deviation of all points so far
+        min = 99999.9,  // Smallest value sampled so far
+        max = -99999.9, // Largest value sampled so far
+        sample_set[n_samples];  // Storage for sampled values
+
+  auto dev_report = [](const bool verbose, const_float_t mean, const_float_t sigma, const_float_t min, const_float_t max, const bool final=false) {
+    if (verbose) {
+      SERIAL_ECHOPAIR_F("Mean: ", mean, 6);
+      if (!final) SERIAL_ECHOPAIR_F(" Sigma: ", sigma, 6);
+      SERIAL_ECHOPAIR_F(" Min: ", min, 3);
+      SERIAL_ECHOPAIR_F(" Max: ", max, 3);
+      SERIAL_ECHOPAIR_F(" Range: ", max-min, 3);
+      if (final) SERIAL_EOL();
+    }
+    if (final) {
+      SERIAL_ECHOLNPAIR_F("Standard Deviation: ", sigma, 6);
+      SERIAL_EOL();
+    }
+  };
+
+  // Move to the first point, deploy, and probe
+  const float t = probe.probe_at_point(test_position, raise_after, verbose_level);
+  bool probing_good = !isnan(t);
+
+  if (probing_good) {
+    randomSeed(millis());
+
+    float sample_sum = 0.0;
+
+    LOOP_L_N(n, n_samples) {
+      #if HAS_STATUS_MESSAGE
+        // Display M48 progress in the status bar
+        ui.status_printf_P(0, PSTR(S_FMT ": %d/%d"), GET_TEXT(MSG_M48_POINT), int(n + 1), int(n_samples));
+      #endif
+
+      // When there are "legs" of movement move around the point before probing
+      if (n_legs) {
+
+        // Pick a random direction, starting angle, and radius
+        const int dir = (random(0, 10) > 5.0) ? -1 : 1;  // clockwise or counter clockwise
+        float angle = random(0, 360);
+        const float radius = random(
+          #if ENABLED(DELTA)
+            int(0.1250000000 * (DELTA_PRINTABLE_RADIUS)),
+            int(0.3333333333 * (DELTA_PRINTABLE_RADIUS))
+          #else
+            int(5), int(0.125 * _MIN(X_BED_SIZE, Y_BED_SIZE))
+          #endif
+        );
+        if (verbose_level > 3) {
+          SERIAL_ECHOPAIR("Start radius:", radius, " angle:", angle, " dir:");
+          if (dir > 0) SERIAL_CHAR('C');
+          SERIAL_ECHOLNPGM("CW");
+        }
+
+        // Move from leg to leg in rapid succession
+        LOOP_L_N(l, n_legs - 1) {
+
+          // Move some distance around the perimeter
+          float delta_angle;
+          if (schizoid_flag) {
+            // The points of a 5 point star are 72 degrees apart.
+            // Skip a point and go to the next one on the star.
+            delta_angle = dir * 2.0 * 72.0;
+          }
+          else {
+            // Just move further along the perimeter.
+            delta_angle = dir * (float)random(25, 45);
+          }
+          angle += delta_angle;
+
+          // Trig functions work without clamping, but just to be safe...
+          while (angle > 360.0) angle -= 360.0;
+          while (angle < 0.0) angle += 360.0;
+
+          // Choose the next position as an offset to chosen test position
+          const xy_pos_t noz_pos = test_position - probe.offset_xy;
+          xy_pos_t next_pos = {
+            noz_pos.x + float(cos(RADIANS(angle))) * radius,
+            noz_pos.y + float(sin(RADIANS(angle))) * radius
+          };
+
+          #if ENABLED(DELTA)
+            // If the probe can't reach the point on a round bed...
+            // Simply scale the numbers to bring them closer to origin.
+            while (!probe.can_reach(next_pos)) {
+              next_pos *= 0.8f;
+              if (verbose_level > 3)
+                SERIAL_ECHOLNPAIR_P(PSTR("Moving inward: X"), next_pos.x, SP_Y_STR, next_pos.y);
+            }
+          #elif HAS_ENDSTOPS
+            // For a rectangular bed just keep the probe in bounds
+            LIMIT(next_pos.x, X_MIN_POS, X_MAX_POS);
+            LIMIT(next_pos.y, Y_MIN_POS, Y_MAX_POS);
+          #endif
+
+          if (verbose_level > 3)
+            SERIAL_ECHOLNPAIR_P(PSTR("Going to: X"), next_pos.x, SP_Y_STR, next_pos.y);
+
+          do_blocking_move_to_xy(next_pos);
+        } // n_legs loop
+      } // n_legs
+
+      // Probe a single point
+      const float pz = probe.probe_at_point(test_position, raise_after, 0);
+
+      // Break the loop if the probe fails
+      probing_good = !isnan(pz);
+      if (!probing_good) break;
+
+      // Store the new sample
+      sample_set[n] = pz;
+
+      // Keep track of the largest and smallest samples
+      NOMORE(min, pz);
+      NOLESS(max, pz);
+
+      // Get the mean value of all samples thus far
+      sample_sum += pz;
+      mean = sample_sum / (n + 1);
+
+      // Calculate the standard deviation so far.
+      // The value after the last sample will be the final output.
+      float dev_sum = 0.0;
+      LOOP_LE_N(j, n) dev_sum += sq(sample_set[j] - mean);
+      sigma = SQRT(dev_sum / (n + 1));
+
+      if (verbose_level > 1) {
+        SERIAL_ECHO(n + 1);
+        SERIAL_ECHOPAIR(" of ", n_samples);
+        SERIAL_ECHOPAIR_F(": z: ", pz, 3);
+        SERIAL_CHAR(' ');
+        dev_report(verbose_level > 2, mean, sigma, min, max);
+        SERIAL_EOL();
+      }
+
+    } // n_samples loop
+  }
+
+  probe.stow();
+
+  if (probing_good) {
+    SERIAL_ECHOLNPGM("Finished!");
+    dev_report(verbose_level > 0, mean, sigma, min, max, true);
+
+    #if HAS_STATUS_MESSAGE
+      // Display M48 results in the status bar
+      char sigma_str[8];
+      ui.status_printf_P(0, PSTR(S_FMT ": %s"), GET_TEXT(MSG_M48_DEVIATION), dtostrf(sigma, 2, 6, sigma_str));
+    #endif
+  }
+
+  restore_feedrate_and_scaling();
+
+  // Re-enable bed level correction if it had been on
+  TERN_(HAS_LEVELING, set_bed_leveling_enabled(was_enabled));
+
+  report_current_position();
+}
+
+#endif // Z_MIN_PROBE_REPEATABILITY_TEST

Marlin/src/gcode/calibrate/M665.cpp

@@ -0,0 +1,112 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if IS_KINEMATIC
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+
+#if ENABLED(DELTA)
+
+  #include "../../module/delta.h"
+  /**
+   * M665: Set delta configurations
+   *
+   *    H = delta height
+   *    L = diagonal rod
+   *    R = delta radius
+   *    S = segments per second
+   *    X = Alpha (Tower 1) angle trim
+   *    Y = Beta  (Tower 2) angle trim
+   *    Z = Gamma (Tower 3) angle trim
+   *    A = Alpha (Tower 1) diagonal rod trim
+   *    B = Beta  (Tower 2) diagonal rod trim
+   *    C = Gamma (Tower 3) diagonal rod trim
+   */
+  void GcodeSuite::M665() {
+    if (parser.seenval('H')) delta_height              = parser.value_linear_units();
+    if (parser.seenval('L')) delta_diagonal_rod        = parser.value_linear_units();
+    if (parser.seenval('R')) delta_radius              = parser.value_linear_units();
+    if (parser.seenval('S')) segments_per_second       = parser.value_float();
+    if (parser.seenval('X')) delta_tower_angle_trim.a  = parser.value_float();
+    if (parser.seenval('Y')) delta_tower_angle_trim.b  = parser.value_float();
+    if (parser.seenval('Z')) delta_tower_angle_trim.c  = parser.value_float();
+    if (parser.seenval('A')) delta_diagonal_rod_trim.a = parser.value_float();
+    if (parser.seenval('B')) delta_diagonal_rod_trim.b = parser.value_float();
+    if (parser.seenval('C')) delta_diagonal_rod_trim.c = parser.value_float();
+    recalc_delta_settings();
+  }
+
+#elif IS_SCARA
+
+  #include "../../module/scara.h"
+
+  /**
+   * M665: Set SCARA settings
+   *
+   * Parameters:
+   *
+   *   S[segments-per-second] - Segments-per-second
+   *   P[theta-psi-offset]    - Theta-Psi offset, added to the shoulder (A/X) angle
+   *   T[theta-offset]        - Theta     offset, added to the elbow    (B/Y) angle
+   *   Z[z-offset]            - Z offset, added to Z
+   *
+   *   A, P, and X are all aliases for the shoulder angle
+   *   B, T, and Y are all aliases for the elbow angle
+   */
+  void GcodeSuite::M665() {
+    if (parser.seenval('S')) segments_per_second = parser.value_float();
+
+    #if HAS_SCARA_OFFSET
+
+      if (parser.seenval('Z')) scara_home_offset.z = parser.value_linear_units();
+
+      const bool hasA = parser.seenval('A'), hasP = parser.seenval('P'), hasX = parser.seenval('X');
+      const uint8_t sumAPX = hasA + hasP + hasX;
+      if (sumAPX) {
+        if (sumAPX == 1)
+          scara_home_offset.a = parser.value_float();
+        else {
+          SERIAL_ERROR_MSG("Only one of A, P, or X is allowed.");
+          return;
+        }
+      }
+
+      const bool hasB = parser.seenval('B'), hasT = parser.seenval('T'), hasY = parser.seenval('Y');
+      const uint8_t sumBTY = hasB + hasT + hasY;
+      if (sumBTY) {
+        if (sumBTY == 1)
+          scara_home_offset.b = parser.value_float();
+        else {
+          SERIAL_ERROR_MSG("Only one of B, T, or Y is allowed.");
+          return;
+        }
+      }
+
+    #endif // HAS_SCARA_OFFSET
+  }
+
+#endif
+
+#endif // IS_KINEMATIC

Marlin/src/gcode/calibrate/M666.cpp

@@ -0,0 +1,131 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(DELTA) || HAS_EXTRA_ENDSTOPS
+
+#include "../gcode.h"
+
+#if ENABLED(DELTA)
+  #include "../../module/delta.h"
+  #include "../../module/motion.h"
+#else
+  #include "../../module/endstops.h"
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../../core/debug_out.h"
+
+void M666_report(const bool forReplay=true) {
+  if (!forReplay) { SERIAL_ECHOLNPGM("; Endstop adjustment:"); SERIAL_ECHO_START(); }
+  #if ENABLED(DELTA)
+    SERIAL_ECHOLNPAIR_P(
+        PSTR("  M666 X"), LINEAR_UNIT(delta_endstop_adj.a)
+      , SP_Y_STR, LINEAR_UNIT(delta_endstop_adj.b)
+      , SP_Z_STR, LINEAR_UNIT(delta_endstop_adj.c)
+    );
+  #else
+    SERIAL_ECHOPGM("  M666");
+    #if ENABLED(X_DUAL_ENDSTOPS)
+      SERIAL_ECHOLNPAIR_P(SP_X_STR, LINEAR_UNIT(endstops.x2_endstop_adj));
+    #endif
+    #if ENABLED(Y_DUAL_ENDSTOPS)
+      SERIAL_ECHOLNPAIR_P(SP_Y_STR, LINEAR_UNIT(endstops.y2_endstop_adj));
+    #endif
+    #if ENABLED(Z_MULTI_ENDSTOPS)
+      #if NUM_Z_STEPPER_DRIVERS >= 3
+        SERIAL_ECHOPAIR(" S2 Z", LINEAR_UNIT(endstops.z3_endstop_adj));
+        if (!forReplay) SERIAL_ECHO_START();
+        SERIAL_ECHOPAIR("  M666 S3 Z", LINEAR_UNIT(endstops.z3_endstop_adj));
+        #if NUM_Z_STEPPER_DRIVERS >= 4
+          if (!forReplay) SERIAL_ECHO_START();
+          SERIAL_ECHOPAIR("  M666 S4 Z", LINEAR_UNIT(endstops.z4_endstop_adj));
+        #endif
+      #else
+        SERIAL_ECHOLNPAIR_P(SP_Z_STR, LINEAR_UNIT(endstops.z2_endstop_adj));
+      #endif
+    #endif
+  #endif
+}
+
+#if ENABLED(DELTA)
+
+  /**
+   * M666: Set delta endstop adjustment
+   */
+  void GcodeSuite::M666() {
+    DEBUG_SECTION(log_M666, "M666", DEBUGGING(LEVELING));
+    bool is_err = false, is_set = false;
+    LOOP_LINEAR_AXES(i) {
+      if (parser.seen(AXIS_CHAR(i))) {
+        is_set = true;
+        const float v = parser.value_linear_units();
+        if (v > 0)
+          is_err = true;
+        else {
+          delta_endstop_adj[i] = v;
+          if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("delta_endstop_adj[", AS_CHAR(AXIS_CHAR(i)), "] = ", v);
+        }
+      }
+    }
+    if (is_err) SERIAL_ECHOLNPAIR("?M666 offsets must be <= 0");
+    if (!is_set) M666_report();
+  }
+
+#else
+
+  /**
+   * M666: Set Dual Endstops offsets for X, Y, and/or Z.
+   *       With no parameters report current offsets.
+   *
+   * For Triple / Quad Z Endstops:
+   *   Set Z2 Only: M666 S2 Z<offset>
+   *   Set Z3 Only: M666 S3 Z<offset>
+   *   Set Z4 Only: M666 S4 Z<offset>
+   *       Set All: M666 Z<offset>
+   */
+  void GcodeSuite::M666() {
+    #if ENABLED(X_DUAL_ENDSTOPS)
+      if (parser.seenval('X')) endstops.x2_endstop_adj = parser.value_linear_units();
+    #endif
+    #if ENABLED(Y_DUAL_ENDSTOPS)
+      if (parser.seenval('Y')) endstops.y2_endstop_adj = parser.value_linear_units();
+    #endif
+    #if ENABLED(Z_MULTI_ENDSTOPS)
+      if (parser.seenval('Z')) {
+        const float z_adj = parser.value_linear_units();
+        #if NUM_Z_STEPPER_DRIVERS == 2
+          endstops.z2_endstop_adj = z_adj;
+        #else
+          const int ind = parser.intval('S');
+          #define _SET_ZADJ(N) if (!ind || ind == N) endstops.z##N##_endstop_adj = z_adj;
+          REPEAT_S(2, INCREMENT(NUM_Z_STEPPER_DRIVERS), _SET_ZADJ)
+        #endif
+      }
+    #endif
+    if (!parser.seen("XYZ")) M666_report();
+  }
+
+#endif // HAS_EXTRA_ENDSTOPS
+
+#endif // DELTA || HAS_EXTRA_ENDSTOPS

Marlin/src/gcode/calibrate/M852.cpp

@@ -0,0 +1,106 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(SKEW_CORRECTION_GCODE)
+
+#include "../gcode.h"
+#include "../../module/planner.h"
+
+/**
+ * M852: Get or set the machine skew factors. Reports current values with no arguments.
+ *
+ *  S[xy_factor] - Alias for 'I'
+ *  I[xy_factor] - New XY skew factor
+ *  J[xz_factor] - New XZ skew factor
+ *  K[yz_factor] - New YZ skew factor
+ */
+void GcodeSuite::M852() {
+  uint8_t ijk = 0, badval = 0, setval = 0;
+
+  if (parser.seen('I') || parser.seen('S')) {
+    ++ijk;
+    const float value = parser.value_linear_units();
+    if (WITHIN(value, SKEW_FACTOR_MIN, SKEW_FACTOR_MAX)) {
+      if (planner.skew_factor.xy != value) {
+        planner.skew_factor.xy = value;
+        ++setval;
+      }
+    }
+    else
+      ++badval;
+  }
+
+  #if ENABLED(SKEW_CORRECTION_FOR_Z)
+
+    if (parser.seen('J')) {
+      ++ijk;
+      const float value = parser.value_linear_units();
+      if (WITHIN(value, SKEW_FACTOR_MIN, SKEW_FACTOR_MAX)) {
+        if (planner.skew_factor.xz != value) {
+          planner.skew_factor.xz = value;
+          ++setval;
+        }
+      }
+      else
+        ++badval;
+    }
+
+    if (parser.seen('K')) {
+      ++ijk;
+      const float value = parser.value_linear_units();
+      if (WITHIN(value, SKEW_FACTOR_MIN, SKEW_FACTOR_MAX)) {
+        if (planner.skew_factor.yz != value) {
+          planner.skew_factor.yz = value;
+          ++setval;
+        }
+      }
+      else
+        ++badval;
+    }
+
+  #endif
+
+  if (badval)
+    SERIAL_ECHOLNPGM(STR_SKEW_MIN " " STRINGIFY(SKEW_FACTOR_MIN) " " STR_SKEW_MAX " " STRINGIFY(SKEW_FACTOR_MAX));
+
+  // When skew is changed the current position changes
+  if (setval) {
+    set_current_from_steppers_for_axis(ALL_AXES_ENUM);
+    sync_plan_position();
+    report_current_position();
+  }
+
+  if (!ijk) {
+    SERIAL_ECHO_START();
+    SERIAL_ECHOPGM("Skew Factor");
+    SERIAL_ECHOPAIR_F(" XY: ", planner.skew_factor.xy, 6);
+    #if ENABLED(SKEW_CORRECTION_FOR_Z)
+      SERIAL_ECHOPAIR_F(" XZ: ", planner.skew_factor.xz, 6);
+      SERIAL_ECHOPAIR_F(" YZ: ", planner.skew_factor.yz, 6);
+    #endif
+    SERIAL_EOL();
+  }
+}
+
+#endif // SKEW_CORRECTION_GCODE

Marlin/src/gcode/config/M200-M205.cpp

@@ -0,0 +1,186 @@
+/**
+ * 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 "../gcode.h"
+#include "../../MarlinCore.h"
+#include "../../module/planner.h"
+
+#if DISABLED(NO_VOLUMETRICS)
+
+  /**
+   * M200: Set filament diameter and set E axis units to cubic units
+   *
+   *    T<extruder> - Optional extruder number. Current extruder if omitted.
+   *    D<linear>   - Set filament diameter and enable. D0 disables volumetric.
+   *    S<bool>     - Turn volumetric ON or OFF.
+   *    L<float>    - Volumetric extruder limit (in mm^3/sec). L0 disables the limit.
+   */
+  void GcodeSuite::M200() {
+
+    const int8_t target_extruder = get_target_extruder_from_command();
+    if (target_extruder < 0) return;
+
+    bool vol_enable = parser.volumetric_enabled,
+         can_enable = true;
+
+    if (parser.seenval('D')) {
+      const float dval = parser.value_linear_units();
+      if (dval) { // Set filament size for volumetric calculation
+        planner.set_filament_size(target_extruder, dval);
+        vol_enable = true;    // Dn = enable for compatibility
+      }
+      else
+        can_enable = false;   // D0 = disable for compatibility
+    }
+
+    // Enable or disable with S1 / S0
+    parser.volumetric_enabled = can_enable && parser.boolval('S', vol_enable);
+
+    #if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT)
+      if (parser.seenval('L')) {
+        // Set volumetric limit (in mm^3/sec)
+        const float lval = parser.value_float();
+        if (WITHIN(lval, 0, 20))
+          planner.set_volumetric_extruder_limit(target_extruder, lval);
+        else
+          SERIAL_ECHOLNPGM("?L value out of range (0-20).");
+      }
+    #endif
+
+    planner.calculate_volumetric_multipliers();
+  }
+
+#endif // !NO_VOLUMETRICS
+
+/**
+ * M201: Set max acceleration in units/s^2 for print moves (M201 X1000 Y1000)
+ *
+ *       With multiple extruders use T to specify which one.
+ */
+void GcodeSuite::M201() {
+
+  const int8_t target_extruder = get_target_extruder_from_command();
+  if (target_extruder < 0) return;
+
+  #ifdef XY_FREQUENCY_LIMIT
+    if (parser.seenval('F')) planner.set_frequency_limit(parser.value_byte());
+    if (parser.seenval('G')) planner.xy_freq_min_speed_factor = constrain(parser.value_float(), 1, 100) / 100;
+  #endif
+
+  LOOP_LOGICAL_AXES(i) {
+    if (parser.seenval(axis_codes[i])) {
+      const uint8_t a = TERN(HAS_EXTRUDERS, (i == E_AXIS ? uint8_t(E_AXIS_N(target_extruder)) : i), i);
+      planner.set_max_acceleration(a, parser.value_axis_units((AxisEnum)a));
+    }
+  }
+}
+
+/**
+ * M203: Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in units/sec
+ *
+ *       With multiple extruders use T to specify which one.
+ */
+void GcodeSuite::M203() {
+
+  const int8_t target_extruder = get_target_extruder_from_command();
+  if (target_extruder < 0) return;
+
+  LOOP_LOGICAL_AXES(i)
+    if (parser.seenval(axis_codes[i])) {
+      const uint8_t a = TERN(HAS_EXTRUDERS, (i == E_AXIS ? uint8_t(E_AXIS_N(target_extruder)) : i), i);
+      planner.set_max_feedrate(a, parser.value_axis_units((AxisEnum)a));
+    }
+}
+
+/**
+ * M204: Set Accelerations in units/sec^2 (M204 P1200 R3000 T3000)
+ *
+ *    P = Printing moves
+ *    R = Retract only (no X, Y, Z) moves
+ *    T = Travel (non printing) moves
+ */
+void GcodeSuite::M204() {
+  if (!parser.seen("PRST")) {
+    SERIAL_ECHOPAIR("Acceleration: P", planner.settings.acceleration);
+    SERIAL_ECHOPAIR(" R", planner.settings.retract_acceleration);
+    SERIAL_ECHOLNPAIR_P(SP_T_STR, planner.settings.travel_acceleration);
+  }
+  else {
+    //planner.synchronize();
+    // 'S' for legacy compatibility. Should NOT BE USED for new development
+    if (parser.seenval('S')) planner.settings.travel_acceleration = planner.settings.acceleration = parser.value_linear_units();
+    if (parser.seenval('P')) planner.settings.acceleration = parser.value_linear_units();
+    if (parser.seenval('R')) planner.settings.retract_acceleration = parser.value_linear_units();
+    if (parser.seenval('T')) planner.settings.travel_acceleration = parser.value_linear_units();
+  }
+}
+
+/**
+ * M205: Set Advanced Settings
+ *
+ *    B = Min Segment Time (µs)
+ *    S = Min Feed Rate (units/s)
+ *    T = Min Travel Feed Rate (units/s)
+ *    X = Max X Jerk (units/sec^2)
+ *    Y = Max Y Jerk (units/sec^2)
+ *    Z = Max Z Jerk (units/sec^2)
+ *    E = Max E Jerk (units/sec^2)
+ *    J = Junction Deviation (mm) (If not using CLASSIC_JERK)
+ */
+void GcodeSuite::M205() {
+  if (!parser.seen("BST" TERN_(HAS_JUNCTION_DEVIATION, "J") TERN_(HAS_CLASSIC_JERK, "XYZE"))) return;
+
+  //planner.synchronize();
+  if (parser.seenval('B')) planner.settings.min_segment_time_us = parser.value_ulong();
+  if (parser.seenval('S')) planner.settings.min_feedrate_mm_s = parser.value_linear_units();
+  if (parser.seenval('T')) planner.settings.min_travel_feedrate_mm_s = parser.value_linear_units();
+  #if HAS_JUNCTION_DEVIATION
+    #if HAS_CLASSIC_JERK && (AXIS4_NAME == 'J' || AXIS5_NAME == 'J' || AXIS6_NAME == 'J')
+      #error "Can't set_max_jerk for 'J' axis because 'J' is used for Junction Deviation."
+    #endif
+    if (parser.seenval('J')) {
+      const float junc_dev = parser.value_linear_units();
+      if (WITHIN(junc_dev, 0.01f, 0.3f)) {
+        planner.junction_deviation_mm = junc_dev;
+        TERN_(LIN_ADVANCE, planner.recalculate_max_e_jerk());
+      }
+      else
+        SERIAL_ERROR_MSG("?J out of range (0.01 to 0.3)");
+    }
+  #endif
+  #if HAS_CLASSIC_JERK
+    bool seenZ = false;
+    LOGICAL_AXIS_CODE(
+      if (parser.seenval('E')) planner.set_max_jerk(E_AXIS, parser.value_linear_units()),
+      if (parser.seenval('X')) planner.set_max_jerk(X_AXIS, parser.value_linear_units()),
+      if (parser.seenval('Y')) planner.set_max_jerk(Y_AXIS, parser.value_linear_units()),
+      if ((seenZ = parser.seenval('Z'))) planner.set_max_jerk(Z_AXIS, parser.value_linear_units()),
+      if (parser.seenval(AXIS4_NAME)) planner.set_max_jerk(I_AXIS, parser.value_linear_units()),
+      if (parser.seenval(AXIS5_NAME)) planner.set_max_jerk(J_AXIS, parser.value_linear_units()),
+      if (parser.seenval(AXIS6_NAME)) planner.set_max_jerk(K_AXIS, parser.value_linear_units())
+    );
+    #if HAS_MESH && DISABLED(LIMITED_JERK_EDITING)
+      if (seenZ && planner.max_jerk.z <= 0.1f)
+        SERIAL_ECHOLNPGM("WARNING! Low Z Jerk may lead to unwanted pauses.");
+    #endif
+  #endif // HAS_CLASSIC_JERK
+}

Marlin/src/gcode/config/M217.cpp

@@ -0,0 +1,171 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if HAS_MULTI_EXTRUDER
+
+#include "../gcode.h"
+#include "../../module/tool_change.h"
+
+#if ENABLED(TOOLCHANGE_MIGRATION_FEATURE)
+  #include "../../module/motion.h"
+#endif
+
+#include "../../MarlinCore.h" // for SP_X_STR, etc.
+
+void M217_report(const bool eeprom=false) {
+
+  #if ENABLED(TOOLCHANGE_FILAMENT_SWAP)
+    SERIAL_ECHOPGM_P(eeprom ? PSTR("  M217") : PSTR("Toolchange:"));
+    SERIAL_ECHOPAIR(" S", LINEAR_UNIT(toolchange_settings.swap_length));
+    SERIAL_ECHOPAIR_P(SP_B_STR, LINEAR_UNIT(toolchange_settings.extra_resume),
+                      SP_E_STR, LINEAR_UNIT(toolchange_settings.extra_prime),
+                      SP_P_STR, LINEAR_UNIT(toolchange_settings.prime_speed));
+    SERIAL_ECHOPAIR(" R", LINEAR_UNIT(toolchange_settings.retract_speed),
+                    " U", LINEAR_UNIT(toolchange_settings.unretract_speed),
+                    " F", toolchange_settings.fan_speed,
+                    " G", toolchange_settings.fan_time);
+
+    #if ENABLED(TOOLCHANGE_MIGRATION_FEATURE)
+      SERIAL_ECHOPAIR(" A", migration.automode);
+      SERIAL_ECHOPAIR(" L", LINEAR_UNIT(migration.last));
+    #endif
+
+    #if ENABLED(TOOLCHANGE_PARK)
+      SERIAL_ECHOPAIR(" W", LINEAR_UNIT(toolchange_settings.enable_park));
+      SERIAL_ECHOPAIR_P(SP_X_STR, LINEAR_UNIT(toolchange_settings.change_point.x));
+      SERIAL_ECHOPAIR_P(SP_Y_STR, LINEAR_UNIT(toolchange_settings.change_point.y));
+    #endif
+
+    #if ENABLED(TOOLCHANGE_FS_PRIME_FIRST_USED)
+      SERIAL_ECHOPAIR(" V", LINEAR_UNIT(enable_first_prime));
+    #endif
+
+  #else
+
+    UNUSED(eeprom);
+
+  #endif
+
+  SERIAL_ECHOPAIR_P(SP_Z_STR, LINEAR_UNIT(toolchange_settings.z_raise));
+  SERIAL_EOL();
+}
+
+/**
+ * M217 - Set SINGLENOZZLE toolchange parameters
+ *
+ *  // Tool change command
+ *  Q           Prime active tool and exit
+ *
+ *  // Tool change settings
+ *  S[linear]   Swap length
+ *  B[linear]   Extra Swap length
+ *  E[linear]   Prime length
+ *  P[linear/m] Prime speed
+ *  R[linear/m] Retract speed
+ *  U[linear/m] UnRetract speed
+ *  V[linear]   0/1 Enable auto prime first extruder used
+ *  W[linear]   0/1 Enable park & Z Raise
+ *  X[linear]   Park X (Requires TOOLCHANGE_PARK)
+ *  Y[linear]   Park Y (Requires TOOLCHANGE_PARK)
+ *  Z[linear]   Z Raise
+ *  F[linear]   Fan Speed 0-255
+ *  G[linear/s] Fan time
+ *
+ * Tool migration settings
+ *  A[0|1]      Enable auto-migration on runout
+ *  L[index]    Last extruder to use for auto-migration
+ *
+ * Tool migration command
+ *  T[index]    Migrate to next extruder or the given extruder
+ */
+void GcodeSuite::M217() {
+
+  #if ENABLED(TOOLCHANGE_FILAMENT_SWAP)
+
+    static constexpr float max_extrude = TERN(PREVENT_LENGTHY_EXTRUDE, EXTRUDE_MAXLENGTH, 500);
+
+    if (parser.seen('Q')) { tool_change_prime(); return; }
+
+    if (parser.seenval('S')) { const float v = parser.value_linear_units(); toolchange_settings.swap_length = constrain(v, 0, max_extrude); }
+    if (parser.seenval('B')) { const float v = parser.value_linear_units(); toolchange_settings.extra_resume = constrain(v, -10, 10); }
+    if (parser.seenval('E')) { const float v = parser.value_linear_units(); toolchange_settings.extra_prime = constrain(v, 0, max_extrude); }
+    if (parser.seenval('P')) { const int16_t v = parser.value_linear_units(); toolchange_settings.prime_speed = constrain(v, 10, 5400); }
+    if (parser.seenval('R')) { const int16_t v = parser.value_linear_units(); toolchange_settings.retract_speed = constrain(v, 10, 5400); }
+    if (parser.seenval('U')) { const int16_t v = parser.value_linear_units(); toolchange_settings.unretract_speed = constrain(v, 10, 5400); }
+    #if TOOLCHANGE_FS_FAN >= 0 && HAS_FAN
+      if (parser.seenval('F')) { const int16_t v = parser.value_linear_units(); toolchange_settings.fan_speed = constrain(v, 0, 255); }
+      if (parser.seenval('G')) { const int16_t v = parser.value_linear_units(); toolchange_settings.fan_time = constrain(v, 1, 30); }
+    #endif
+  #endif
+
+  #if ENABLED(TOOLCHANGE_FS_PRIME_FIRST_USED)
+    if (parser.seenval('V')) { enable_first_prime = parser.value_linear_units(); }
+  #endif
+
+  #if ENABLED(TOOLCHANGE_PARK)
+    if (parser.seenval('W')) { toolchange_settings.enable_park = parser.value_linear_units(); }
+    if (parser.seenval('X')) { const int16_t v = parser.value_linear_units(); toolchange_settings.change_point.x = constrain(v, X_MIN_POS, X_MAX_POS); }
+    if (parser.seenval('Y')) { const int16_t v = parser.value_linear_units(); toolchange_settings.change_point.y = constrain(v, Y_MIN_POS, Y_MAX_POS); }
+  #endif
+
+  if (parser.seenval('Z')) { toolchange_settings.z_raise = parser.value_linear_units(); }
+
+  #if ENABLED(TOOLCHANGE_MIGRATION_FEATURE)
+    migration.target = 0;       // 0 = disabled
+
+    if (parser.seenval('L')) {  // Last
+      const int16_t lval = parser.value_int();
+      if (WITHIN(lval, 0, EXTRUDERS - 1)) {
+        migration.last = lval;
+        migration.automode = (active_extruder < migration.last);
+      }
+    }
+
+    if (parser.seen('A'))       // Auto on/off
+      migration.automode = parser.value_bool();
+
+    if (parser.seen('T')) {     // Migrate now
+      if (parser.has_value()) {
+        const int16_t tval = parser.value_int();
+        if (WITHIN(tval, 0, EXTRUDERS - 1) && tval != active_extruder) {
+          migration.target = tval + 1;
+          extruder_migration();
+          migration.target = 0; // disable
+          return;
+        }
+        else
+          migration.target = 0; // disable
+      }
+      else {
+        extruder_migration();
+        return;
+      }
+    }
+
+  #endif
+
+  M217_report();
+}
+
+#endif // HAS_MULTI_EXTRUDER

Marlin/src/gcode/config/M218.cpp

@@ -0,0 +1,71 @@
+/**
+ * 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_HOTEND_OFFSET
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+
+#if ENABLED(DELTA)
+  #include "../../module/planner.h"
+#endif
+
+/**
+ * M218 - set hotend offset (in linear units)
+ *
+ *   T<tool>
+ *   X<xoffset>
+ *   Y<yoffset>
+ *   Z<zoffset>
+ */
+void GcodeSuite::M218() {
+
+  const int8_t target_extruder = get_target_extruder_from_command();
+  if (target_extruder < 0) return;
+
+  if (parser.seenval('X')) hotend_offset[target_extruder].x = parser.value_linear_units();
+  if (parser.seenval('Y')) hotend_offset[target_extruder].y = parser.value_linear_units();
+  if (parser.seenval('Z')) hotend_offset[target_extruder].z = parser.value_linear_units();
+
+  if (!parser.seen("XYZ")) {
+    SERIAL_ECHO_START();
+    SERIAL_ECHOPGM(STR_HOTEND_OFFSET);
+    HOTEND_LOOP() {
+      SERIAL_CHAR(' ');
+      SERIAL_ECHO(hotend_offset[e].x);
+      SERIAL_CHAR(',');
+      SERIAL_ECHO(hotend_offset[e].y);
+      SERIAL_CHAR(',');
+      SERIAL_ECHO_F(hotend_offset[e].z, 3);
+    }
+    SERIAL_EOL();
+  }
+
+  #if ENABLED(DELTA)
+    if (target_extruder == active_extruder)
+      do_blocking_move_to_xy(current_position, planner.settings.max_feedrate_mm_s[X_AXIS]);
+  #endif
+}
+
+#endif // HAS_HOTEND_OFFSET

Marlin/src/gcode/config/M220.cpp

@@ -0,0 +1,51 @@
+/**
+ * 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 "../gcode.h"
+#include "../../module/motion.h"
+
+/**
+ * M220: Set speed percentage factor, aka "Feed Rate"
+ *
+ * Parameters
+ *   S<percent> : Set the feed rate percentage factor
+ *
+ * Report the current speed percentage factor if no parameter is specified
+ *
+ * For MMU2 and MMU2S devices...
+ *   B : Flag to back up the current factor
+ *   R : Flag to restore the last-saved factor
+ */
+void GcodeSuite::M220() {
+
+  static int16_t backup_feedrate_percentage = 100;
+  if (parser.seen('B')) backup_feedrate_percentage = feedrate_percentage;
+  if (parser.seen('R')) feedrate_percentage = backup_feedrate_percentage;
+
+  if (parser.seenval('S')) feedrate_percentage = parser.value_int();
+
+  if (!parser.seen_any()) {
+    SERIAL_ECHOPAIR("FR:", feedrate_percentage);
+    SERIAL_CHAR('%');
+    SERIAL_EOL();
+  }
+}

Marlin/src/gcode/config/M221.cpp

@@ -0,0 +1,47 @@
+/**
+ * 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 "../gcode.h"
+#include "../../module/planner.h"
+
+#if HAS_EXTRUDERS
+
+/**
+ * M221: Set extrusion percentage (M221 T0 S95)
+ */
+void GcodeSuite::M221() {
+
+  const int8_t target_extruder = get_target_extruder_from_command();
+  if (target_extruder < 0) return;
+
+  if (parser.seenval('S'))
+    planner.set_flow(target_extruder, parser.value_int());
+  else {
+    SERIAL_ECHO_START();
+    SERIAL_CHAR('E', '0' + target_extruder);
+    SERIAL_ECHOPAIR(" Flow: ", planner.flow_percentage[target_extruder]);
+    SERIAL_CHAR('%');
+    SERIAL_EOL();
+  }
+}
+
+#endif // EXTRUDERS

Marlin/src/gcode/config/M281.cpp

@@ -0,0 +1,68 @@
+/**
+ * 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(EDITABLE_SERVO_ANGLES)
+
+#include "../gcode.h"
+#include "../../module/servo.h"
+
+/**
+ * M281 - Edit / Report Servo Angles
+ *
+ *  P<index> - Servo to update
+ *  L<angle> - Deploy Angle
+ *  U<angle> - Stowed Angle
+ */
+void GcodeSuite::M281() {
+
+  if (!parser.seenval('P')) return;
+
+  const int servo_index = parser.value_int();
+  if (WITHIN(servo_index, 0, NUM_SERVOS - 1)) {
+    #if ENABLED(BLTOUCH)
+      if (servo_index == Z_PROBE_SERVO_NR) {
+        SERIAL_ERROR_MSG("BLTouch angles can't be changed.");
+        return;
+      }
+    #endif
+    bool angle_change = false;
+    if (parser.seen('L')) {
+      servo_angles[servo_index][0] = parser.value_int();
+      angle_change = true;
+    }
+    if (parser.seen('U')) {
+      servo_angles[servo_index][1] = parser.value_int();
+      angle_change = true;
+    }
+    if (!angle_change) {
+      SERIAL_ECHO_MSG(" Servo ", servo_index,
+                      " L", servo_angles[servo_index][0],
+                      " U", servo_angles[servo_index][1]);
+    }
+  }
+  else
+    SERIAL_ERROR_MSG("Servo ", servo_index, " out of range");
+
+}
+
+#endif // EDITABLE_SERVO_ANGLES

Marlin/src/gcode/config/M301.cpp

@@ -0,0 +1,91 @@
+/**
+ * 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(PIDTEMP)
+
+#include "../gcode.h"
+#include "../../module/temperature.h"
+
+/**
+ * M301: Set PID parameters P I D (and optionally C, L)
+ *
+ *   E[extruder] Default: 0
+ *
+ *   P[float] Kp term
+ *   I[float] Ki term (unscaled)
+ *   D[float] Kd term (unscaled)
+ *
+ * With PID_EXTRUSION_SCALING:
+ *
+ *   C[float] Kc term
+ *   L[int] LPQ length
+ *
+ * With PID_FAN_SCALING:
+ *
+ *   F[float] Kf term
+ */
+void GcodeSuite::M301() {
+
+  // multi-extruder PID patch: M301 updates or prints a single extruder's PID values
+  // default behavior (omitting E parameter) is to update for extruder 0 only
+  const uint8_t e = parser.byteval('E'); // extruder being updated
+
+  if (e < HOTENDS) { // catch bad input value
+    if (parser.seen('P')) PID_PARAM(Kp, e) = parser.value_float();
+    if (parser.seen('I')) PID_PARAM(Ki, e) = scalePID_i(parser.value_float());
+    if (parser.seen('D')) PID_PARAM(Kd, e) = scalePID_d(parser.value_float());
+    #if ENABLED(PID_EXTRUSION_SCALING)
+      if (parser.seen('C')) PID_PARAM(Kc, e) = parser.value_float();
+      if (parser.seenval('L')) thermalManager.lpq_len = parser.value_int();
+      NOMORE(thermalManager.lpq_len, LPQ_MAX_LEN);
+      NOLESS(thermalManager.lpq_len, 0);
+    #endif
+
+    #if ENABLED(PID_FAN_SCALING)
+      if (parser.seen('F')) PID_PARAM(Kf, e) = parser.value_float();
+    #endif
+
+    thermalManager.updatePID();
+
+    SERIAL_ECHO_START();
+    #if ENABLED(PID_PARAMS_PER_HOTEND)
+      SERIAL_ECHOPAIR(" e:", e); // specify extruder in serial output
+    #endif
+    SERIAL_ECHOPAIR(" p:", PID_PARAM(Kp, e),
+                    " i:", unscalePID_i(PID_PARAM(Ki, e)),
+                    " d:", unscalePID_d(PID_PARAM(Kd, e)));
+    #if ENABLED(PID_EXTRUSION_SCALING)
+      SERIAL_ECHOPAIR(" c:", PID_PARAM(Kc, e));
+    #endif
+    #if ENABLED(PID_FAN_SCALING)
+      SERIAL_ECHOPAIR(" f:", PID_PARAM(Kf, e));
+    #endif
+
+    SERIAL_EOL();
+  }
+  else
+    SERIAL_ERROR_MSG(STR_INVALID_EXTRUDER);
+}
+
+#endif // PIDTEMP

Marlin/src/gcode/config/M302.cpp

@@ -0,0 +1,63 @@
+/**
+ * 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(PREVENT_COLD_EXTRUSION)
+
+#include "../gcode.h"
+#include "../../module/temperature.h"
+
+/**
+ * M302: Allow cold extrudes, or set the minimum extrude temperature
+ *
+ *       S<temperature> sets the minimum extrude temperature
+ *       P<bool> enables (1) or disables (0) cold extrusion
+ *
+ *  Examples:
+ *
+ *       M302         ; report current cold extrusion state
+ *       M302 P0      ; enable cold extrusion checking
+ *       M302 P1      ; disable cold extrusion checking
+ *       M302 S0      ; always allow extrusion (disables checking)
+ *       M302 S170    ; only allow extrusion above 170
+ *       M302 S170 P1 ; set min extrude temp to 170 but leave disabled
+ */
+void GcodeSuite::M302() {
+  const bool seen_S = parser.seen('S');
+  if (seen_S) {
+    thermalManager.extrude_min_temp = parser.value_celsius();
+    thermalManager.allow_cold_extrude = (thermalManager.extrude_min_temp == 0);
+  }
+
+  if (parser.seen('P'))
+    thermalManager.allow_cold_extrude = (thermalManager.extrude_min_temp == 0) || parser.value_bool();
+  else if (!seen_S) {
+    // Report current state
+    SERIAL_ECHO_START();
+    SERIAL_ECHOPGM("Cold extrudes are ");
+    SERIAL_ECHOPGM_P(thermalManager.allow_cold_extrude ? PSTR("en") : PSTR("dis"));
+    SERIAL_ECHOLNPAIR("abled (min temp ", thermalManager.extrude_min_temp, "C)");
+  }
+}
+
+#endif // PREVENT_COLD_EXTRUSION

Marlin/src/gcode/config/M304.cpp

@@ -0,0 +1,49 @@
+/**
+ * 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(PIDTEMPBED)
+
+#include "../gcode.h"
+#include "../../module/temperature.h"
+
+/**
+ * M304 - Set and/or Report the current Bed PID values
+ *
+ *  P<pval> - Set the P value
+ *  I<ival> - Set the I value
+ *  D<dval> - Set the D value
+ */
+void GcodeSuite::M304() {
+
+  if (parser.seen('P')) thermalManager.temp_bed.pid.Kp = parser.value_float();
+  if (parser.seen('I')) thermalManager.temp_bed.pid.Ki = scalePID_i(parser.value_float());
+  if (parser.seen('D')) thermalManager.temp_bed.pid.Kd = scalePID_d(parser.value_float());
+
+  SERIAL_ECHO_MSG(" p:", thermalManager.temp_bed.pid.Kp,
+                  " i:", unscalePID_i(thermalManager.temp_bed.pid.Ki),
+                  " d:", unscalePID_d(thermalManager.temp_bed.pid.Kd));
+
+}
+
+#endif // PIDTEMPBED

Marlin/src/gcode/config/M305.cpp

@@ -0,0 +1,79 @@
+/**
+ * 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_USER_THERMISTORS
+
+#include "../gcode.h"
+#include "../../module/temperature.h"
+
+/**
+ * M305: Set (or report) custom thermistor parameters
+ *
+ *   P[index]  Thermistor table index
+ *   R[ohms]   Pullup resistor value
+ *   T[ohms]   Resistance at 25C
+ *   B[beta]   Thermistor "beta" value
+ *   C[coeff]  Steinhart-Hart Coefficient 'C'
+ *
+ *   Format: M305 P[tbl_index] R[pullup_resistor_val] T[therm_25C_resistance] B[therm_beta] C[Steinhart_Hart_C_coeff]
+ *
+ * Examples: M305 P0 R4700 T100000 B3950 C0.0
+ *           M305 P0 R4700
+ *           M305 P0 T100000
+ *           M305 P0 B3950
+ *           M305 P0 C0.0
+ */
+void GcodeSuite::M305() {
+  const int8_t t_index = parser.intval('P', -1);
+  const bool do_set = parser.seen("BCRT");
+
+  // A valid P index is required
+  if (t_index >= (USER_THERMISTORS) || (do_set && t_index < 0))
+    SERIAL_ECHO_MSG("!Invalid index. (0 <= P <= ", USER_THERMISTORS - 1, ")");
+  else if (do_set) {
+    if (parser.seen('R')) // Pullup resistor value
+      if (!thermalManager.set_pull_up_res(t_index, parser.value_float()))
+        SERIAL_ECHO_MSG("!Invalid series resistance. (0 < R < 1000000)");
+
+    if (parser.seen('T')) // Resistance at 25C
+      if (!thermalManager.set_res25(t_index, parser.value_float()))
+        SERIAL_ECHO_MSG("!Invalid 25C resistance. (0 < T < 10000000)");
+
+    if (parser.seen('B')) // Beta value
+      if (!thermalManager.set_beta(t_index, parser.value_float()))
+        SERIAL_ECHO_MSG("!Invalid beta. (0 < B < 1000000)");
+
+    if (parser.seen('C')) // Steinhart-Hart C coefficient
+      if (!thermalManager.set_sh_coeff(t_index, parser.value_float()))
+        SERIAL_ECHO_MSG("!Invalid Steinhart-Hart C coeff. (-0.01 < C < +0.01)");
+  }                       // If not setting then report parameters
+  else if (t_index < 0) { // ...all user thermistors
+    LOOP_L_N(i, USER_THERMISTORS)
+      thermalManager.log_user_thermistor(i);
+  }
+  else                    // ...one user thermistor
+    thermalManager.log_user_thermistor(t_index);
+}
+
+#endif // HAS_USER_THERMISTORS

Marlin/src/gcode/config/M309.cpp

@@ -0,0 +1,48 @@
+/**
+ * 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(PIDTEMPCHAMBER)
+
+#include "../gcode.h"
+#include "../../module/temperature.h"
+
+/**
+ * M309 - Set and/or Report the current Chamber PID values
+ *
+ *  P<pval> - Set the P value
+ *  I<ival> - Set the I value
+ *  D<dval> - Set the D value
+ */
+void GcodeSuite::M309() {
+  if (parser.seen('P')) thermalManager.temp_chamber.pid.Kp = parser.value_float();
+  if (parser.seen('I')) thermalManager.temp_chamber.pid.Ki = scalePID_i(parser.value_float());
+  if (parser.seen('D')) thermalManager.temp_chamber.pid.Kd = scalePID_d(parser.value_float());
+
+  SERIAL_ECHO_START();
+  SERIAL_ECHOLNPAIR(" p:", thermalManager.temp_chamber.pid.Kp,
+                    " i:", unscalePID_i(thermalManager.temp_chamber.pid.Ki),
+                    " d:", unscalePID_d(thermalManager.temp_chamber.pid.Kd));
+}
+
+#endif // PIDTEMPCHAMBER

Marlin/src/gcode/config/M43.cpp

@@ -0,0 +1,386 @@
+/**
+ * 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(PINS_DEBUGGING)
+
+#include "../gcode.h"
+#include "../../MarlinCore.h" // for pin_is_protected
+#include "../../pins/pinsDebug.h"
+#include "../../module/endstops.h"
+
+#if HAS_Z_SERVO_PROBE
+  #include "../../module/probe.h"
+  #include "../../module/servo.h"
+#endif
+
+#if ENABLED(BLTOUCH)
+  #include "../../feature/bltouch.h"
+#endif
+
+#if ENABLED(HOST_PROMPT_SUPPORT)
+  #include "../../feature/host_actions.h"
+#endif
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../../lcd/extui/ui_api.h"
+#endif
+
+#if HAS_RESUME_CONTINUE
+  #include "../../lcd/marlinui.h"
+#endif
+
+#ifndef GET_PIN_MAP_PIN_M43
+  #define GET_PIN_MAP_PIN_M43(Q) GET_PIN_MAP_PIN(Q)
+#endif
+
+inline void toggle_pins() {
+  const bool ignore_protection = parser.boolval('I');
+  const int repeat = parser.intval('R', 1),
+            start = PARSED_PIN_INDEX('S', 0),
+            end = PARSED_PIN_INDEX('L', NUM_DIGITAL_PINS - 1),
+            wait = parser.intval('W', 500);
+
+  LOOP_S_LE_N(i, start, end) {
+    pin_t pin = GET_PIN_MAP_PIN_M43(i);
+    if (!VALID_PIN(pin)) continue;
+    if (M43_NEVER_TOUCH(i) || (!ignore_protection && pin_is_protected(pin))) {
+      report_pin_state_extended(pin, ignore_protection, true, PSTR("Untouched "));
+      SERIAL_EOL();
+    }
+    else {
+      watchdog_refresh();
+      report_pin_state_extended(pin, ignore_protection, true, PSTR("Pulsing   "));
+      #ifdef __STM32F1__
+        const auto prior_mode = _GET_MODE(i);
+      #else
+        const bool prior_mode = GET_PINMODE(pin);
+      #endif
+      #if AVR_AT90USB1286_FAMILY // Teensy IDEs don't know about these pins so must use FASTIO
+        if (pin == TEENSY_E2) {
+          SET_OUTPUT(TEENSY_E2);
+          for (int16_t j = 0; j < repeat; j++) {
+            WRITE(TEENSY_E2, LOW);  safe_delay(wait);
+            WRITE(TEENSY_E2, HIGH); safe_delay(wait);
+            WRITE(TEENSY_E2, LOW);  safe_delay(wait);
+          }
+        }
+        else if (pin == TEENSY_E3) {
+          SET_OUTPUT(TEENSY_E3);
+          for (int16_t j = 0; j < repeat; j++) {
+            WRITE(TEENSY_E3, LOW);  safe_delay(wait);
+            WRITE(TEENSY_E3, HIGH); safe_delay(wait);
+            WRITE(TEENSY_E3, LOW);  safe_delay(wait);
+          }
+        }
+        else
+      #endif
+      {
+        pinMode(pin, OUTPUT);
+        for (int16_t j = 0; j < repeat; j++) {
+          watchdog_refresh(); extDigitalWrite(pin, 0); safe_delay(wait);
+          watchdog_refresh(); extDigitalWrite(pin, 1); safe_delay(wait);
+          watchdog_refresh(); extDigitalWrite(pin, 0); safe_delay(wait);
+          watchdog_refresh();
+        }
+      }
+      #ifdef __STM32F1__
+        _SET_MODE(i, prior_mode);
+      #else
+        pinMode(pin, prior_mode);
+      #endif
+    }
+    SERIAL_EOL();
+  }
+  SERIAL_ECHOLNPGM("Done.");
+
+} // toggle_pins
+
+inline void servo_probe_test() {
+
+  #if !(NUM_SERVOS > 0 && HAS_SERVO_0)
+
+    SERIAL_ERROR_MSG("SERVO not set up.");
+
+  #elif !HAS_Z_SERVO_PROBE
+
+    SERIAL_ERROR_MSG("Z_PROBE_SERVO_NR not set up.");
+
+  #else // HAS_Z_SERVO_PROBE
+
+    const uint8_t probe_index = parser.byteval('P', Z_PROBE_SERVO_NR);
+
+    SERIAL_ECHOLNPAIR("Servo probe test\n"
+                      ". using index:  ", probe_index,
+                      ", deploy angle: ", servo_angles[probe_index][0],
+                      ", stow angle:   ", servo_angles[probe_index][1]
+    );
+
+    bool deploy_state = false, stow_state;
+
+    #if ENABLED(Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN)
+
+      #define PROBE_TEST_PIN Z_MIN_PIN
+      constexpr bool probe_inverting = Z_MIN_ENDSTOP_INVERTING;
+
+      SERIAL_ECHOLNPAIR(". Probe Z_MIN_PIN: ", PROBE_TEST_PIN);
+      SERIAL_ECHOPGM(". Z_MIN_ENDSTOP_INVERTING: ");
+
+    #else
+
+      #define PROBE_TEST_PIN Z_MIN_PROBE_PIN
+      constexpr bool probe_inverting = Z_MIN_PROBE_ENDSTOP_INVERTING;
+
+      SERIAL_ECHOLNPAIR(". Probe Z_MIN_PROBE_PIN: ", PROBE_TEST_PIN);
+      SERIAL_ECHOPGM(   ". Z_MIN_PROBE_ENDSTOP_INVERTING: ");
+
+    #endif
+
+    serialprint_truefalse(probe_inverting);
+    SERIAL_EOL();
+
+    SET_INPUT_PULLUP(PROBE_TEST_PIN);
+
+    // First, check for a probe that recognizes an advanced BLTouch sequence.
+    // In addition to STOW and DEPLOY, it uses SW MODE (and RESET in the beginning)
+    // to see if this is one of the following: BLTOUCH Classic 1.2, 1.3,  or
+    // BLTouch Smart 1.0, 2.0, 2.2, 3.0, 3.1. But only if the user has actually
+    // configured a BLTouch as being present. If the user has not configured this,
+    // the BLTouch will be detected in the last phase of these tests (see further on).
+    bool blt = false;
+    // This code will try to detect a BLTouch probe or clone
+    #if ENABLED(BLTOUCH)
+      SERIAL_ECHOLNPGM(". Check for BLTOUCH");
+      bltouch._reset();
+      bltouch._stow();
+      if (probe_inverting == READ(PROBE_TEST_PIN)) {
+        bltouch._set_SW_mode();
+        if (probe_inverting != READ(PROBE_TEST_PIN)) {
+          bltouch._deploy();
+          if (probe_inverting == READ(PROBE_TEST_PIN)) {
+            bltouch._stow();
+            SERIAL_ECHOLNPGM("= BLTouch Classic 1.2, 1.3, Smart 1.0, 2.0, 2.2, 3.0, 3.1 detected.");
+            // Check for a 3.1 by letting the user trigger it, later
+            blt = true;
+        }
+      }
+    }
+    #endif
+
+    // The following code is common to all kinds of servo probes.
+    // Since it could be a real servo or a BLTouch (any kind) or a clone,
+    // use only "common" functions - i.e. SERVO_MOVE. No bltouch.xxxx stuff.
+
+    // If it is already recognised as a being a BLTouch, no need for this test
+    if (!blt) {
+      // DEPLOY and STOW 4 times and see if the signal follows
+      // Then it is a mechanical switch
+      uint8_t i = 0;
+      SERIAL_ECHOLNPGM(". Deploy & stow 4 times");
+      do {
+        MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][0]); // Deploy
+        safe_delay(500);
+        deploy_state = READ(PROBE_TEST_PIN);
+        MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][1]); // Stow
+        safe_delay(500);
+        stow_state = READ(PROBE_TEST_PIN);
+      } while (++i < 4);
+
+      if (probe_inverting != deploy_state) SERIAL_ECHOLNPGM("WARNING: INVERTING setting probably backwards.");
+
+      if (deploy_state != stow_state) {
+        SERIAL_ECHOLNPGM("= Mechanical Switch detected");
+        if (deploy_state) {
+          SERIAL_ECHOLNPAIR("  DEPLOYED state: HIGH (logic 1)",
+                            "  STOWED (triggered) state: LOW (logic 0)");
+        }
+        else {
+          SERIAL_ECHOLNPAIR("  DEPLOYED state: LOW (logic 0)",
+                            "  STOWED (triggered) state: HIGH (logic 1)");
+        }
+        #if ENABLED(BLTOUCH)
+          SERIAL_ECHOLNPGM("FAIL: BLTOUCH enabled - Set up this device as a Servo Probe with INVERTING set to 'true'.");
+        #endif
+        return;
+      }
+    }
+
+    // Ask the user for a trigger event and measure the pulse width.
+    MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][0]); // Deploy
+    safe_delay(500);
+    SERIAL_ECHOLNPGM("** Please trigger probe within 30 sec **");
+    uint16_t probe_counter = 0;
+
+    // Wait 30 seconds for user to trigger probe
+    for (uint16_t j = 0; j < 500 * 30 && probe_counter == 0 ; j++) {
+      safe_delay(2);
+
+      if (0 == j % (500 * 1)) gcode.reset_stepper_timeout();    // Keep steppers powered
+
+      if (deploy_state != READ(PROBE_TEST_PIN)) {               // probe triggered
+        for (probe_counter = 0; probe_counter < 15 && deploy_state != READ(PROBE_TEST_PIN); ++probe_counter) safe_delay(2);
+
+        SERIAL_ECHOPGM(". Pulse width");
+        if (probe_counter == 15)
+          SERIAL_ECHOLNPGM(": 30ms or more");
+        else
+          SERIAL_ECHOLNPAIR(" (+/- 4ms): ", probe_counter * 2);
+
+        if (probe_counter >= 4) {
+          if (probe_counter == 15) {
+            if (blt) SERIAL_ECHOPGM("= BLTouch V3.1");
+            else     SERIAL_ECHOPGM("= Z Servo Probe");
+          }
+          else SERIAL_ECHOPGM("= BLTouch pre V3.1 (or compatible)");
+          SERIAL_ECHOLNPGM(" detected.");
+        }
+        else SERIAL_ECHOLNPGM("FAIL: Noise detected - please re-run test");
+
+        MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][1]); // Stow
+        return;
+      }
+    }
+
+    if (!probe_counter) SERIAL_ECHOLNPGM("FAIL: No trigger detected");
+
+  #endif // HAS_Z_SERVO_PROBE
+
+} // servo_probe_test
+
+/**
+ * M43: Pin debug - report pin state, watch pins, toggle pins and servo probe test/report
+ *
+ *  M43         - report name and state of pin(s)
+ *                  P<pin>  Pin to read or watch. If omitted, reads all pins.
+ *                  I       Flag to ignore Marlin's pin protection.
+ *
+ *  M43 W       - Watch pins -reporting changes- until reset, click, or M108.
+ *                  P<pin>  Pin to read or watch. If omitted, read/watch all pins.
+ *                  I       Flag to ignore Marlin's pin protection.
+ *
+ *  M43 E<bool> - Enable / disable background endstop monitoring
+ *                  - Machine continues to operate
+ *                  - Reports changes to endstops
+ *                  - Toggles LED_PIN when an endstop changes
+ *                  - Cannot reliably catch the 5mS pulse from BLTouch type probes
+ *
+ *  M43 T       - Toggle pin(s) and report which pin is being toggled
+ *                  S<pin>  - Start Pin number.   If not given, will default to 0
+ *                  L<pin>  - End Pin number.   If not given, will default to last pin defined for this board
+ *                  I<bool> - Flag to ignore Marlin's pin protection.   Use with caution!!!!
+ *                  R       - Repeat pulses on each pin this number of times before continuing to next pin
+ *                  W       - Wait time (in milliseconds) between pulses.  If not given will default to 500
+ *
+ *  M43 S       - Servo probe test
+ *                  P<index> - Probe index (optional - defaults to 0
+ */
+void GcodeSuite::M43() {
+
+  // 'T' must be first. It uses 'S' and 'E' differently.
+  if (parser.seen('T')) return toggle_pins();
+
+  // 'E' Enable or disable endstop monitoring and return
+  if (parser.seen('E')) {
+    endstops.monitor_flag = parser.value_bool();
+    SERIAL_ECHOPGM("endstop monitor ");
+    SERIAL_ECHOPGM_P(endstops.monitor_flag ? PSTR("en") : PSTR("dis"));
+    SERIAL_ECHOLNPGM("abled");
+    return;
+  }
+
+  // 'S' Run servo probe test and return
+  if (parser.seen('S')) return servo_probe_test();
+
+  // 'P' Get the range of pins to test or watch
+  uint8_t first_pin = PARSED_PIN_INDEX('P', 0),
+          last_pin = parser.seenval('P') ? first_pin : NUMBER_PINS_TOTAL - 1;
+
+  if (first_pin > last_pin) return;
+
+  // 'I' to ignore protected pins
+  const bool ignore_protection = parser.boolval('I');
+
+  // 'W' Watch until click, M108, or reset
+  if (parser.boolval('W')) {
+    SERIAL_ECHOLNPGM("Watching pins");
+    #ifdef ARDUINO_ARCH_SAM
+      NOLESS(first_pin, 2); // Don't hijack the UART pins
+    #endif
+    uint8_t pin_state[last_pin - first_pin + 1];
+    LOOP_S_LE_N(i, first_pin, last_pin) {
+      pin_t pin = GET_PIN_MAP_PIN_M43(i);
+      if (!VALID_PIN(pin)) continue;
+      if (M43_NEVER_TOUCH(i) || (!ignore_protection && pin_is_protected(pin))) continue;
+      pinMode(pin, INPUT_PULLUP);
+      delay(1);
+        /*
+        if (IS_ANALOG(pin))
+          pin_state[pin - first_pin] = analogRead(DIGITAL_PIN_TO_ANALOG_PIN(pin)); // int16_t pin_state[...]
+        else
+        //*/
+          pin_state[i - first_pin] = extDigitalRead(pin);
+    }
+
+    #if HAS_RESUME_CONTINUE
+      KEEPALIVE_STATE(PAUSED_FOR_USER);
+      wait_for_user = true;
+      TERN_(HOST_PROMPT_SUPPORT, host_prompt_do(PROMPT_USER_CONTINUE, PSTR("M43 Wait Called"), CONTINUE_STR));
+      TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired_P(PSTR("M43 Wait Called")));
+    #endif
+
+    for (;;) {
+      LOOP_S_LE_N(i, first_pin, last_pin) {
+        pin_t pin = GET_PIN_MAP_PIN_M43(i);
+        if (!VALID_PIN(pin)) continue;
+        if (M43_NEVER_TOUCH(i) || (!ignore_protection && pin_is_protected(pin))) continue;
+        const byte val =
+          /*
+          IS_ANALOG(pin)
+            ? analogRead(DIGITAL_PIN_TO_ANALOG_PIN(pin)) : // int16_t val
+            :
+          //*/
+            extDigitalRead(pin);
+        if (val != pin_state[i - first_pin]) {
+          report_pin_state_extended(pin, ignore_protection, false);
+          pin_state[i - first_pin] = val;
+        }
+      }
+
+      #if HAS_RESUME_CONTINUE
+        ui.update();
+        if (!wait_for_user) break;
+      #endif
+
+      safe_delay(200);
+    }
+  }
+  else {
+    // Report current state of selected pin(s)
+    LOOP_S_LE_N(i, first_pin, last_pin) {
+      pin_t pin = GET_PIN_MAP_PIN_M43(i);
+      if (VALID_PIN(pin)) report_pin_state_extended(pin, ignore_protection, true);
+    }
+  }
+}
+
+#endif // PINS_DEBUGGING

Marlin/src/gcode/config/M540.cpp

@@ -0,0 +1,40 @@
+/**
+ * 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(SD_ABORT_ON_ENDSTOP_HIT)
+
+#include "../gcode.h"
+#include "../../module/stepper.h"
+
+/**
+ * M540: Set whether SD card print should abort on endstop hit (M540 S<0|1>)
+ */
+void GcodeSuite::M540() {
+
+  if (parser.seen('S'))
+    planner.abort_on_endstop_hit = parser.value_bool();
+
+}
+
+#endif // SD_ABORT_ON_ENDSTOP_HIT

Marlin/src/gcode/config/M575.cpp

@@ -0,0 +1,81 @@
+/**
+ * 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(BAUD_RATE_GCODE)
+
+#include "../gcode.h"
+
+/**
+ * M575 - Change serial baud rate
+ *
+ *   P<index>    - Serial port index. Omit for all.
+ *   B<baudrate> - Baud rate (bits per second)
+ */
+void GcodeSuite::M575() {
+  int32_t baud = parser.ulongval('B');
+  switch (baud) {
+    case   24:
+    case   96:
+    case  192:
+    case  384:
+    case  576:
+    case 1152: baud *= 100; break;
+    case  250:
+    case  500: baud *= 1000; break;
+    case   19: baud = 19200; break;
+    case   38: baud = 38400; break;
+    case   57: baud = 57600; break;
+    case  115: baud = 115200; break;
+  }
+  switch (baud) {
+    case 2400: case 9600: case 19200: case 38400: case 57600:
+    case 115200: case 250000: case 500000: case 1000000: {
+      const int8_t port = parser.intval('P', -99);
+      const bool set1 = (port == -99 || port == 0);
+      if (set1) SERIAL_ECHO_MSG(" Serial ", AS_CHAR('0'), " baud rate set to ", baud);
+      #if HAS_MULTI_SERIAL
+        const bool set2 = (port == -99 || port == 1);
+        if (set2) SERIAL_ECHO_MSG(" Serial ", AS_CHAR('1'), " baud rate set to ", baud);
+        #ifdef SERIAL_PORT_3
+          const bool set3 = (port == -99 || port == 2);
+          if (set3) SERIAL_ECHO_MSG(" Serial ", AS_CHAR('2'), " baud rate set to ", baud);
+        #endif
+      #endif
+
+      SERIAL_FLUSH();
+
+      if (set1) { MYSERIAL1.end(); MYSERIAL1.begin(baud); }
+      #if HAS_MULTI_SERIAL
+        if (set2) { MYSERIAL2.end(); MYSERIAL2.begin(baud); }
+        #ifdef SERIAL_PORT_3
+          if (set3) { MYSERIAL3.end(); MYSERIAL3.begin(baud); }
+        #endif
+      #endif
+
+    } break;
+    default: SERIAL_ECHO_MSG("?(B)aud rate implausible.");
+  }
+}
+
+#endif // BAUD_RATE_GCODE

Marlin/src/gcode/config/M672.cpp

@@ -0,0 +1,98 @@
+/**
+ * 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(DUET_SMART_EFFECTOR) && PIN_EXISTS(SMART_EFFECTOR_MOD)
+
+#include "../gcode.h"
+#include "../../HAL/shared/Delay.h"
+#include "../parser.h"
+
+/**
+ * The Marlin format for the M672 command is different than shown in the Duet Smart Effector
+ * documentation https://duet3d.dozuki.com/Wiki/Smart_effector_and_carriage_adapters_for_delta_printer
+ *
+ * To set custom sensitivity:
+ *   Duet:   M672 S105:aaa:bbb
+ *   Marlin: M672 Saaa
+ *
+ *   (where aaa is the desired sensitivity and bbb is 255 - aaa).
+ *
+ * Revert sensitivity to factory settings:
+ *   Duet:   M672 S105:131:131
+ *   Marlin: M672 R
+ */
+
+#define M672_PROGBYTE    105                // magic byte to start programming custom sensitivity
+#define M672_ERASEBYTE   131                // magic byte to clear custom sensitivity
+
+//
+// Smart Effector byte send protocol:
+//
+//  0  0  1  0       ... always 0010
+//  b7 b6 b5 b4 ~b4  ... hi bits, NOT last bit
+//  b3 b2 b1 b0 ~b0  ... lo bits, NOT last bit
+//
+void M672_send(uint8_t b) {    // bit rate requirement: 1KHz +/- 30%
+  LOOP_L_N(bits, 14) {
+    switch (bits) {
+      default: { OUT_WRITE(SMART_EFFECTOR_MOD_PIN, !!(b & 0x80)); b <<= 1; break; } // send bit, shift next into place
+      case  7:
+      case 12: { OUT_WRITE(SMART_EFFECTOR_MOD_PIN, !!(b & 0x80));          break; } // send bit. no shift
+      case  8:
+      case 13: { OUT_WRITE(SMART_EFFECTOR_MOD_PIN,  !(b & 0x80)); b <<= 1; break; } // send inverted previous bit
+      case  0: case  1:                                   // 00
+      case  3: { OUT_WRITE(SMART_EFFECTOR_MOD_PIN, LOW); break; }   // 0010
+      case  2: { OUT_WRITE(SMART_EFFECTOR_MOD_PIN, HIGH); break; }  // 001
+    }
+    DELAY_US(1000);
+  }
+}
+
+/**
+ * M672 - Set/reset Duet Smart Effector sensitivity
+ *
+ *  One of these is required:
+ *    S<sensitivity> - 0-255
+ *    R              - Flag to reset sensitivity to default
+ */
+void GcodeSuite::M672() {
+  if (parser.seen('R')) {
+    M672_send(M672_ERASEBYTE);
+    M672_send(M672_ERASEBYTE);
+  }
+  else if (parser.seenval('S')) {
+    const int8_t M672_sensitivity = parser.value_byte();
+    M672_send(M672_PROGBYTE);
+    M672_send(M672_sensitivity);
+    M672_send(255 - M672_sensitivity);
+  }
+  else {
+    SERIAL_ECHO_MSG("!'S' or 'R' parameter required.");
+    return;
+  }
+
+  OUT_WRITE(SMART_EFFECTOR_MOD_PIN, LOW);  // Keep Smart Effector in NORMAL mode
+}
+
+#endif // DUET_SMART_EFFECTOR && SMART_EFFECTOR_MOD_PIN

Marlin/src/gcode/config/M92.cpp

@@ -0,0 +1,119 @@
+/**
+ * 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 "../gcode.h"
+#include "../../module/planner.h"
+
+void report_M92(const bool echo=true, const int8_t e=-1) {
+  if (echo) SERIAL_ECHO_START(); else SERIAL_CHAR(' ');
+  SERIAL_ECHOPAIR_P(LIST_N(DOUBLE(LINEAR_AXES),
+    PSTR(" M92 X"), LINEAR_UNIT(planner.settings.axis_steps_per_mm[X_AXIS]),
+    SP_Y_STR, LINEAR_UNIT(planner.settings.axis_steps_per_mm[Y_AXIS]),
+    SP_Z_STR, LINEAR_UNIT(planner.settings.axis_steps_per_mm[Z_AXIS]),
+    SP_I_STR, LINEAR_UNIT(planner.settings.axis_steps_per_mm[I_AXIS]),
+    SP_J_STR, LINEAR_UNIT(planner.settings.axis_steps_per_mm[J_AXIS]),
+    SP_K_STR, LINEAR_UNIT(planner.settings.axis_steps_per_mm[K_AXIS]))
+  );
+  #if HAS_EXTRUDERS && DISABLED(DISTINCT_E_FACTORS)
+    SERIAL_ECHOPAIR_P(SP_E_STR, VOLUMETRIC_UNIT(planner.settings.axis_steps_per_mm[E_AXIS]));
+  #endif
+  SERIAL_EOL();
+
+  #if ENABLED(DISTINCT_E_FACTORS)
+    LOOP_L_N(i, E_STEPPERS) {
+      if (e >= 0 && i != e) continue;
+      if (echo) SERIAL_ECHO_START(); else SERIAL_CHAR(' ');
+      SERIAL_ECHOLNPAIR_P(PSTR(" M92 T"), i,
+                        SP_E_STR, VOLUMETRIC_UNIT(planner.settings.axis_steps_per_mm[E_AXIS_N(i)]));
+    }
+  #endif
+
+  UNUSED(e);
+}
+
+/**
+ * M92: Set axis steps-per-unit for one or more axes, X, Y, Z, and E.
+ *      (Follows the same syntax as G92)
+ *
+ *      With multiple extruders use T to specify which one.
+ *
+ *      If no argument is given print the current values.
+ *
+ *    With MAGIC_NUMBERS_GCODE:
+ *      Use 'H' and/or 'L' to get ideal layer-height information.
+ *      'H' specifies micro-steps to use. We guess if it's not supplied.
+ *      'L' specifies a desired layer height. Nearest good heights are shown.
+ */
+void GcodeSuite::M92() {
+
+  const int8_t target_extruder = get_target_extruder_from_command();
+  if (target_extruder < 0) return;
+
+  // No arguments? Show M92 report.
+  if (!parser.seen(
+    LOGICAL_AXIS_GANG("E", "X", "Y", "Z", AXIS4_STR, AXIS5_STR, AXIS6_STR)
+    TERN_(MAGIC_NUMBERS_GCODE, "HL")
+  )) return report_M92(true, target_extruder);
+
+  LOOP_LOGICAL_AXES(i) {
+    if (parser.seenval(axis_codes[i])) {
+      if (TERN1(HAS_EXTRUDERS, i != E_AXIS))
+        planner.settings.axis_steps_per_mm[i] = parser.value_per_axis_units((AxisEnum)i);
+      else {
+        #if HAS_EXTRUDERS
+          const float value = parser.value_per_axis_units((AxisEnum)(E_AXIS_N(target_extruder)));
+          if (value < 20) {
+            float factor = planner.settings.axis_steps_per_mm[E_AXIS_N(target_extruder)] / value; // increase e constants if M92 E14 is given for netfab.
+            #if HAS_CLASSIC_JERK && HAS_CLASSIC_E_JERK
+              planner.max_jerk.e *= factor;
+            #endif
+            planner.settings.max_feedrate_mm_s[E_AXIS_N(target_extruder)] *= factor;
+            planner.max_acceleration_steps_per_s2[E_AXIS_N(target_extruder)] *= factor;
+          }
+          planner.settings.axis_steps_per_mm[E_AXIS_N(target_extruder)] = value;
+        #endif
+      }
+    }
+  }
+  planner.refresh_positioning();
+
+  #if ENABLED(MAGIC_NUMBERS_GCODE)
+    #ifndef Z_MICROSTEPS
+      #define Z_MICROSTEPS 16
+    #endif
+    const float wanted = parser.floatval('L');
+    if (parser.seen('H') || wanted) {
+      const uint16_t argH = parser.ushortval('H'),
+                     micro_steps = argH ?: Z_MICROSTEPS;
+      const float z_full_step_mm = micro_steps * planner.steps_to_mm[Z_AXIS];
+      SERIAL_ECHO_START();
+      SERIAL_ECHOPAIR("{ micro_steps:", micro_steps, ", z_full_step_mm:", z_full_step_mm);
+      if (wanted) {
+        const float best = uint16_t(wanted / z_full_step_mm) * z_full_step_mm;
+        SERIAL_ECHOPAIR(", best:[", best);
+        if (best != wanted) { SERIAL_CHAR(','); SERIAL_DECIMAL(best + z_full_step_mm); }
+        SERIAL_CHAR(']');
+      }
+      SERIAL_ECHOLNPGM(" }");
+    }
+  #endif
+}

Marlin/src/gcode/control/M10-M11.cpp

@@ -0,0 +1,44 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2021 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(AIR_EVACUATION)
+
+#include "../gcode.h"
+#include "../../feature/spindle_laser.h"
+
+/**
+ * M10: Vacuum or Blower On
+ */
+void GcodeSuite::M10() {
+  cutter.air_evac_enable();   // Turn on Vacuum or Blower motor
+}
+
+/**
+ * M11: Vacuum or Blower OFF
+ */
+void GcodeSuite::M11() {
+  cutter.air_evac_disable();  // Turn off Vacuum or Blower motor
+}
+
+#endif // AIR_EVACUATION

Marlin/src/gcode/control/M108_M112_M410.cpp

@@ -0,0 +1,56 @@
+/**
+ * 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 DISABLED(EMERGENCY_PARSER)
+
+#include "../gcode.h"
+#include "../../MarlinCore.h" // for wait_for_heatup, kill, M112_KILL_STR
+#include "../../module/motion.h" // for quickstop_stepper
+
+/**
+ * M108: Stop the waiting for heaters in M109, M190, M303. Does not affect the target temperature.
+ */
+void GcodeSuite::M108() {
+  TERN_(HAS_RESUME_CONTINUE, wait_for_user = false);
+  wait_for_heatup = false;
+}
+
+/**
+ * M112: Full Shutdown
+ */
+void GcodeSuite::M112() {
+  kill(M112_KILL_STR, nullptr, true);
+}
+
+/**
+ * M410: Quickstop - Abort all planned moves
+ *
+ * This will stop the carriages mid-move, so most likely they
+ * will be out of sync with the stepper position after this.
+ */
+void GcodeSuite::M410() {
+  quickstop_stepper();
+}
+
+#endif // !EMERGENCY_PARSER

Marlin/src/gcode/control/M111.cpp

@@ -0,0 +1,77 @@
+/**
+ * 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 "../gcode.h"
+
+/**
+ * M111: Set the debug level
+ */
+void GcodeSuite::M111() {
+  if (parser.seen('S')) marlin_debug_flags = parser.byteval('S');
+
+  static PGMSTR(str_debug_1, STR_DEBUG_ECHO);
+  static PGMSTR(str_debug_2, STR_DEBUG_INFO);
+  static PGMSTR(str_debug_4, STR_DEBUG_ERRORS);
+  static PGMSTR(str_debug_8, STR_DEBUG_DRYRUN);
+  static PGMSTR(str_debug_16, STR_DEBUG_COMMUNICATION);
+  #if ENABLED(DEBUG_LEVELING_FEATURE)
+    static PGMSTR(str_debug_lvl, STR_DEBUG_LEVELING);
+  #endif
+
+  static PGM_P const debug_strings[] PROGMEM = {
+    str_debug_1, str_debug_2, str_debug_4, str_debug_8, str_debug_16,
+    TERN_(DEBUG_LEVELING_FEATURE, str_debug_lvl)
+  };
+
+  SERIAL_ECHO_START();
+  SERIAL_ECHOPGM(STR_DEBUG_PREFIX);
+  if (marlin_debug_flags) {
+    uint8_t comma = 0;
+    LOOP_L_N(i, COUNT(debug_strings)) {
+      if (TEST(marlin_debug_flags, i)) {
+        if (comma++) SERIAL_CHAR(',');
+        SERIAL_ECHOPGM_P((char*)pgm_read_ptr(&debug_strings[i]));
+      }
+    }
+  }
+  else {
+    SERIAL_ECHOPGM(STR_DEBUG_OFF);
+    #if !defined(__AVR__) || !defined(USBCON)
+      #if ENABLED(SERIAL_STATS_RX_BUFFER_OVERRUNS)
+        SERIAL_ECHOPAIR("\nBuffer Overruns: ", MYSERIAL1.buffer_overruns());
+      #endif
+
+      #if ENABLED(SERIAL_STATS_RX_FRAMING_ERRORS)
+        SERIAL_ECHOPAIR("\nFraming Errors: ", MYSERIAL1.framing_errors());
+      #endif
+
+      #if ENABLED(SERIAL_STATS_DROPPED_RX)
+        SERIAL_ECHOPAIR("\nDropped bytes: ", MYSERIAL1.dropped());
+      #endif
+
+      #if ENABLED(SERIAL_STATS_MAX_RX_QUEUED)
+        SERIAL_ECHOPAIR("\nMax RX Queue Size: ", MYSERIAL1.rxMaxEnqueued());
+      #endif
+    #endif // !__AVR__ || !USBCON
+  }
+  SERIAL_EOL();
+}

Marlin/src/gcode/control/M120_M121.cpp

@@ -0,0 +1,34 @@
+/**
+ * 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 "../gcode.h"
+#include "../../module/endstops.h"
+
+/**
+ * M120: Enable endstops and set non-homing endstop state to "enabled"
+ */
+void GcodeSuite::M120() { endstops.enable_globally(true); }
+
+/**
+ * M121: Disable endstops and set non-homing endstop state to "disabled"
+ */
+void GcodeSuite::M121() { endstops.enable_globally(false); }

Marlin/src/gcode/control/M17_M18_M84.cpp

@@ -0,0 +1,79 @@
+/**
+ * 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 "../gcode.h"
+#include "../../MarlinCore.h" // for stepper_inactive_time, disable_e_steppers
+#include "../../lcd/marlinui.h"
+#include "../../module/stepper.h"
+
+#if ENABLED(AUTO_BED_LEVELING_UBL)
+  #include "../../feature/bedlevel/bedlevel.h"
+#endif
+
+/**
+ * M17: Enable stepper motors
+ */
+void GcodeSuite::M17() {
+  if (parser.seen(LOGICAL_AXIS_GANG("E", "X", "Y", "Z", AXIS4_STR, AXIS5_STR, AXIS6_STR))) {
+    LOGICAL_AXIS_CODE(
+      if (TERN0(HAS_E_STEPPER_ENABLE, parser.seen_test('E'))) enable_e_steppers(),
+      if (parser.seen_test('X'))        ENABLE_AXIS_X(),
+      if (parser.seen_test('Y'))        ENABLE_AXIS_Y(),
+      if (parser.seen_test('Z'))        ENABLE_AXIS_Z(),
+      if (parser.seen_test(AXIS4_NAME)) ENABLE_AXIS_I(),
+      if (parser.seen_test(AXIS5_NAME)) ENABLE_AXIS_J(),
+      if (parser.seen_test(AXIS6_NAME)) ENABLE_AXIS_K()
+    );
+  }
+  else {
+    LCD_MESSAGEPGM(MSG_NO_MOVE);
+    enable_all_steppers();
+  }
+}
+
+/**
+ * M18, M84: Disable stepper motors
+ */
+void GcodeSuite::M18_M84() {
+  if (parser.seenval('S')) {
+    reset_stepper_timeout();
+    stepper_inactive_time = parser.value_millis_from_seconds();
+  }
+  else {
+    if (parser.seen(LOGICAL_AXIS_GANG("E", "X", "Y", "Z", AXIS4_STR, AXIS5_STR, AXIS6_STR))) {
+      planner.synchronize();
+      LOGICAL_AXIS_CODE(
+        if (TERN0(HAS_E_STEPPER_ENABLE, parser.seen_test('E'))) disable_e_steppers(),
+        if (parser.seen_test('X'))        DISABLE_AXIS_X(),
+        if (parser.seen_test('Y'))        DISABLE_AXIS_Y(),
+        if (parser.seen_test('Z'))        DISABLE_AXIS_Z(),
+        if (parser.seen_test(AXIS4_NAME)) DISABLE_AXIS_I(),
+        if (parser.seen_test(AXIS5_NAME)) DISABLE_AXIS_J(),
+        if (parser.seen_test(AXIS6_NAME)) DISABLE_AXIS_K()
+      );
+    }
+    else
+      planner.finish_and_disable();
+
+    TERN_(AUTO_BED_LEVELING_UBL, ubl.steppers_were_disabled());
+  }
+}

Marlin/src/gcode/control/M211.cpp

@@ -0,0 +1,46 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if HAS_SOFTWARE_ENDSTOPS
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+
+/**
+ * M211: Enable, Disable, and/or Report software endstops
+ *
+ * Usage: M211 S1 to enable, M211 S0 to disable, M211 alone for report
+ */
+void GcodeSuite::M211() {
+  const xyz_pos_t l_soft_min = soft_endstop.min.asLogical(),
+                  l_soft_max = soft_endstop.max.asLogical();
+  SERIAL_ECHO_START();
+  SERIAL_ECHOPGM(STR_SOFT_ENDSTOPS);
+  if (parser.seen('S')) soft_endstop._enabled = parser.value_bool();
+  serialprint_onoff(soft_endstop._enabled);
+  print_pos(l_soft_min, PSTR(STR_SOFT_MIN), PSTR(" "));
+  print_pos(l_soft_max, PSTR(STR_SOFT_MAX));
+}
+
+#endif

Marlin/src/gcode/control/M226.cpp

@@ -0,0 +1,60 @@
+/**
+ * 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(DIRECT_PIN_CONTROL)
+
+#include "../gcode.h"
+#include "../../MarlinCore.h" // for pin_is_protected and idle()
+#include "../../module/stepper.h"
+
+void protected_pin_err();
+
+/**
+ * M226: Wait until the specified pin reaches the state required (M226 P<pin> S<state>)
+ */
+void GcodeSuite::M226() {
+  if (parser.seen('P')) {
+    const int pin_number = PARSED_PIN_INDEX('P', 0),
+              pin_state = parser.intval('S', -1); // required pin state - default is inverted
+    const pin_t pin = GET_PIN_MAP_PIN(pin_number);
+
+    if (WITHIN(pin_state, -1, 1) && pin > -1) {
+      if (pin_is_protected(pin))
+        protected_pin_err();
+      else {
+        int target = LOW;
+        planner.synchronize();
+        pinMode(pin, INPUT);
+        switch (pin_state) {
+          case 1: target = HIGH; break;
+          case 0: target = LOW; break;
+          case -1: target = !extDigitalRead(pin); break;
+        }
+        while (int(extDigitalRead(pin)) != target) idle();
+      }
+    } // pin_state -1 0 1 && pin > -1
+  } // parser.seen('P')
+}
+
+#endif // DIRECT_PIN_CONTROL

Marlin/src/gcode/control/M280.cpp

@@ -0,0 +1,54 @@
+/**
+ * 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_SERVOS
+
+#include "../gcode.h"
+#include "../../module/servo.h"
+
+/**
+ * M280: Get or set servo position. P<index> [S<angle>]
+ */
+void GcodeSuite::M280() {
+
+  if (!parser.seen('P')) return;
+
+  const int servo_index = parser.value_int();
+  if (WITHIN(servo_index, 0, NUM_SERVOS - 1)) {
+    if (parser.seen('S')) {
+      const int a = parser.value_int();
+      if (a == -1)
+        servo[servo_index].detach();
+      else
+        MOVE_SERVO(servo_index, a);
+    }
+    else
+      SERIAL_ECHO_MSG(" Servo ", servo_index, ": ", servo[servo_index].read());
+  }
+  else
+    SERIAL_ERROR_MSG("Servo ", servo_index, " out of range");
+
+}
+
+#endif // HAS_SERVOS

Marlin/src/gcode/control/M3-M5.cpp

@@ -0,0 +1,140 @@
+/**
+ * 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_CUTTER
+
+#include "../gcode.h"
+#include "../../feature/spindle_laser.h"
+#include "../../module/stepper.h"
+
+/**
+ * Laser:
+ *  M3 - Laser ON/Power (Ramped power)
+ *  M4 - Laser ON/Power (Continuous power)
+ *
+ * Spindle:
+ *  M3 - Spindle ON (Clockwise)
+ *  M4 - Spindle ON (Counter-clockwise)
+ *
+ * Parameters:
+ *  S<power> - Set power. S0 will turn the spindle/laser off, except in relative mode.
+ *  O<ocr>   - Set power and OCR (oscillator count register)
+ *
+ *  If no PWM pin is defined then M3/M4 just turns it on.
+ *
+ *  At least 12.8KHz (50Hz * 256) is needed for Spindle PWM.
+ *  Hardware PWM is required on AVR. ISRs are too slow.
+ *
+ * NOTE: WGM for timers 3, 4, and 5 must be either Mode 1 or Mode 5.
+ *       No other settings give a PWM signal that goes from 0 to 5 volts.
+ *
+ *       The system automatically sets WGM to Mode 1, so no special
+ *       initialization is needed.
+ *
+ *       WGM bits for timer 2 are automatically set by the system to
+ *       Mode 1. This produces an acceptable 0 to 5 volt signal.
+ *       No special initialization is needed.
+ *
+ * NOTE: A minimum PWM frequency of 50 Hz is needed. All prescaler
+ *       factors for timers 2, 3, 4, and 5 are acceptable.
+ *
+ *  SPINDLE_LASER_ENA_PIN needs an external pullup or it may power on
+ *  the spindle/laser during power-up or when connecting to the host
+ *  (usually goes through a reset which sets all I/O pins to tri-state)
+ *
+ *  PWM duty cycle goes from 0 (off) to 255 (always on).
+ */
+void GcodeSuite::M3_M4(const bool is_M4) {
+  auto get_s_power = [] {
+    if (parser.seenval('S')) {
+      const float spwr = parser.value_float();
+      #if ENABLED(SPINDLE_SERVO)
+        cutter.unitPower = spwr;
+      #else
+        cutter.unitPower = TERN(SPINDLE_LASER_PWM,
+                              cutter.power_to_range(cutter_power_t(round(spwr))),
+                              spwr > 0 ? 255 : 0);
+      #endif
+    }
+    else
+      cutter.unitPower = cutter.cpwr_to_upwr(SPEED_POWER_STARTUP);
+    return cutter.unitPower;
+  };
+
+  #if ENABLED(LASER_POWER_INLINE)
+    if (parser.seen('I') == DISABLED(LASER_POWER_INLINE_INVERT)) {
+      // Laser power in inline mode
+      cutter.inline_direction(is_M4); // Should always be unused
+      #if ENABLED(SPINDLE_LASER_PWM)
+        if (parser.seen('O')) {
+          cutter.unitPower = cutter.power_to_range(parser.value_byte(), 0);
+          cutter.inline_ocr_power(cutter.unitPower); // The OCR is a value from 0 to 255 (uint8_t)
+        }
+        else
+          cutter.inline_power(cutter.upower_to_ocr(get_s_power()));
+      #else
+        cutter.set_inline_enabled(true);
+      #endif
+      return;
+    }
+    // Non-inline, standard case
+    cutter.inline_disable(); // Prevent future blocks re-setting the power
+  #endif
+
+  planner.synchronize();   // Wait for previous movement commands (G0/G0/G2/G3) to complete before changing power
+  cutter.set_reverse(is_M4);
+
+  #if ENABLED(SPINDLE_LASER_PWM)
+    if (parser.seenval('O')) {
+      cutter.unitPower = cutter.power_to_range(parser.value_byte(), 0);
+      cutter.set_ocr_power(cutter.unitPower); // The OCR is a value from 0 to 255 (uint8_t)
+    }
+    else
+      cutter.set_power(cutter.upower_to_ocr(get_s_power()));
+  #elif ENABLED(SPINDLE_SERVO)
+    cutter.set_power(get_s_power());
+  #else
+    cutter.set_enabled(true);
+  #endif
+  cutter.menuPower = cutter.unitPower;
+}
+
+/**
+ * M5 - Cutter OFF (when moves are complete)
+ */
+void GcodeSuite::M5() {
+  #if ENABLED(LASER_POWER_INLINE)
+    if (parser.seen('I') == DISABLED(LASER_POWER_INLINE_INVERT)) {
+      cutter.set_inline_enabled(false); // Laser power in inline mode
+      return;
+    }
+    // Non-inline, standard case
+    cutter.inline_disable(); // Prevent future blocks re-setting the power
+  #endif
+  planner.synchronize();
+  cutter.set_enabled(false);
+  cutter.menuPower = cutter.unitPower;
+}
+
+#endif // HAS_CUTTER

Marlin/src/gcode/control/M350_M351.cpp

@@ -0,0 +1,64 @@
+/**
+ * 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_MICROSTEPS
+
+#include "../gcode.h"
+#include "../../module/stepper.h"
+
+/**
+ * M350: Set axis microstepping modes. S sets mode for all drivers.
+ *
+ * Warning: Steps-per-unit remains unchanged.
+ */
+void GcodeSuite::M350() {
+  if (parser.seen('S')) LOOP_LE_N(i, 4) stepper.microstep_mode(i, parser.value_byte());
+  LOOP_LOGICAL_AXES(i) if (parser.seen(axis_codes[i])) stepper.microstep_mode(i, parser.value_byte());
+  if (parser.seen('B')) stepper.microstep_mode(4, parser.value_byte());
+  stepper.microstep_readings();
+}
+
+/**
+ * M351: Toggle MS1 MS2 pins directly with axis codes X Y Z E B
+ *       S# determines MS1, MS2 or MS3, X# sets the pin high/low.
+ */
+void GcodeSuite::M351() {
+  if (parser.seenval('S')) switch (parser.value_byte()) {
+    case 1:
+      LOOP_LOGICAL_AXES(i) if (parser.seenval(axis_codes[i])) stepper.microstep_ms(i, parser.value_byte(), -1, -1);
+      if (parser.seenval('B')) stepper.microstep_ms(4, parser.value_byte(), -1, -1);
+      break;
+    case 2:
+      LOOP_LOGICAL_AXES(i) if (parser.seenval(axis_codes[i])) stepper.microstep_ms(i, -1, parser.value_byte(), -1);
+      if (parser.seenval('B')) stepper.microstep_ms(4, -1, parser.value_byte(), -1);
+      break;
+    case 3:
+      LOOP_LOGICAL_AXES(i) if (parser.seenval(axis_codes[i])) stepper.microstep_ms(i, -1, -1, parser.value_byte());
+      if (parser.seenval('B')) stepper.microstep_ms(4, -1, -1, parser.value_byte());
+      break;
+  }
+  stepper.microstep_readings();
+}
+
+#endif // HAS_MICROSTEPS

Marlin/src/gcode/control/M380_M381.cpp

@@ -0,0 +1,56 @@
+/**
+ * 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 EITHER(EXT_SOLENOID, MANUAL_SOLENOID_CONTROL)
+
+#include "../gcode.h"
+#include "../../feature/solenoid.h"
+#include "../../module/motion.h"
+
+/**
+ * M380: Enable solenoid on the active extruder
+ *
+ *   S<index> to specify a solenoid (Requires MANUAL_SOLENOID_CONTROL)
+ */
+void GcodeSuite::M380() {
+  #if ENABLED(MANUAL_SOLENOID_CONTROL)
+    enable_solenoid(parser.intval('S', active_extruder));
+  #else
+    enable_solenoid_on_active_extruder();
+  #endif
+}
+
+/**
+ * M381: Disable all solenoids if EXT_SOLENOID
+ *       Disable selected/active solenoid if MANUAL_SOLENOID_CONTROL
+ */
+void GcodeSuite::M381() {
+  #if ENABLED(MANUAL_SOLENOID_CONTROL)
+    disable_solenoid(parser.intval('S', active_extruder));
+  #else
+    disable_all_solenoids();
+  #endif
+}
+
+#endif // EXT_SOLENOID || MANUAL_SOLENOID_CONTROL

Marlin/src/gcode/control/M400.cpp

@@ -0,0 +1,33 @@
+/**
+ * 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 "../gcode.h"
+#include "../../module/stepper.h"
+
+/**
+ * M400: Finish all moves
+ */
+void GcodeSuite::M400() {
+
+  planner.synchronize();
+
+}

Marlin/src/gcode/control/M42.cpp

@@ -0,0 +1,135 @@
+/**
+ * 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(DIRECT_PIN_CONTROL)
+
+#include "../gcode.h"
+#include "../../MarlinCore.h" // for pin_is_protected
+
+#if HAS_FAN
+  #include "../../module/temperature.h"
+#endif
+
+#ifdef MAPLE_STM32F1
+  // these are enums on the F1...
+  #define INPUT_PULLDOWN INPUT_PULLDOWN
+  #define INPUT_ANALOG INPUT_ANALOG
+  #define OUTPUT_OPEN_DRAIN OUTPUT_OPEN_DRAIN
+#endif
+
+void protected_pin_err() {
+  SERIAL_ERROR_MSG(STR_ERR_PROTECTED_PIN);
+}
+
+/**
+ * M42: Change pin status via GCode
+ *
+ *  P<pin>  Pin number (LED if omitted)
+ *          For LPC1768 specify pin P1_02 as M42 P102,
+ *                                  P1_20 as M42 P120, etc.
+ *
+ *  S<byte> Pin status from 0 - 255
+ *  I       Flag to ignore Marlin's pin protection
+ *
+ *  M<mode> Pin mode: 0=INPUT  1=OUTPUT  2=INPUT_PULLUP  3=INPUT_PULLDOWN
+ */
+void GcodeSuite::M42() {
+  const int pin_index = PARSED_PIN_INDEX('P', GET_PIN_MAP_INDEX(LED_PIN));
+  if (pin_index < 0) return;
+
+  const pin_t pin = GET_PIN_MAP_PIN(pin_index);
+
+  if (!parser.boolval('I') && pin_is_protected(pin)) return protected_pin_err();
+
+  bool avoidWrite = false;
+  if (parser.seenval('M')) {
+    switch (parser.value_byte()) {
+      case 0: pinMode(pin, INPUT); avoidWrite = true; break;
+      case 1: pinMode(pin, OUTPUT); break;
+      case 2: pinMode(pin, INPUT_PULLUP); avoidWrite = true; break;
+      #ifdef INPUT_PULLDOWN
+        case 3: pinMode(pin, INPUT_PULLDOWN); avoidWrite = true; break;
+      #endif
+      #ifdef INPUT_ANALOG
+        case 4: pinMode(pin, INPUT_ANALOG); avoidWrite = true; break;
+      #endif
+      #ifdef OUTPUT_OPEN_DRAIN
+        case 5: pinMode(pin, OUTPUT_OPEN_DRAIN); break;
+      #endif
+      default: SERIAL_ECHOLNPGM("Invalid Pin Mode"); return;
+    }
+  }
+
+  if (!parser.seenval('S')) return;
+  const byte pin_status = parser.value_byte();
+
+  #if HAS_FAN
+    switch (pin) {
+      #if HAS_FAN0
+        case FAN0_PIN: thermalManager.fan_speed[0] = pin_status; return;
+      #endif
+      #if HAS_FAN1
+        case FAN1_PIN: thermalManager.fan_speed[1] = pin_status; return;
+      #endif
+      #if HAS_FAN2
+        case FAN2_PIN: thermalManager.fan_speed[2] = pin_status; return;
+      #endif
+      #if HAS_FAN3
+        case FAN3_PIN: thermalManager.fan_speed[3] = pin_status; return;
+      #endif
+      #if HAS_FAN4
+        case FAN4_PIN: thermalManager.fan_speed[4] = pin_status; return;
+      #endif
+      #if HAS_FAN5
+        case FAN5_PIN: thermalManager.fan_speed[5] = pin_status; return;
+      #endif
+      #if HAS_FAN6
+        case FAN6_PIN: thermalManager.fan_speed[6] = pin_status; return;
+      #endif
+      #if HAS_FAN7
+        case FAN7_PIN: thermalManager.fan_speed[7] = pin_status; return;
+      #endif
+    }
+  #endif
+
+  if (avoidWrite) {
+    SERIAL_ECHOLNPGM("?Cannot write to INPUT");
+    return;
+  }
+
+  // An OUTPUT_OPEN_DRAIN should not be changed to normal OUTPUT (STM32)
+  // Use M42 Px M1/5 S0/1 to set the output type and then set value
+  #ifndef OUTPUT_OPEN_DRAIN
+    pinMode(pin, OUTPUT);
+  #endif
+  extDigitalWrite(pin, pin_status);
+
+  #ifdef ARDUINO_ARCH_STM32
+    // A simple I/O will be set to 0 by analogWrite()
+    if (pin_status <= 1) return;
+  #endif
+  analogWrite(pin, pin_status);
+}
+
+#endif // DIRECT_PIN_CONTROL

Marlin/src/gcode/control/M605.cpp

@@ -0,0 +1,190 @@
+/**
+ * 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_DUPLICATION_MODE
+
+//#define DEBUG_DXC_MODE
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+#include "../../module/stepper.h"
+#include "../../module/tool_change.h"
+#include "../../module/planner.h"
+
+#define DEBUG_OUT ENABLED(DEBUG_DXC_MODE)
+#include "../../core/debug_out.h"
+
+#if ENABLED(DUAL_X_CARRIAGE)
+
+  /**
+   * M605: Set dual x-carriage movement mode
+   *
+   *   M605 S0 : (FULL_CONTROL) The slicer has full control over both X-carriages and can achieve optimal travel
+   *             results as long as it supports dual X-carriages.
+   *
+   *   M605 S1 : (AUTO_PARK) The firmware automatically parks and unparks the X-carriages on tool-change so that
+   *             additional slicer support is not required.
+   *
+   *   M605 S2 X R : (DUPLICATION) The firmware moves the second X-carriage and extruder in synchronization with
+   *             the first X-carriage and extruder, to print 2 copies of the same object at the same time.
+   *             Set the constant X-offset and temperature differential with M605 S2 X[offs] R[deg] and
+   *             follow with "M605 S2" to initiate duplicated movement. For example, use "M605 S2 X100 R2" to
+   *             make a copy 100mm to the right with E1 2° hotter than E0.
+   *
+   *   M605 S3 : (MIRRORED) Formbot/Vivedino-inspired mirrored mode in which the second extruder duplicates
+   *             the movement of the first except the second extruder is reversed in the X axis.
+   *             The temperature differential and initial X offset must be set with "M605 S2 X[offs] R[deg]",
+   *             then followed by "M605 S3" to initiate mirrored movement.
+   *
+   *    M605 W  : IDEX What? command.
+   *
+   *    Note: the X axis should be homed after changing Dual X-carriage mode.
+   */
+  void GcodeSuite::M605() {
+    planner.synchronize();
+
+    if (parser.seen('S')) {
+      const DualXMode previous_mode = dual_x_carriage_mode;
+
+      dual_x_carriage_mode = (DualXMode)parser.value_byte();
+      idex_set_mirrored_mode(false);
+
+      switch (dual_x_carriage_mode) {
+
+        case DXC_FULL_CONTROL_MODE:
+        case DXC_AUTO_PARK_MODE:
+          break;
+
+        case DXC_DUPLICATION_MODE:
+          // Set the X offset, but no less than the safety gap
+          if (parser.seen('X')) duplicate_extruder_x_offset = _MAX(parser.value_linear_units(), (X2_MIN_POS) - (X1_MIN_POS));
+          if (parser.seen('R')) duplicate_extruder_temp_offset = parser.value_celsius_diff();
+          // Always switch back to tool 0
+          if (active_extruder != 0) tool_change(0);
+          break;
+
+        case DXC_MIRRORED_MODE: {
+          if (previous_mode != DXC_DUPLICATION_MODE) {
+            SERIAL_ECHOLNPGM("Printer must be in DXC_DUPLICATION_MODE prior to ");
+            SERIAL_ECHOLNPGM("specifying DXC_MIRRORED_MODE.");
+            dual_x_carriage_mode = DEFAULT_DUAL_X_CARRIAGE_MODE;
+            return;
+          }
+          idex_set_mirrored_mode(true);
+
+          // Do a small 'jog' motion in the X axis
+          xyze_pos_t dest = current_position; dest.x -= 0.1f;
+          for (uint8_t i = 2; --i;) {
+            planner.buffer_line(dest, feedrate_mm_s, 0);
+            dest.x += 0.1f;
+          }
+        } return;
+
+        default:
+          dual_x_carriage_mode = DEFAULT_DUAL_X_CARRIAGE_MODE;
+          break;
+      }
+
+      idex_set_parked(false);
+      set_duplication_enabled(false);
+
+      #ifdef EVENT_GCODE_IDEX_AFTER_MODECHANGE
+        gcode.process_subcommands_now_P(PSTR(EVENT_GCODE_IDEX_AFTER_MODECHANGE));
+      #endif
+    }
+    else if (!parser.seen('W'))  // if no S or W parameter, the DXC mode gets reset to the user's default
+      dual_x_carriage_mode = DEFAULT_DUAL_X_CARRIAGE_MODE;
+
+    #ifdef DEBUG_DXC_MODE
+
+      if (parser.seen('W')) {
+        DEBUG_ECHO_START();
+        DEBUG_ECHOPGM("Dual X Carriage Mode ");
+        switch (dual_x_carriage_mode) {
+          case DXC_FULL_CONTROL_MODE: DEBUG_ECHOPGM("FULL_CONTROL"); break;
+          case DXC_AUTO_PARK_MODE:    DEBUG_ECHOPGM("AUTO_PARK");    break;
+          case DXC_DUPLICATION_MODE:  DEBUG_ECHOPGM("DUPLICATION");  break;
+          case DXC_MIRRORED_MODE:     DEBUG_ECHOPGM("MIRRORED");     break;
+        }
+        DEBUG_ECHOPAIR("\nActive Ext: ", active_extruder);
+        if (!active_extruder_parked) DEBUG_ECHOPGM(" NOT ");
+        DEBUG_ECHOPGM(" parked.");
+        DEBUG_ECHOPAIR("\nactive_extruder_x_pos: ", current_position.x);
+        DEBUG_ECHOPAIR("\ninactive_extruder_x: ", inactive_extruder_x);
+        DEBUG_ECHOPAIR("\nextruder_duplication_enabled: ", extruder_duplication_enabled);
+        DEBUG_ECHOPAIR("\nduplicate_extruder_x_offset: ", duplicate_extruder_x_offset);
+        DEBUG_ECHOPAIR("\nduplicate_extruder_temp_offset: ", duplicate_extruder_temp_offset);
+        DEBUG_ECHOPAIR("\ndelayed_move_time: ", delayed_move_time);
+        DEBUG_ECHOPAIR("\nX1 Home X: ", x_home_pos(0), "\nX1_MIN_POS=", X1_MIN_POS, "\nX1_MAX_POS=", X1_MAX_POS);
+        DEBUG_ECHOPAIR("\nX2 Home X: ", x_home_pos(1), "\nX2_MIN_POS=", X2_MIN_POS, "\nX2_MAX_POS=", X2_MAX_POS);
+        DEBUG_ECHOPAIR("\nX2_HOME_DIR=", X2_HOME_DIR, "\nX2_HOME_POS=", X2_HOME_POS);
+        DEBUG_ECHOPAIR("\nDEFAULT_DUAL_X_CARRIAGE_MODE=", STRINGIFY(DEFAULT_DUAL_X_CARRIAGE_MODE));
+        DEBUG_ECHOPAIR("\toolchange_settings.z_raise=", toolchange_settings.z_raise);
+        DEBUG_ECHOPAIR("\nDEFAULT_DUPLICATION_X_OFFSET=", DEFAULT_DUPLICATION_X_OFFSET);
+        DEBUG_EOL();
+
+        HOTEND_LOOP() {
+          DEBUG_ECHOPAIR_P(SP_T_STR, e);
+          LOOP_LINEAR_AXES(a) DEBUG_ECHOPAIR("  hotend_offset[", e, "].", AS_CHAR(AXIS_CHAR(a) | 0x20), "=", hotend_offset[e][a]);
+          DEBUG_EOL();
+        }
+        DEBUG_EOL();
+      }
+    #endif // DEBUG_DXC_MODE
+  }
+
+#elif ENABLED(MULTI_NOZZLE_DUPLICATION)
+
+  /**
+   * M605: Set multi-nozzle duplication mode
+   *
+   *  S2       - Enable duplication mode
+   *  P[mask]  - Bit-mask of nozzles to include in the duplication set.
+   *             A value of 0 disables duplication.
+   *  E[index] - Last nozzle index to include in the duplication set.
+   *             A value of 0 disables duplication.
+   */
+  void GcodeSuite::M605() {
+    bool ena = false;
+    if (parser.seen("EPS")) {
+      planner.synchronize();
+      if (parser.seenval('P')) duplication_e_mask = parser.value_int();   // Set the mask directly
+      else if (parser.seenval('E')) duplication_e_mask = pow(2, parser.value_int() + 1) - 1; // Set the mask by E index
+      ena = (2 == parser.intval('S', extruder_duplication_enabled ? 2 : 0));
+      set_duplication_enabled(ena && (duplication_e_mask >= 3));
+    }
+    SERIAL_ECHO_START();
+    SERIAL_ECHOPGM(STR_DUPLICATION_MODE);
+    serialprint_onoff(extruder_duplication_enabled);
+    if (ena) {
+      SERIAL_ECHOPGM(" ( ");
+      HOTEND_LOOP() if (TEST(duplication_e_mask, e)) { SERIAL_ECHO(e); SERIAL_CHAR(' '); }
+      SERIAL_CHAR(')');
+    }
+    SERIAL_EOL();
+  }
+
+#endif // MULTI_NOZZLE_DUPLICATION
+
+#endif // HAS_DUPICATION_MODE

Marlin/src/gcode/control/M7-M9.cpp

@@ -0,0 +1,77 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if ANY(COOLANT_MIST, COOLANT_FLOOD, AIR_ASSIST)
+
+#include "../gcode.h"
+#include "../../module/planner.h"
+
+#if ENABLED(COOLANT_MIST)
+  /**
+   * M7: Mist Coolant On
+   */
+  void GcodeSuite::M7() {
+    planner.synchronize();                            // Wait for move to arrive
+    WRITE(COOLANT_MIST_PIN, !(COOLANT_MIST_INVERT));  // Turn on Mist coolant
+  }
+#endif
+
+#if EITHER(COOLANT_FLOOD, AIR_ASSIST)
+
+  #if ENABLED(AIR_ASSIST)
+    #include "../../feature/spindle_laser.h"
+  #endif
+
+  /**
+   * M8: Flood Coolant / Air Assist ON
+   */
+  void GcodeSuite::M8() {
+    planner.synchronize();                            // Wait for move to arrive
+    #if ENABLED(COOLANT_FLOOD)
+      WRITE(COOLANT_FLOOD_PIN, !(COOLANT_FLOOD_INVERT)); // Turn on Flood coolant
+    #endif
+    #if ENABLED(AIR_ASSIST)
+      cutter.air_assist_enable();                     // Turn on Air Assist
+    #endif
+  }
+
+#endif
+
+/**
+ * M9: Coolant / Air Assist OFF
+ */
+void GcodeSuite::M9() {
+  planner.synchronize();                              // Wait for move to arrive
+  #if ENABLED(COOLANT_MIST)
+    WRITE(COOLANT_MIST_PIN, COOLANT_MIST_INVERT);     // Turn off Mist coolant
+  #endif
+  #if ENABLED(COOLANT_FLOOD)
+    WRITE(COOLANT_FLOOD_PIN, COOLANT_FLOOD_INVERT);   // Turn off Flood coolant
+  #endif
+  #if ENABLED(AIR_ASSIST)
+    cutter.air_assist_disable();                      // Turn off Air Assist
+  #endif
+}
+
+#endif // COOLANT_MIST | COOLANT_FLOOD | AIR_ASSIST

Marlin/src/gcode/control/M80_M81.cpp

@@ -0,0 +1,99 @@
+/**
+ * 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 "../gcode.h"
+
+#include "../../module/temperature.h"
+#include "../../module/planner.h"       // for planner.finish_and_disable
+#include "../../module/printcounter.h"  // for print_job_timer.stop
+#include "../../lcd/marlinui.h"         // for LCD_MESSAGEPGM_P
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(PSU_CONTROL)
+  #include "../queue.h"
+  #include "../../feature/power.h"
+#endif
+
+#if HAS_SUICIDE
+  #include "../../MarlinCore.h"
+#endif
+
+#if ENABLED(PSU_CONTROL)
+
+  /**
+   * M80   : Turn on the Power Supply
+   * M80 S : Report the current state and exit
+   */
+  void GcodeSuite::M80() {
+
+    // S: Report the current power supply state and exit
+    if (parser.seen('S')) {
+      SERIAL_ECHOPGM_P(powerManager.psu_on ? PSTR("PS:1\n") : PSTR("PS:0\n"));
+      return;
+    }
+
+    powerManager.power_on();
+
+    /**
+     * If you have a switch on suicide pin, this is useful
+     * if you want to start another print with suicide feature after
+     * a print without suicide...
+     */
+    #if HAS_SUICIDE
+      OUT_WRITE(SUICIDE_PIN, !SUICIDE_PIN_INVERTING);
+    #endif
+
+    TERN_(HAS_LCD_MENU, ui.reset_status());
+  }
+
+#endif // PSU_CONTROL
+
+/**
+ * M81: Turn off Power, including Power Supply, if there is one.
+ *
+ *      This code should ALWAYS be available for FULL SHUTDOWN!
+ */
+void GcodeSuite::M81() {
+  thermalManager.disable_all_heaters();
+  planner.finish_and_disable();
+
+  print_job_timer.stop();
+
+  #if HAS_FAN
+    thermalManager.zero_fan_speeds();
+    #if ENABLED(PROBING_FANS_OFF)
+      thermalManager.fans_paused = false;
+      ZERO(thermalManager.saved_fan_speed);
+    #endif
+  #endif
+
+  safe_delay(1000); // Wait 1 second before switching off
+
+  #if HAS_SUICIDE
+    suicide();
+  #elif ENABLED(PSU_CONTROL)
+    powerManager.power_off_soon();
+  #endif
+
+  LCD_MESSAGEPGM_P(PSTR(MACHINE_NAME " " STR_OFF "."));
+}

Marlin/src/gcode/control/M85.cpp

@@ -0,0 +1,35 @@
+/**
+ * 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 "../gcode.h"
+
+/**
+ * M85: Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
+ */
+void GcodeSuite::M85() {
+
+  if (parser.seen('S')) {
+    reset_stepper_timeout();
+    max_inactive_time = parser.value_millis_from_seconds();
+  }
+
+}

Marlin/src/gcode/control/M993_M994.cpp

@@ -0,0 +1,88 @@
+/**
+ * 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 ALL(HAS_SPI_FLASH, SDSUPPORT, MARLIN_DEV_MODE)
+
+#include "../gcode.h"
+#include "../../sd/cardreader.h"
+#include "../../libs/W25Qxx.h"
+
+/**
+ * M993: Backup SPI Flash to SD
+ */
+void GcodeSuite::M993() {
+  if (!card.isMounted()) card.mount();
+
+  char fname[] = "spiflash.bin";
+  card.openFileWrite(fname);
+  if (!card.isFileOpen()) {
+    SERIAL_ECHOLNPAIR("Failed to open ", fname, " to write.");
+    return;
+  }
+
+  uint8_t buf[1024];
+  uint32_t addr = 0;
+  W25QXX.init(SPI_QUARTER_SPEED);
+  SERIAL_ECHOPGM("Save SPI Flash");
+  while (addr < SPI_FLASH_SIZE) {
+    W25QXX.SPI_FLASH_BufferRead(buf, addr, COUNT(buf));
+    addr += COUNT(buf);
+    card.write(buf, COUNT(buf));
+    if (addr % (COUNT(buf) * 10) == 0) SERIAL_CHAR('.');
+  }
+  SERIAL_ECHOLNPGM(" done");
+
+  card.closefile();
+}
+
+/**
+ * M994: Load a backup from SD to SPI Flash
+ */
+void GcodeSuite::M994() {
+  if (!card.isMounted()) card.mount();
+
+  char fname[] = "spiflash.bin";
+  card.openFileRead(fname);
+  if (!card.isFileOpen()) {
+    SERIAL_ECHOLNPAIR("Failed to open ", fname, " to read.");
+    return;
+  }
+
+  uint8_t buf[1024];
+  uint32_t addr = 0;
+  W25QXX.init(SPI_QUARTER_SPEED);
+  W25QXX.SPI_FLASH_BulkErase();
+  SERIAL_ECHOPGM("Load SPI Flash");
+  while (addr < SPI_FLASH_SIZE) {
+    card.read(buf, COUNT(buf));
+    W25QXX.SPI_FLASH_BufferWrite(buf, addr, COUNT(buf));
+    addr += COUNT(buf);
+    if (addr % (COUNT(buf) * 10) == 0) SERIAL_CHAR('.');
+  }
+  SERIAL_ECHOLNPGM(" done");
+
+  card.closefile();
+}
+
+#endif // HAS_SPI_FLASH && SDSUPPORT && MARLIN_DEV_MODE

Marlin/src/gcode/control/M997.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/>.
+ *
+ */
+
+#include "../gcode.h"
+
+#if ENABLED(PLATFORM_M997_SUPPORT)
+
+/**
+ * M997: Perform in-application firmware update
+ */
+void GcodeSuite::M997() {
+
+  flashFirmware(parser.intval('S'));
+
+}
+
+#endif

Marlin/src/gcode/control/M999.cpp

@@ -0,0 +1,45 @@
+/**
+ * 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 "../gcode.h"
+
+#include "../../lcd/marlinui.h" // for lcd_reset_alert_level
+#include "../../MarlinCore.h"   // for marlin_state
+#include "../queue.h"           // for flush_and_request_resend
+
+/**
+ * M999: Restart after being stopped
+ *
+ * Default behavior is to flush the serial buffer and request
+ * a resend to the host starting on the last N line received.
+ *
+ * Sending "M999 S1" will resume printing without flushing the
+ * existing command buffer.
+ */
+void GcodeSuite::M999() {
+  marlin_state = MF_RUNNING;
+  ui.reset_alert_level();
+
+  if (parser.boolval('S')) return;
+
+  queue.flush_and_request_resend(queue.ring_buffer.command_port());
+}

Marlin/src/gcode/control/T.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/>.
+ *
+ */
+
+#include "../gcode.h"
+#include "../../module/tool_change.h"
+
+#if EITHER(HAS_MULTI_EXTRUDER, DEBUG_LEVELING_FEATURE)
+  #include "../../module/motion.h"
+#endif
+
+#if HAS_PRUSA_MMU2
+  #include "../../feature/mmu/mmu2.h"
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../../core/debug_out.h"
+
+/**
+ * T0-T<n>: Switch tool, usually switching extruders
+ *
+ *   F[units/min] Set the movement feedrate
+ *   S1           Don't move the tool in XY after change
+ *
+ * For PRUSA_MMU2(S) and EXTENDABLE_EMU_MMU2(S)
+ *   T[n] Gcode to extrude at least 38.10 mm at feedrate 19.02 mm/s must follow immediately to load to extruder wheels.
+ *   T?   Gcode to extrude shouldn't have to follow. Load to extruder wheels is done automatically.
+ *   Tx   Same as T?, but nozzle doesn't have to be preheated. Tc requires a preheated nozzle to finish filament load.
+ *   Tc   Load to nozzle after filament was prepared by Tc and nozzle is already heated.
+ */
+void GcodeSuite::T(const int8_t tool_index) {
+
+  DEBUG_SECTION(log_T, "T", DEBUGGING(LEVELING));
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("...(", tool_index, ")");
+
+  // Count this command as movement / activity
+  reset_stepper_timeout();
+
+  #if HAS_PRUSA_MMU2
+    if (parser.string_arg) {
+      mmu2.tool_change(parser.string_arg);   // Special commands T?/Tx/Tc
+      return;
+    }
+  #endif
+
+  tool_change(tool_index
+    #if HAS_MULTI_EXTRUDER
+      ,  TERN(PARKING_EXTRUDER, false, tool_index == active_extruder) // For PARKING_EXTRUDER motion is decided in tool_change()
+      || parser.boolval('S')
+    #endif
+  );
+}

Marlin/src/gcode/eeprom/M500-M504.cpp

@@ -0,0 +1,104 @@
+/**
+ * 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 "../gcode.h"
+#include "../../module/settings.h"
+#include "../../core/serial.h"
+#include "../../inc/MarlinConfig.h"
+
+/**
+ * M500: Store settings in EEPROM
+ */
+void GcodeSuite::M500() {
+  (void)settings.save();
+}
+
+/**
+ * M501: Read settings from EEPROM
+ */
+void GcodeSuite::M501() {
+  (void)settings.load();
+}
+
+/**
+ * M502: Revert to default settings
+ */
+void GcodeSuite::M502() {
+  (void)settings.reset();
+}
+
+#if DISABLED(DISABLE_M503)
+
+  /**
+   * M503: print settings currently in memory
+   */
+  void GcodeSuite::M503() {
+    (void)settings.report(!parser.boolval('S', true));
+  }
+
+#endif // !DISABLE_M503
+
+#if ENABLED(EEPROM_SETTINGS)
+
+  #if ENABLED(MARLIN_DEV_MODE)
+    #include "../../libs/hex_print.h"
+  #endif
+
+  /**
+   * M504: Validate EEPROM Contents
+   */
+  void GcodeSuite::M504() {
+    #if ENABLED(MARLIN_DEV_MODE)
+      const bool dowrite = parser.seenval('W');
+      if (dowrite || parser.seenval('R')) {
+        uint8_t val = 0;
+        int addr = parser.value_ushort();
+        if (dowrite) {
+          val = parser.byteval('V');
+          persistentStore.write_data(addr, &val);
+          SERIAL_ECHOLNPAIR("Wrote address ", addr, " with ", val);
+        }
+        else {
+          if (parser.seenval('T')) {
+            const int endaddr = parser.value_ushort();
+            while (addr <= endaddr) {
+              persistentStore.read_data(addr, &val);
+              SERIAL_ECHOLNPAIR("0x", hex_word(addr), ":", hex_byte(val));
+              addr++;
+              safe_delay(10);
+            }
+            SERIAL_EOL();
+          }
+          else {
+            persistentStore.read_data(addr, &val);
+            SERIAL_ECHOLNPAIR("Read address ", addr, " and got ", val);
+          }
+        }
+        return;
+      }
+    #endif
+
+    if (settings.validate())
+      SERIAL_ECHO_MSG("EEPROM OK");
+  }
+
+#endif

Marlin/src/gcode/feature/L6470/M122.cpp

@@ -0,0 +1,151 @@
+/**
+ * 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_L64XX
+
+#include "../../gcode.h"
+#include "../../../libs/L64XX/L64XX_Marlin.h"
+#include "../../../module/stepper/indirection.h"
+
+void echo_yes_no(const bool yes);
+
+inline void L6470_say_status(const L64XX_axis_t axis) {
+  if (L64xxManager.spi_abort) return;
+  const L64XX_Marlin::L64XX_shadow_t &sh = L64xxManager.shadow;
+  L64xxManager.get_status(axis);
+  L64xxManager.say_axis(axis);
+  #if ENABLED(L6470_CHITCHAT)
+    char temp_buf[20];
+    sprintf_P(temp_buf, PSTR("   status: %4x   "), sh.STATUS_AXIS_RAW);
+    SERIAL_ECHO(temp_buf);
+    print_bin(sh.STATUS_AXIS_RAW);
+    switch (sh.STATUS_AXIS_LAYOUT) {
+      case L6470_STATUS_LAYOUT: SERIAL_ECHOPGM("   L6470"); break;
+      case L6474_STATUS_LAYOUT: SERIAL_ECHOPGM("   L6474"); break;
+      case L6480_STATUS_LAYOUT: SERIAL_ECHOPGM("   L6480/powerSTEP01"); break;
+    }
+  #endif
+  SERIAL_ECHOPGM("\n...OUTPUT: ");
+  SERIAL_ECHOPGM_P(sh.STATUS_AXIS & STATUS_HIZ ? PSTR("OFF") : PSTR("ON "));
+  SERIAL_ECHOPGM("   BUSY: "); echo_yes_no((sh.STATUS_AXIS & STATUS_BUSY) == 0);
+  SERIAL_ECHOPGM("   DIR: ");
+  SERIAL_ECHOPGM_P((((sh.STATUS_AXIS & STATUS_DIR) >> 4) ^ L64xxManager.index_to_dir[axis]) ? PSTR("FORWARD") : PSTR("REVERSE"));
+  if (sh.STATUS_AXIS_LAYOUT == L6480_STATUS_LAYOUT) {
+    SERIAL_ECHOPGM("   Last Command: ");
+    if (sh.STATUS_AXIS & sh.STATUS_AXIS_WRONG_CMD) SERIAL_ECHOPGM("VALID");
+    else                                           SERIAL_ECHOPGM("ERROR");
+    SERIAL_ECHOPGM("\n...THERMAL: ");
+    switch ((sh.STATUS_AXIS & (sh.STATUS_AXIS_TH_SD | sh.STATUS_AXIS_TH_WRN)) >> 11) {
+      case 0: SERIAL_ECHOPGM("DEVICE SHUTDOWN"); break;
+      case 1: SERIAL_ECHOPGM("BRIDGE SHUTDOWN"); break;
+      case 2: SERIAL_ECHOPGM("WARNING        "); break;
+      case 3: SERIAL_ECHOPGM("OK             "); break;
+    }
+  }
+  else {
+    SERIAL_ECHOPGM("   Last Command: ");
+    if (!(sh.STATUS_AXIS & sh.STATUS_AXIS_WRONG_CMD)) SERIAL_ECHOPGM("IN");
+    SERIAL_ECHOPGM("VALID    ");
+    SERIAL_ECHOPGM_P(sh.STATUS_AXIS & sh.STATUS_AXIS_NOTPERF_CMD ?  PSTR("COMPLETED    ") : PSTR("Not PERFORMED"));
+    SERIAL_ECHOPAIR("\n...THERMAL: ", !(sh.STATUS_AXIS & sh.STATUS_AXIS_TH_SD) ? "SHUTDOWN       " : !(sh.STATUS_AXIS & sh.STATUS_AXIS_TH_WRN) ? "WARNING        " : "OK             ");
+  }
+  SERIAL_ECHOPGM("   OVERCURRENT:"); echo_yes_no((sh.STATUS_AXIS & sh.STATUS_AXIS_OCD) == 0);
+  if (sh.STATUS_AXIS_LAYOUT != L6474_STATUS_LAYOUT) {
+    SERIAL_ECHOPGM("   STALL:"); echo_yes_no((sh.STATUS_AXIS & sh.STATUS_AXIS_STEP_LOSS_A) == 0 || (sh.STATUS_AXIS & sh.STATUS_AXIS_STEP_LOSS_B) == 0);
+    SERIAL_ECHOPGM("   STEP-CLOCK MODE:"); echo_yes_no((sh.STATUS_AXIS & sh.STATUS_AXIS_SCK_MOD) != 0);
+  }
+  else {
+    SERIAL_ECHOPGM("   STALL: NA "
+                   "   STEP-CLOCK MODE: NA"
+                   "   UNDER VOLTAGE LOCKOUT: "); echo_yes_no((sh.STATUS_AXIS & sh.STATUS_AXIS_UVLO) == 0);
+  }
+  SERIAL_EOL();
+}
+
+/**
+ * M122: Debug L6470 drivers
+ */
+void GcodeSuite::M122() {
+  L64xxManager.pause_monitor(true); // Keep monitor_driver() from stealing status
+  L64xxManager.spi_active = true;   // Tell set_directions() a series of SPI transfers is underway
+
+  //if (parser.seen('S'))
+  //  tmc_set_report_interval(parser.value_bool());
+  //else
+
+  #if AXIS_IS_L64XX(X)
+    L6470_say_status(X);
+  #endif
+  #if AXIS_IS_L64XX(X2)
+    L6470_say_status(X2);
+  #endif
+  #if AXIS_IS_L64XX(Y)
+    L6470_say_status(Y);
+  #endif
+  #if AXIS_IS_L64XX(Y2)
+    L6470_say_status(Y2);
+  #endif
+  #if AXIS_IS_L64XX(Z)
+    L6470_say_status(Z);
+  #endif
+  #if AXIS_IS_L64XX(Z2)
+    L6470_say_status(Z2);
+  #endif
+  #if AXIS_IS_L64XX(Z3)
+    L6470_say_status(Z3);
+  #endif
+  #if AXIS_IS_L64XX(Z4)
+    L6470_say_status(Z4);
+  #endif
+  #if AXIS_IS_L64XX(E0)
+    L6470_say_status(E0);
+  #endif
+  #if AXIS_IS_L64XX(E1)
+    L6470_say_status(E1);
+  #endif
+  #if AXIS_IS_L64XX(E2)
+    L6470_say_status(E2);
+  #endif
+  #if AXIS_IS_L64XX(E3)
+    L6470_say_status(E3);
+  #endif
+  #if AXIS_IS_L64XX(E4)
+    L6470_say_status(E4);
+  #endif
+  #if AXIS_IS_L64XX(E5)
+    L6470_say_status(E5);
+  #endif
+  #if AXIS_IS_L64XX(E6)
+    L6470_say_status(E6);
+  #endif
+  #if AXIS_IS_L64XX(E7)
+    L6470_say_status(E7);
+  #endif
+
+  L64xxManager.spi_active = false;   // done with all SPI transfers - clear handshake flags
+  L64xxManager.spi_abort = false;
+  L64xxManager.pause_monitor(false);
+}
+
+#endif // HAS_L64XX

Marlin/src/gcode/feature/L6470/M906.cpp

@@ -0,0 +1,376 @@
+/**
+ * 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_L64XX
+
+#include "../../gcode.h"
+#include "../../../libs/L64XX/L64XX_Marlin.h"
+#include "../../../module/stepper/indirection.h"
+#include "../../../module/planner.h"
+
+#define DEBUG_OUT ENABLED(L6470_CHITCHAT)
+#include "../../../core/debug_out.h"
+
+/**
+ * MACRO to fetch information on the items associated with current limiting
+ * and maximum voltage output.
+ *
+ * L6470 can be setup to shutdown if either current threshold is exceeded.
+ *
+ * L6470 output current can not be set directly.  It is set indirectly by
+ * setting the maximum effective output voltage.
+ *
+ *  Effective output voltage is set by PWM duty cycle.
+ *
+ *  Maximum effective output voltage is affected by MANY variables.  The main ones are:
+ *    KVAL_HOLD
+ *    KVAL_RUN
+ *    KVAL_ACC
+ *    KVAL_DEC
+ *    Vs compensation (if enabled)
+ */
+void L64XX_report_current(L64XX &motor, const L64XX_axis_t axis) {
+
+  if (L64xxManager.spi_abort) return;  // don't do anything if set_directions() has occurred
+
+  const L64XX_Marlin::L64XX_shadow_t &sh = L64xxManager.shadow;
+  const uint16_t status = L64xxManager.get_status(axis);    //also populates shadow structure
+  const uint8_t OverCurrent_Threshold = uint8_t(motor.GetParam(L6470_OCD_TH));
+
+  auto say_axis_status = [](const L64XX_axis_t axis, const uint16_t status) {
+    L64xxManager.say_axis(axis);
+    #if ENABLED(L6470_CHITCHAT)
+      char tmp[10];
+      sprintf_P(tmp, PSTR("%4x   "), status);
+      DEBUG_ECHOPAIR("   status: ", tmp);
+      print_bin(status);
+    #else
+      UNUSED(status);
+    #endif
+    SERIAL_EOL();
+  };
+
+  char temp_buf[10];
+
+  switch (sh.STATUS_AXIS_LAYOUT) {
+    case L6470_STATUS_LAYOUT:       // L6470
+    case L6480_STATUS_LAYOUT: {     // L6480 & powerstep01
+      const uint16_t Stall_Threshold = (uint8_t)motor.GetParam(L6470_STALL_TH),
+                     motor_status = (status & (STATUS_MOT_STATUS)) >> 5,
+                     L6470_ADC_out = motor.GetParam(L6470_ADC_OUT),
+                     L6470_ADC_out_limited = constrain(L6470_ADC_out, 8, 24);
+      const float comp_coef = 1600.0f / L6470_ADC_out_limited;
+      const uint16_t MicroSteps = _BV(motor.GetParam(L6470_STEP_MODE) & 0x07);
+
+      say_axis_status(axis, sh.STATUS_AXIS_RAW);
+
+      SERIAL_ECHOPGM("...OverCurrent Threshold: ");
+      sprintf_P(temp_buf, PSTR("%2d ("), OverCurrent_Threshold);
+      SERIAL_ECHO(temp_buf);
+      SERIAL_ECHO((OverCurrent_Threshold + 1) * motor.OCD_CURRENT_CONSTANT_INV);
+      SERIAL_ECHOPGM(" mA)");
+      SERIAL_ECHOPGM("   Stall Threshold: ");
+      sprintf_P(temp_buf, PSTR("%2d ("), Stall_Threshold);
+      SERIAL_ECHO(temp_buf);
+      SERIAL_ECHO((Stall_Threshold + 1) * motor.STALL_CURRENT_CONSTANT_INV);
+      SERIAL_ECHOPGM(" mA)");
+      SERIAL_ECHOPGM("   Motor Status: ");
+      switch (motor_status) {
+        case 0: SERIAL_ECHOPGM("stopped"); break;
+        case 1: SERIAL_ECHOPGM("accelerating"); break;
+        case 2: SERIAL_ECHOPGM("decelerating"); break;
+        case 3: SERIAL_ECHOPGM("at constant speed"); break;
+      }
+      SERIAL_EOL();
+
+      SERIAL_ECHOPAIR("...MicroSteps: ", MicroSteps,
+                      "   ADC_OUT: ", L6470_ADC_out);
+      SERIAL_ECHOPGM("   Vs_compensation: ");
+      SERIAL_ECHOPGM_P((motor.GetParam(sh.L6470_AXIS_CONFIG) & CONFIG_EN_VSCOMP) ? PSTR("ENABLED ") : PSTR("DISABLED"));
+      SERIAL_ECHOLNPAIR("   Compensation coefficient: ~", comp_coef * 0.01f);
+
+      SERIAL_ECHOPAIR("...KVAL_HOLD: ", motor.GetParam(L6470_KVAL_HOLD),
+                      "   KVAL_RUN : ", motor.GetParam(L6470_KVAL_RUN),
+                      "   KVAL_ACC: ", motor.GetParam(L6470_KVAL_ACC),
+                      "   KVAL_DEC: ", motor.GetParam(L6470_KVAL_DEC),
+                      "   V motor max = ");
+      switch (motor_status) {
+        case 0: SERIAL_ECHO(motor.GetParam(L6470_KVAL_HOLD) * 100 / 256); SERIAL_ECHOPGM("% (KVAL_HOLD)"); break;
+        case 1: SERIAL_ECHO(motor.GetParam(L6470_KVAL_RUN)  * 100 / 256); SERIAL_ECHOPGM("% (KVAL_RUN)"); break;
+        case 2: SERIAL_ECHO(motor.GetParam(L6470_KVAL_ACC)  * 100 / 256); SERIAL_ECHOPGM("% (KVAL_ACC)"); break;
+        case 3: SERIAL_ECHO(motor.GetParam(L6470_KVAL_DEC)  * 100 / 256); SERIAL_ECHOPGM("% (KVAL_HOLD)"); break;
+      }
+      SERIAL_EOL();
+
+      #if ENABLED(L6470_CHITCHAT)
+        DEBUG_ECHOPGM("...SLEW RATE: ");
+        switch (sh.STATUS_AXIS_LAYOUT) {
+          case L6470_STATUS_LAYOUT: {
+            switch ((motor.GetParam(sh.L6470_AXIS_CONFIG) & CONFIG_POW_SR) >> CONFIG_POW_SR_BIT) {
+              case 0: { DEBUG_ECHOLNPGM("320V/uS") ; break; }
+              case 1: { DEBUG_ECHOLNPGM("75V/uS")  ; break; }
+              case 2: { DEBUG_ECHOLNPGM("110V/uS") ; break; }
+              case 3: { DEBUG_ECHOLNPGM("260V/uS") ; break; }
+            }
+            break;
+          }
+        case L6480_STATUS_LAYOUT: {
+            switch (motor.GetParam(L6470_GATECFG1) & CONFIG1_SR ) {
+              case CONFIG1_SR_220V_us: { DEBUG_ECHOLNPGM("220V/uS") ; break; }
+              case CONFIG1_SR_400V_us: { DEBUG_ECHOLNPGM("400V/uS") ; break; }
+              case CONFIG1_SR_520V_us: { DEBUG_ECHOLNPGM("520V/uS") ; break; }
+              case CONFIG1_SR_980V_us: { DEBUG_ECHOLNPGM("980V/uS") ; break; }
+              default: { DEBUG_ECHOLNPGM("unknown") ; break; }
+            }
+          }
+        }
+      #endif
+      SERIAL_EOL();
+      break;
+    }
+
+    case L6474_STATUS_LAYOUT: {  // L6474
+      const uint16_t L6470_ADC_out = motor.GetParam(L6470_ADC_OUT) & 0x1F,
+                     L6474_TVAL_val = motor.GetParam(L6474_TVAL) & 0x7F;
+
+      say_axis_status(axis, sh.STATUS_AXIS_RAW);
+
+      SERIAL_ECHOPGM("...OverCurrent Threshold: ");
+      sprintf_P(temp_buf, PSTR("%2d ("), OverCurrent_Threshold);
+      SERIAL_ECHO(temp_buf);
+      SERIAL_ECHO((OverCurrent_Threshold + 1) * motor.OCD_CURRENT_CONSTANT_INV);
+      SERIAL_ECHOPGM(" mA)");
+      SERIAL_ECHOPGM("   TVAL: ");
+      sprintf_P(temp_buf, PSTR("%2d ("), L6474_TVAL_val);
+      SERIAL_ECHO(temp_buf);
+      SERIAL_ECHO((L6474_TVAL_val + 1) * motor.STALL_CURRENT_CONSTANT_INV);
+      SERIAL_ECHOLNPGM(" mA)   Motor Status: NA");
+
+      const uint16_t MicroSteps = _BV(motor.GetParam(L6470_STEP_MODE) & 0x07); //NOMORE(MicroSteps, 16);
+      SERIAL_ECHOPAIR("...MicroSteps: ", MicroSteps,
+                      "   ADC_OUT: ", L6470_ADC_out);
+
+      SERIAL_ECHOLNPGM("   Vs_compensation: NA\n");
+      SERIAL_ECHOLNPGM("...KVAL_HOLD: NA"
+                       "   KVAL_RUN : NA"
+                       "   KVAL_ACC: NA"
+                       "   KVAL_DEC: NA"
+                       "   V motor max =  NA");
+
+      #if ENABLED(L6470_CHITCHAT)
+        DEBUG_ECHOPGM("...SLEW RATE: ");
+        switch ((motor.GetParam(sh.L6470_AXIS_CONFIG) & CONFIG_POW_SR) >> CONFIG_POW_SR_BIT) {
+          case 0:  DEBUG_ECHOLNPGM("320V/uS") ; break;
+          case 1:  DEBUG_ECHOLNPGM("75V/uS")  ; break;
+          case 2:  DEBUG_ECHOLNPGM("110V/uS") ; break;
+          case 3:  DEBUG_ECHOLNPGM("260V/uS") ; break;
+          default: DEBUG_ECHOLNPAIR("slew rate: ", (motor.GetParam(sh.L6470_AXIS_CONFIG) & CONFIG_POW_SR) >> CONFIG_POW_SR_BIT); break;
+        }
+      #endif
+      SERIAL_EOL();
+      SERIAL_EOL();
+      break;
+    }
+  }
+}
+
+/**
+ * M906: report or set KVAL_HOLD which sets the maximum effective voltage provided by the
+ *       PWMs to the steppers
+ *
+ * On L6474 this sets the TVAL register (same address).
+ *
+ * I - select which driver(s) to change on multi-driver axis
+ *     0 - (default) all drivers on the axis or E0
+ *     1 - monitor only X, Y, Z or E1
+ *     2 - monitor only X2, Y2, Z2 or E2
+ *     3 - monitor only Z3 or E3
+ *     4 - monitor only Z4 or E4
+ *     5 - monitor only E5
+ * Xxxx, Yxxx, Zxxx, Exxx - axis to change (optional)
+ *     L6474 - current in mA (4A max)
+ *     All others - 0-255
+ *
+ * Sets KVAL_HOLD which affects the current being driven through the stepper.
+ *
+ * L6470 is used in the STEP-CLOCK mode.  KVAL_HOLD is the only KVAL_xxx
+ * that affects the effective voltage seen by the stepper.
+ */
+void GcodeSuite::M906() {
+
+  L64xxManager.pause_monitor(true); // Keep monitor_driver() from stealing status
+
+  #define L6470_SET_KVAL_HOLD(Q) (AXIS_IS_L64XX(Q) ? stepper##Q.setTVALCurrent(value) : stepper##Q.SetParam(L6470_KVAL_HOLD, uint8_t(value)))
+
+  DEBUG_ECHOLNPGM("M906");
+
+  uint8_t report_current = true;
+
+  #if HAS_L64XX
+    const uint8_t index = parser.byteval('I');
+  #endif
+
+  LOOP_LOGICAL_AXES(i) if (uint16_t value = parser.intval(axis_codes[i])) {
+
+    report_current = false;
+
+    if (planner.has_blocks_queued() || planner.cleaning_buffer_counter) {
+      SERIAL_ECHOLNPGM("Test aborted. Can't set KVAL_HOLD while steppers are moving.");
+      return;
+    }
+
+    switch (i) {
+      case X_AXIS:
+        #if AXIS_IS_L64XX(X)
+          if (index == 0) L6470_SET_KVAL_HOLD(X);
+        #endif
+        #if AXIS_IS_L64XX(X2)
+          if (index == 1) L6470_SET_KVAL_HOLD(X2);
+        #endif
+        break;
+
+      #if HAS_Y_AXIS
+        case Y_AXIS:
+          #if AXIS_IS_L64XX(Y)
+            if (index == 0) L6470_SET_KVAL_HOLD(Y);
+          #endif
+          #if AXIS_IS_L64XX(Y2)
+            if (index == 1) L6470_SET_KVAL_HOLD(Y2);
+          #endif
+          break;
+      #endif
+
+      #if HAS_Z_AXIS
+        case Z_AXIS:
+          #if AXIS_IS_L64XX(Z)
+            if (index == 0) L6470_SET_KVAL_HOLD(Z);
+          #endif
+          #if AXIS_IS_L64XX(Z2)
+            if (index == 1) L6470_SET_KVAL_HOLD(Z2);
+          #endif
+          #if AXIS_IS_L64XX(Z3)
+            if (index == 2) L6470_SET_KVAL_HOLD(Z3);
+          #endif
+          #if AXIS_DRIVER_TYPE_Z4(L6470)
+            if (index == 3) L6470_SET_KVAL_HOLD(Z4);
+          #endif
+          break;
+      #endif
+
+      #if E_STEPPERS
+        case E_AXIS: {
+          const int8_t target_e_stepper = get_target_e_stepper_from_command();
+          if (target_e_stepper < 0) return;
+          switch (target_e_stepper) {
+            #if AXIS_IS_L64XX(E0)
+              case 0: L6470_SET_KVAL_HOLD(E0); break;
+            #endif
+            #if AXIS_IS_L64XX(E1)
+              case 1: L6470_SET_KVAL_HOLD(E1); break;
+            #endif
+            #if AXIS_IS_L64XX(E2)
+              case 2: L6470_SET_KVAL_HOLD(E2); break;
+            #endif
+            #if AXIS_IS_L64XX(E3)
+              case 3: L6470_SET_KVAL_HOLD(E3); break;
+            #endif
+            #if AXIS_IS_L64XX(E4)
+              case 4: L6470_SET_KVAL_HOLD(E4); break;
+            #endif
+            #if AXIS_IS_L64XX(E5)
+              case 5: L6470_SET_KVAL_HOLD(E5); break;
+            #endif
+            #if AXIS_IS_L64XX(E6)
+              case 6: L6470_SET_KVAL_HOLD(E6); break;
+            #endif
+            #if AXIS_IS_L64XX(E7)
+              case 7: L6470_SET_KVAL_HOLD(E7); break;
+            #endif
+          }
+        } break;
+      #endif
+    }
+  }
+
+  if (report_current) {
+    #define L64XX_REPORT_CURRENT(Q) L64XX_report_current(stepper##Q, Q)
+
+    L64xxManager.spi_active = true; // Tell set_directions() a series of SPI transfers is underway
+
+    #if AXIS_IS_L64XX(X)
+      L64XX_REPORT_CURRENT(X);
+    #endif
+    #if AXIS_IS_L64XX(X2)
+      L64XX_REPORT_CURRENT(X2);
+    #endif
+    #if AXIS_IS_L64XX(Y)
+      L64XX_REPORT_CURRENT(Y);
+    #endif
+    #if AXIS_IS_L64XX(Y2)
+      L64XX_REPORT_CURRENT(Y2);
+    #endif
+    #if AXIS_IS_L64XX(Z)
+      L64XX_REPORT_CURRENT(Z);
+    #endif
+    #if AXIS_IS_L64XX(Z2)
+      L64XX_REPORT_CURRENT(Z2);
+    #endif
+    #if AXIS_IS_L64XX(Z3)
+      L64XX_REPORT_CURRENT(Z3);
+    #endif
+    #if AXIS_IS_L64XX(Z4)
+      L64XX_REPORT_CURRENT(Z4);
+    #endif
+    #if AXIS_IS_L64XX(E0)
+      L64XX_REPORT_CURRENT(E0);
+    #endif
+    #if AXIS_IS_L64XX(E1)
+      L64XX_REPORT_CURRENT(E1);
+    #endif
+    #if AXIS_IS_L64XX(E2)
+      L64XX_REPORT_CURRENT(E2);
+    #endif
+    #if AXIS_IS_L64XX(E3)
+      L64XX_REPORT_CURRENT(E3);
+    #endif
+    #if AXIS_IS_L64XX(E4)
+      L64XX_REPORT_CURRENT(E4);
+    #endif
+    #if AXIS_IS_L64XX(E5)
+      L64XX_REPORT_CURRENT(E5);
+    #endif
+    #if AXIS_IS_L64XX(E6)
+      L64XX_REPORT_CURRENT(E6);
+    #endif
+    #if AXIS_IS_L64XX(E7)
+      L64XX_REPORT_CURRENT(E7);
+    #endif
+
+    L64xxManager.spi_active = false;   // done with all SPI transfers - clear handshake flags
+    L64xxManager.spi_abort = false;
+    L64xxManager.pause_monitor(false);
+  }
+}
+
+#endif // HAS_L64XX

Marlin/src/gcode/feature/L6470/M916-918.cpp

@@ -0,0 +1,651 @@
+/**
+ * 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/>.
+ *
+ */
+
+//
+// NOTE: All tests assume each axis uses matching driver chips.
+//
+
+#include "../../../inc/MarlinConfig.h"
+
+#if HAS_L64XX
+
+#include "../../gcode.h"
+#include "../../../module/stepper/indirection.h"
+#include "../../../module/planner.h"
+#include "../../../libs/L64XX/L64XX_Marlin.h"
+
+#define DEBUG_OUT ENABLED(L6470_CHITCHAT)
+#include "../../../core/debug_out.h"
+
+/**
+ * M916: increase KVAL_HOLD until get thermal warning
+ *       NOTE - on L6474 it is TVAL that is used
+ *
+ * J - select which driver(s) to monitor on multi-driver axis
+ *     0 - (default) monitor all drivers on the axis or E0
+ *     1 - monitor only X, Y, Z, E1
+ *     2 - monitor only X2, Y2, Z2, E2
+ *     3 - monitor only Z3, E3
+ *     4 - monitor only Z4, E4
+ *
+ * Xxxx, Yxxx, Zxxx, Exxx - axis to be monitored with displacement
+ *     xxx (1-255) is distance moved on either side of current position
+ *
+ * F - feedrate
+ *     optional - will use default max feedrate from configuration.h if not specified
+ *
+ * T - current (mA) setting for TVAL (0 - 4A in 31.25mA increments, rounds down) - L6474 only
+ *     optional - will report current value from driver if not specified
+ *
+ * K - value for KVAL_HOLD (0 - 255) (ignored for L6474)
+ *     optional - will report current value from driver if not specified
+ *
+ * D - time (in seconds) to run each setting of KVAL_HOLD/TVAL
+ *     optional - defaults to zero (runs each setting once)
+ */
+
+/**
+ * This routine is also useful for determining the approximate KVAL_HOLD
+ * where the stepper stops losing steps. The sound will get noticeably quieter
+ * as it stops losing steps.
+ */
+
+void GcodeSuite::M916() {
+
+  DEBUG_ECHOLNPGM("M916");
+
+  L64xxManager.pause_monitor(true); // Keep monitor_driver() from stealing status
+
+  // Variables used by L64xxManager.get_user_input function - some may not be used
+  char axis_mon[3][3] = { {"  "}, {"  "}, {"  "} };   // list of Axes to be monitored
+  L64XX_axis_t axis_index[3];
+  uint16_t axis_status[3];
+  uint8_t driver_count = 1;
+  float position_max;
+  float position_min;
+  float final_feedrate;
+  uint8_t kval_hold;
+  uint8_t OCD_TH_val = 0;
+  uint8_t STALL_TH_val = 0;
+  uint16_t over_current_threshold;
+  constexpr uint8_t over_current_flag = false;  // M916 doesn't play with the overcurrent thresholds
+
+  #define DRIVER_TYPE_L6474(Q) AXIS_DRIVER_TYPE_##Q(L6474)
+
+  uint8_t j;   // general purpose counter
+
+  if (L64xxManager.get_user_input(driver_count, axis_index, axis_mon, position_max, position_min, final_feedrate, kval_hold, over_current_flag, OCD_TH_val, STALL_TH_val, over_current_threshold))
+    return;  // quit if invalid user input
+
+  DEBUG_ECHOLNPAIR("feedrate = ", final_feedrate);
+
+  planner.synchronize();                             // wait for all current movement commands to complete
+
+  const L64XX_Marlin::L64XX_shadow_t &sh = L64xxManager.shadow;
+  for (j = 0; j < driver_count; j++)
+    L64xxManager.get_status(axis_index[j]);  // clear out any pre-existing error flags
+
+  char temp_axis_string[] = " ";
+  temp_axis_string[0] = axis_mon[0][0];  // need to have a string for use within sprintf format section
+  char gcode_string[80];
+  uint16_t status_composite = 0;
+
+  uint16_t M91x_counter = kval_hold;
+  uint16_t M91x_counter_max;
+  if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT) {
+    M91x_counter_max = 128;  // TVAL is 7 bits
+    LIMIT(M91x_counter, 0U, 127U);
+  }
+  else
+    M91x_counter_max = 256;  // KVAL_HOLD is 8 bits
+
+  uint8_t M91x_delay_s = parser.byteval('D');   // get delay in seconds
+  millis_t M91x_delay_ms = SEC_TO_MS(M91x_delay_s * 60);
+  millis_t M91x_delay_end;
+
+  DEBUG_ECHOLNPGM(".\n.");
+
+  do {
+
+    if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT)
+      DEBUG_ECHOLNPAIR("TVAL current (mA) = ", (M91x_counter + 1) * sh.AXIS_STALL_CURRENT_CONSTANT_INV);        // report TVAL current for this run
+    else
+      DEBUG_ECHOLNPAIR("kval_hold = ", M91x_counter);                                   // report KVAL_HOLD for this run
+
+    for (j = 0; j < driver_count; j++)
+      L64xxManager.set_param(axis_index[j], L6470_KVAL_HOLD, M91x_counter);  //set KVAL_HOLD or TVAL (same register address)
+
+    M91x_delay_end = millis() + M91x_delay_ms;
+    do {
+      // turn the motor(s) both directions
+      sprintf_P(gcode_string, PSTR("G0 %s%03d F%03d"), temp_axis_string, uint16_t(position_min), uint16_t(final_feedrate));
+      gcode.process_subcommands_now_P(gcode_string);
+
+      sprintf_P(gcode_string, PSTR("G0 %s%03d F%03d"), temp_axis_string, uint16_t(position_max), uint16_t(final_feedrate));
+      gcode.process_subcommands_now_P(gcode_string);
+
+      // get the status after the motors have stopped
+      planner.synchronize();
+
+      status_composite = 0;    // clear out the old bits
+
+      for (j = 0; j < driver_count; j++) {
+        axis_status[j] = (~L64xxManager.get_status(axis_index[j])) & sh.L6470_ERROR_MASK;    // bits of interest are all active low
+        status_composite |= axis_status[j] ;
+      }
+
+      if (status_composite) break;
+    } while (millis() < M91x_delay_end);
+
+    if (status_composite) break;
+
+    M91x_counter++;
+
+  } while (!(status_composite & (sh.STATUS_AXIS_TH_WRN | sh.STATUS_AXIS_TH_SD)) && (M91x_counter < M91x_counter_max));
+
+  DEBUG_ECHOLNPGM(".");
+
+  #if ENABLED(L6470_CHITCHAT)
+    if (status_composite) {
+      L64xxManager.error_status_decode(status_composite, axis_index[0],
+        sh.STATUS_AXIS_TH_SD, sh.STATUS_AXIS_TH_WRN,
+        sh.STATUS_AXIS_STEP_LOSS_A, sh.STATUS_AXIS_STEP_LOSS_B,
+        sh.STATUS_AXIS_OCD, sh.STATUS_AXIS_LAYOUT);
+      DEBUG_ECHOLNPGM(".");
+    }
+  #endif
+
+  if ((status_composite & (sh.STATUS_AXIS_TH_WRN | sh.STATUS_AXIS_TH_SD)))
+    DEBUG_ECHOLNPGM(".\n.\nTest completed normally - Thermal warning/shutdown has occurred");
+  else if (status_composite)
+    DEBUG_ECHOLNPGM(".\n.\nTest completed abnormally - non-thermal error has occurred");
+  else
+    DEBUG_ECHOLNPGM(".\n.\nTest completed normally - Unable to get to thermal warning/shutdown");
+
+  L64xxManager.pause_monitor(false);
+}
+
+/**
+ * M917: Find minimum current thresholds
+ *
+ *   Decrease OCD current until overcurrent error
+ *   Increase OCD until overcurrent error goes away
+ *   Decrease stall threshold until stall (not done on L6474)
+ *   Increase stall until stall error goes away (not done on L6474)
+ *
+ * J - select which driver(s) to monitor on multi-driver axis
+ *     0 - (default) monitor all drivers on the axis or E0
+ *     1 - monitor only X, Y, Z, E1
+ *     2 - monitor only X2, Y2, Z2, E2
+ * Xxxx, Yxxx, Zxxx, Exxx - axis to be monitored with displacement
+ *     xxx (1-255) is distance moved on either side of current position
+ *
+ * F - feedrate
+ *     optional - will use default max feedrate from Configuration.h if not specified
+ *
+ * I - starting over-current threshold
+ *     optional - will report current value from driver if not specified
+ *     if there are multiple drivers on the axis then all will be set the same
+ *
+ * T - current (mA) setting for TVAL (0 - 4A in 31.25mA increments, rounds down) - L6474 only
+ *     optional - will report current value from driver if not specified
+ *
+ * K - value for KVAL_HOLD (0 - 255) (ignored for L6474)
+ *     optional - will report current value from driver if not specified
+ */
+void GcodeSuite::M917() {
+
+  DEBUG_ECHOLNPGM("M917");
+
+  L64xxManager.pause_monitor(true); // Keep monitor_driver() from stealing status
+
+  char axis_mon[3][3] = { {"  "}, {"  "}, {"  "} };   // list of Axes to be monitored
+  L64XX_axis_t axis_index[3];
+  uint16_t axis_status[3];
+  uint8_t driver_count = 1;
+  float position_max;
+  float position_min;
+  float final_feedrate;
+  uint8_t kval_hold;
+  uint8_t OCD_TH_val = 0;
+  uint8_t STALL_TH_val = 0;
+  uint16_t over_current_threshold;
+  constexpr uint8_t over_current_flag = true;
+
+  uint8_t j;   // general purpose counter
+
+  if (L64xxManager.get_user_input(driver_count, axis_index, axis_mon, position_max, position_min, final_feedrate, kval_hold, over_current_flag, OCD_TH_val, STALL_TH_val, over_current_threshold))
+    return;  // quit if invalid user input
+
+  DEBUG_ECHOLNPAIR("feedrate = ", final_feedrate);
+
+  planner.synchronize();                             // wait for all current movement commands to complete
+
+  const L64XX_Marlin::L64XX_shadow_t &sh = L64xxManager.shadow;
+  for (j = 0; j < driver_count; j++)
+    L64xxManager.get_status(axis_index[j]);  // clear error flags
+  char temp_axis_string[] = " ";
+  temp_axis_string[0] = axis_mon[0][0];   // need a sprintf format string
+  char gcode_string[80];
+  uint16_t status_composite = 0;
+  uint8_t test_phase = 0;                 // 0 - decreasing OCD - exit when OCD warning occurs (ignore STALL)
+                                          // 1 - increasing OCD - exit when OCD warning stops (ignore STALL)
+                                          // 2 - OCD finalized - decreasing STALL - exit when STALL warning happens
+                                          // 3 - OCD finalized - increasing STALL - exit when STALL warning stop
+                                          // 4 - all testing completed
+  DEBUG_ECHOPAIR(".\n.\n.\nover_current threshold : ", (OCD_TH_val + 1) * 375);   // first status display
+  DEBUG_ECHOPAIR("  (OCD_TH:  : ", OCD_TH_val);
+  if (sh.STATUS_AXIS_LAYOUT != L6474_STATUS_LAYOUT) {
+    DEBUG_ECHOPAIR(")   Stall threshold: ", (STALL_TH_val + 1) * 31.25);
+    DEBUG_ECHOPAIR("  (STALL_TH: ", STALL_TH_val);
+  }
+  DEBUG_ECHOLNPGM(")");
+
+  do {
+
+    if (sh.STATUS_AXIS_LAYOUT != L6474_STATUS_LAYOUT) DEBUG_ECHOPAIR("STALL threshold : ", (STALL_TH_val + 1) * 31.25);
+    DEBUG_ECHOLNPAIR("   OCD threshold : ", (OCD_TH_val + 1) * 375);
+
+    sprintf_P(gcode_string, PSTR("G0 %s%03d F%03d"), temp_axis_string, uint16_t(position_min), uint16_t(final_feedrate));
+    gcode.process_subcommands_now_P(gcode_string);
+
+    sprintf_P(gcode_string, PSTR("G0 %s%03d F%03d"), temp_axis_string, uint16_t(position_max), uint16_t(final_feedrate));
+    gcode.process_subcommands_now_P(gcode_string);
+
+    planner.synchronize();
+
+    status_composite = 0;    // clear out the old bits
+
+    for (j = 0; j < driver_count; j++) {
+      axis_status[j] = (~L64xxManager.get_status(axis_index[j])) & sh.L6470_ERROR_MASK;    // bits of interest are all active low
+      status_composite |= axis_status[j];
+    }
+
+    if (status_composite && (status_composite & sh.STATUS_AXIS_UVLO)) {
+      DEBUG_ECHOLNPGM("Test aborted (Undervoltage lockout active)");
+      #if ENABLED(L6470_CHITCHAT)
+        for (j = 0; j < driver_count; j++) {
+          if (j) DEBUG_ECHOPGM("...");
+          L64xxManager.error_status_decode(axis_status[j], axis_index[j],
+            sh.STATUS_AXIS_TH_SD, sh.STATUS_AXIS_TH_WRN,
+            sh.STATUS_AXIS_STEP_LOSS_A, sh.STATUS_AXIS_STEP_LOSS_B,
+            sh.STATUS_AXIS_OCD, sh.STATUS_AXIS_LAYOUT);
+        }
+      #endif
+      return;
+    }
+
+    if (status_composite & (sh.STATUS_AXIS_TH_WRN | sh.STATUS_AXIS_TH_SD)) {
+      DEBUG_ECHOLNPGM("thermal problem - waiting for chip(s) to cool down ");
+      uint16_t status_composite_temp = 0;
+      uint8_t k = 0;
+      do {
+        k++;
+        if (!(k % 4)) {
+          kval_hold *= 0.95;
+          DEBUG_EOL();
+          DEBUG_ECHOLNPAIR("Lowering KVAL_HOLD by about 5% to ", kval_hold);
+          for (j = 0; j < driver_count; j++)
+            L64xxManager.set_param(axis_index[j], L6470_KVAL_HOLD, kval_hold);
+        }
+        DEBUG_ECHOLNPGM(".");
+        gcode.reset_stepper_timeout(); // keep steppers powered
+        watchdog_refresh();
+        safe_delay(5000);
+        status_composite_temp = 0;
+        for (j = 0; j < driver_count; j++) {
+          axis_status[j] = (~L64xxManager.get_status(axis_index[j])) & sh.L6470_ERROR_MASK;    // bits of interest are all active low
+          status_composite_temp |= axis_status[j];
+        }
+      }
+      while (status_composite_temp & (sh.STATUS_AXIS_TH_WRN | sh.STATUS_AXIS_TH_SD));
+      DEBUG_EOL();
+    }
+    if (status_composite & (sh.STATUS_AXIS_STEP_LOSS_A | sh.STATUS_AXIS_STEP_LOSS_B | sh.STATUS_AXIS_OCD)) {
+      switch (test_phase) {
+
+        case 0: {
+          if (status_composite & sh.STATUS_AXIS_OCD) {
+            // phase 0 with OCD warning - time to go to next phase
+            if (OCD_TH_val >= sh.AXIS_OCD_TH_MAX) {
+              OCD_TH_val = sh.AXIS_OCD_TH_MAX;           // limit to max
+              test_phase = 2;            // at highest value so skip phase 1
+              //DEBUG_ECHOLNPGM("LOGIC E0A OCD at highest - skip to 2");
+              DEBUG_ECHOLNPGM("OCD at highest - OCD finalized");
+            }
+            else {
+              OCD_TH_val++;              // normal exit to next phase
+              test_phase = 1;            // setup for first pass of phase 1
+              //DEBUG_ECHOLNPGM("LOGIC E0B - inc OCD  & go to 1");
+              DEBUG_ECHOLNPGM("inc OCD");
+            }
+          }
+          else {  // phase 0 without OCD warning - keep on decrementing if can
+            if (OCD_TH_val) {
+              OCD_TH_val--;              // try lower value
+              //DEBUG_ECHOLNPGM("LOGIC E0C - dec OCD");
+              DEBUG_ECHOLNPGM("dec OCD");
+            }
+            else {
+              test_phase = 2;            // at lowest value without warning so skip phase 1
+              //DEBUG_ECHOLNPGM("LOGIC E0D - OCD at latest - go to 2");
+              DEBUG_ECHOLNPGM("OCD finalized");
+            }
+          }
+        } break;
+
+        case 1: {
+          if (status_composite & sh.STATUS_AXIS_OCD) {
+            // phase 1 with OCD warning - increment if can
+            if (OCD_TH_val >= sh.AXIS_OCD_TH_MAX) {
+              OCD_TH_val = sh.AXIS_OCD_TH_MAX;           // limit to max
+              test_phase = 2;            // at highest value so go to next phase
+              //DEBUG_ECHOLNPGM("LOGIC E1A - OCD at max - go to 2");
+              DEBUG_ECHOLNPGM("OCD finalized");
+            }
+            else {
+              OCD_TH_val++;              // try a higher value
+              //DEBUG_ECHOLNPGM("LOGIC E1B - inc OCD");
+              DEBUG_ECHOLNPGM("inc OCD");
+            }
+          }
+          else { // phase 1 without OCD warning - normal exit to phase 2
+            test_phase = 2;
+            //DEBUG_ECHOLNPGM("LOGIC E1C - no OCD warning - go to 1");
+            DEBUG_ECHOLNPGM("OCD finalized");
+          }
+        } break;
+
+        case 2: {
+          if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT) {  // skip all STALL_TH steps if L6474
+            test_phase = 4;
+            break;
+          }
+          if (status_composite & (sh.STATUS_AXIS_STEP_LOSS_A | sh.STATUS_AXIS_STEP_LOSS_B)) {
+            // phase 2 with stall warning - time to go to next phase
+            if (STALL_TH_val >= 127) {
+              STALL_TH_val = 127;  // limit to max
+              //DEBUG_ECHOLNPGM("LOGIC E2A - STALL warning, STALL at max, quit");
+              DEBUG_ECHOLNPGM("finished - STALL at maximum value but still have stall warning");
+              test_phase = 4;
+            }
+            else {
+              test_phase = 3;              // normal exit to next phase (found failing value of STALL)
+              STALL_TH_val++;              // setup for first pass of phase 3
+              //DEBUG_ECHOLNPGM("LOGIC E2B - INC - STALL warning, inc Stall, go to 3");
+              DEBUG_ECHOLNPGM("inc Stall");
+            }
+          }
+          else {  // phase 2 without stall warning - decrement if can
+            if (STALL_TH_val) {
+              STALL_TH_val--;              // try a lower value
+              //DEBUG_ECHOLNPGM("LOGIC E2C - no STALL, dec STALL");
+              DEBUG_ECHOLNPGM("dec STALL");
+            }
+            else {
+              DEBUG_ECHOLNPGM("finished - STALL at lowest value but still do NOT have stall warning");
+              test_phase = 4;
+              //DEBUG_ECHOLNPGM("LOGIC E2D - no STALL, at lowest so quit");
+            }
+          }
+        } break;
+
+        case 3: {
+          if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT) {  // skip all STALL_TH steps if L6474
+            test_phase = 4;
+            break;
+          }
+          if (status_composite & (sh.STATUS_AXIS_STEP_LOSS_A | sh.STATUS_AXIS_STEP_LOSS_B)) {
+            // phase 3 with stall warning - increment if can
+            if (STALL_TH_val >= 127) {
+              STALL_TH_val = 127; // limit to max
+              DEBUG_ECHOLNPGM("finished - STALL at maximum value but still have stall warning");
+              test_phase = 4;
+              //DEBUG_ECHOLNPGM("LOGIC E3A - STALL, at max so quit");
+            }
+            else {
+              STALL_TH_val++;              // still looking for passing value
+              //DEBUG_ECHOLNPGM("LOGIC E3B - STALL, inc stall");
+              DEBUG_ECHOLNPGM("inc stall");
+            }
+          }
+          else {  //phase 3 without stall warning  but have OCD warning
+            DEBUG_ECHOLNPGM("Hardware problem - OCD warning without STALL warning");
+            test_phase = 4;
+            //DEBUG_ECHOLNPGM("LOGIC E3C - not STALLED, hardware problem (quit)");
+          }
+        } break;
+
+      }
+
+    }
+    else {
+      switch (test_phase) {
+        case 0: { // phase 0 without OCD warning - keep on decrementing if can
+          if (OCD_TH_val) {
+            OCD_TH_val--;             // try lower value
+            //DEBUG_ECHOLNPGM("LOGIC N0A - DEC OCD");
+            DEBUG_ECHOLNPGM("DEC OCD");
+          }
+          else {
+            test_phase = 2;           // at lowest value without warning so skip phase 1
+            //DEBUG_ECHOLNPGM("LOGIC N0B - OCD at lowest (go to phase 2)");
+            DEBUG_ECHOLNPGM("OCD finalized");
+          }
+        } break;
+
+        case 1: //DEBUG_ECHOLNPGM("LOGIC N1 (go directly to 2)"); // phase 1 without OCD warning - drop directly to phase 2
+                DEBUG_ECHOLNPGM("OCD finalized");
+
+        case 2: { // phase 2 without stall warning - keep on decrementing if can
+          if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT) {  // skip all STALL_TH steps if L6474
+            test_phase = 4;
+            break;
+          }
+          if (STALL_TH_val) {
+            STALL_TH_val--;              // try a lower value (stay in phase 2)
+            //DEBUG_ECHOLNPGM("LOGIC N2B - dec STALL");
+            DEBUG_ECHOLNPGM("dec STALL");
+          }
+          else {
+            DEBUG_ECHOLNPGM("finished - STALL at lowest value but still no stall warning");
+            test_phase = 4;
+            //DEBUG_ECHOLNPGM("LOGIC N2C - STALL at lowest (quit)");
+          }
+        } break;
+
+        case 3: {
+          if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT) {  // skip all STALL_TH steps if L6474
+            test_phase = 4;
+            break;
+          }
+          test_phase = 4;
+           //DEBUG_ECHOLNPGM("LOGIC N3 - finished!");
+           DEBUG_ECHOLNPGM("finished!");
+        } break;  // phase 3 without any warnings - desired exit
+      }  //
+    }  // end of status checks
+
+    if (test_phase != 4) {
+      for (j = 0; j < driver_count; j++) {                       // update threshold(s)
+        L64xxManager.set_param(axis_index[j], L6470_OCD_TH, OCD_TH_val);
+        if (sh.STATUS_AXIS_LAYOUT != L6474_STATUS_LAYOUT) L64xxManager.set_param(axis_index[j], L6470_STALL_TH, STALL_TH_val);
+        if (L64xxManager.get_param(axis_index[j], L6470_OCD_TH) != OCD_TH_val) DEBUG_ECHOLNPGM("OCD mismatch");
+        if ((L64xxManager.get_param(axis_index[j], L6470_STALL_TH) != STALL_TH_val) && (sh.STATUS_AXIS_LAYOUT != L6474_STATUS_LAYOUT)) DEBUG_ECHOLNPGM("STALL mismatch");
+      }
+    }
+
+  } while (test_phase != 4);
+
+  DEBUG_ECHOLNPGM(".");
+  if (status_composite) {
+    #if ENABLED(L6470_CHITCHAT)
+      for (j = 0; j < driver_count; j++) {
+        if (j) DEBUG_ECHOPGM("...");
+        L64xxManager.error_status_decode(axis_status[j], axis_index[j],
+          sh.STATUS_AXIS_TH_SD, sh.STATUS_AXIS_TH_WRN,
+          sh.STATUS_AXIS_STEP_LOSS_A, sh.STATUS_AXIS_STEP_LOSS_B,
+          sh.STATUS_AXIS_OCD, sh.STATUS_AXIS_LAYOUT);
+      }
+      DEBUG_ECHOLNPGM(".");
+    #endif
+    DEBUG_ECHOLNPGM("Completed with errors");
+  }
+  else
+    DEBUG_ECHOLNPGM("Completed with no errors");
+  DEBUG_ECHOLNPGM(".");
+
+  L64xxManager.pause_monitor(false);
+}
+
+/**
+ * M918: increase speed until error or max feedrate achieved (as shown in configuration.h))
+ *
+ * J - select which driver(s) to monitor on multi-driver axis
+ *     0 - (default) monitor all drivers on the axis or E0
+ *     1 - monitor only X, Y, Z, E1
+ *     2 - monitor only X2, Y2, Z2, E2
+ * Xxxx, Yxxx, Zxxx, Exxx - axis to be monitored with displacement
+ *     xxx (1-255) is distance moved on either side of current position
+ *
+ * I - over current threshold
+ *     optional - will report current value from driver if not specified
+ *
+ * T - current (mA) setting for TVAL (0 - 4A in 31.25mA increments, rounds down) - L6474 only
+ *     optional - will report current value from driver if not specified
+ *
+ * K - value for KVAL_HOLD (0 - 255) (ignored for L6474)
+ *     optional - will report current value from driver if not specified
+ *
+ * M - value for microsteps (1 - 128) (optional)
+ *     optional - will report current value from driver if not specified
+ */
+void GcodeSuite::M918() {
+
+  DEBUG_ECHOLNPGM("M918");
+
+  L64xxManager.pause_monitor(true); // Keep monitor_driver() from stealing status
+
+  char axis_mon[3][3] = { {"  "}, {"  "}, {"  "} };   // list of Axes to be monitored
+  L64XX_axis_t axis_index[3];
+  uint16_t axis_status[3];
+  uint8_t driver_count = 1;
+  float position_max, position_min;
+  float final_feedrate;
+  uint8_t kval_hold;
+  uint8_t OCD_TH_val = 0;
+  uint8_t STALL_TH_val = 0;
+  uint16_t over_current_threshold;
+  constexpr uint8_t over_current_flag = true;
+
+  const L64XX_Marlin::L64XX_shadow_t &sh = L64xxManager.shadow;
+
+  uint8_t j;   // general purpose counter
+
+  if (L64xxManager.get_user_input(driver_count, axis_index, axis_mon, position_max, position_min, final_feedrate, kval_hold, over_current_flag, OCD_TH_val, STALL_TH_val, over_current_threshold))
+    return;  // quit if invalid user input
+
+  L64xxManager.get_status(axis_index[0]); // populate shadow array
+
+  uint8_t m_steps = parser.byteval('M');
+
+  if (m_steps != 0) {
+    LIMIT(m_steps, 1, sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT ? 16 : 128);  // L6474
+
+    uint8_t stepVal;
+    for (stepVal = 0; stepVal < 8; stepVal++) {  // convert to L64xx register value
+      if (m_steps == 1) break;
+      m_steps >>= 1;
+    }
+
+    if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT)
+      stepVal |= 0x98;  // NO SYNC
+    else
+      stepVal |= (!SYNC_EN) | SYNC_SEL_1 | stepVal;
+
+    for (j = 0; j < driver_count; j++) {
+      L64xxManager.set_param(axis_index[j], dSPIN_HARD_HIZ, 0);          // can't write STEP register if stepper being powered
+                                                                         //   results in an extra NOOP being sent (data 00)
+      L64xxManager.set_param(axis_index[j], L6470_STEP_MODE, stepVal);   // set microsteps
+    }
+  }
+  m_steps = L64xxManager.get_param(axis_index[0], L6470_STEP_MODE) & 0x07;   // get microsteps
+
+  DEBUG_ECHOLNPAIR("Microsteps = ", _BV(m_steps));
+  DEBUG_ECHOLNPAIR("target (maximum) feedrate = ", final_feedrate);
+
+  const float feedrate_inc = final_feedrate / 10,   // Start at 1/10 of max & go up by 1/10 per step
+              fr_limit = final_feedrate * 0.99f;    // Rounding-safe comparison value
+  float current_feedrate = 0;
+
+  planner.synchronize();                            // Wait for moves to complete
+
+  for (j = 0; j < driver_count; j++)
+    L64xxManager.get_status(axis_index[j]);         // Clear error flags
+
+  char temp_axis_string[2] = " ";
+  temp_axis_string[0] = axis_mon[0][0];             // Need a sprintf format string
+  //temp_axis_string[1] = '\n';
+
+  char gcode_string[80];
+  uint16_t status_composite = 0;
+  DEBUG_ECHOLNPGM(".\n.\n.");                       // Make feedrate outputs easier to read
+
+  do {
+    current_feedrate += feedrate_inc;
+    DEBUG_ECHOLNPAIR("...feedrate = ", current_feedrate);
+
+    sprintf_P(gcode_string, PSTR("G0 %s%03d F%03d"), temp_axis_string, uint16_t(position_min), uint16_t(current_feedrate));
+    gcode.process_subcommands_now_P(gcode_string);
+
+    sprintf_P(gcode_string, PSTR("G0 %s%03d F%03d"), temp_axis_string, uint16_t(position_max), uint16_t(current_feedrate));
+    gcode.process_subcommands_now_P(gcode_string);
+
+    planner.synchronize();
+
+    for (j = 0; j < driver_count; j++) {
+      axis_status[j] = (~L64xxManager.get_status(axis_index[j])) & 0x0800;  // Bits of interest are all active LOW
+      status_composite |= axis_status[j];
+    }
+    if (status_composite) break;              // Break on any error
+  } while (current_feedrate < fr_limit);
+
+  DEBUG_ECHOPGM("Completed with ");
+  if (status_composite) {
+    DEBUG_ECHOLNPGM("errors");
+    #if ENABLED(L6470_CHITCHAT)
+      for (j = 0; j < driver_count; j++) {
+        if (j) DEBUG_ECHOPGM("...");
+        L64xxManager.error_status_decode(axis_status[j], axis_index[j],
+          sh.STATUS_AXIS_TH_SD, sh.STATUS_AXIS_TH_WRN,
+          sh.STATUS_AXIS_STEP_LOSS_A, sh.STATUS_AXIS_STEP_LOSS_B,
+          sh.STATUS_AXIS_OCD, sh.STATUS_AXIS_LAYOUT);
+      }
+    #endif
+  }
+  else
+    DEBUG_ECHOLNPGM("no errors");
+
+  L64xxManager.pause_monitor(false);
+}
+
+#endif // HAS_L64XX

Marlin/src/gcode/feature/advance/M900.cpp

@@ -0,0 +1,147 @@
+/**
+ * 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(LIN_ADVANCE)
+
+#include "../../gcode.h"
+#include "../../../module/planner.h"
+#include "../../../module/stepper.h"
+
+#if ENABLED(EXTRA_LIN_ADVANCE_K)
+  float other_extruder_advance_K[EXTRUDERS];
+  uint8_t lin_adv_slot = 0;
+#endif
+
+/**
+ * M900: Get or Set Linear Advance K-factor
+ *  T<tool>     Which tool to address
+ *  K<factor>   Set current advance K factor (Slot 0).
+ *  L<factor>   Set secondary advance K factor (Slot 1). Requires EXTRA_LIN_ADVANCE_K.
+ *  S<0/1>      Activate slot 0 or 1. Requires EXTRA_LIN_ADVANCE_K.
+ */
+void GcodeSuite::M900() {
+
+  auto echo_value_oor = [](const char ltr, const bool ten=true) {
+    SERIAL_CHAR('?', ltr);
+    SERIAL_ECHOPGM(" value out of range");
+    if (ten) SERIAL_ECHOPGM(" (0-10)");
+    SERIAL_ECHOLNPGM(".");
+  };
+
+  #if EXTRUDERS < 2
+    constexpr uint8_t tool_index = 0;
+  #else
+    const uint8_t tool_index = parser.intval('T', active_extruder);
+    if (tool_index >= EXTRUDERS) {
+      echo_value_oor('T', false);
+      return;
+    }
+  #endif
+
+  float &kref = planner.extruder_advance_K[tool_index], newK = kref;
+  const float oldK = newK;
+
+  #if ENABLED(EXTRA_LIN_ADVANCE_K)
+
+    float &lref = other_extruder_advance_K[tool_index];
+
+    const bool old_slot = TEST(lin_adv_slot, tool_index), // The tool's current slot (0 or 1)
+               new_slot = parser.boolval('S', old_slot);  // The passed slot (default = current)
+
+    // If a new slot is being selected swap the current and
+    // saved K values. Do here so K/L will apply correctly.
+    if (new_slot != old_slot) {                       // Not the same slot?
+      SET_BIT_TO(lin_adv_slot, tool_index, new_slot); // Update the slot for the tool
+      newK = lref;                                    // Get new K value from backup
+      lref = oldK;                                    // Save K to backup
+    }
+
+    // Set the main K value. Apply if the main slot is active.
+    if (parser.seenval('K')) {
+      const float K = parser.value_float();
+      if (!WITHIN(K, 0, 10)) echo_value_oor('K');
+      else if (new_slot)        lref = K;             // S1 Knn
+      else                      newK = K;             // S0 Knn
+    }
+
+    // Set the extra K value. Apply if the extra slot is active.
+    if (parser.seenval('L')) {
+      const float L = parser.value_float();
+      if (!WITHIN(L, 0, 10)) echo_value_oor('L');
+      else if (!new_slot)       lref = L;             // S0 Lnn
+      else                      newK = L;             // S1 Lnn
+    }
+
+  #else
+
+    if (parser.seenval('K')) {
+      const float K = parser.value_float();
+      if (WITHIN(K, 0, 10))
+        newK = K;
+      else
+        echo_value_oor('K');
+    }
+
+  #endif
+
+  if (newK != oldK) {
+    planner.synchronize();
+    kref = newK;
+  }
+
+  if (!parser.seen_any()) {
+
+    #if ENABLED(EXTRA_LIN_ADVANCE_K)
+
+      #if EXTRUDERS < 2
+        SERIAL_ECHOLNPAIR("Advance S", new_slot, " K", kref, "(S", !new_slot, " K", lref, ")");
+      #else
+        LOOP_L_N(i, EXTRUDERS) {
+          const bool slot = TEST(lin_adv_slot, i);
+          SERIAL_ECHOLNPAIR("Advance T", i, " S", slot, " K", planner.extruder_advance_K[i],
+                            "(S", !slot, " K", other_extruder_advance_K[i], ")");
+          SERIAL_EOL();
+        }
+      #endif
+
+    #else
+
+      SERIAL_ECHO_START();
+      #if EXTRUDERS < 2
+        SERIAL_ECHOLNPAIR("Advance K=", planner.extruder_advance_K[0]);
+      #else
+        SERIAL_ECHOPGM("Advance K");
+        LOOP_L_N(i, EXTRUDERS) {
+          SERIAL_CHAR(' ', '0' + i, ':');
+          SERIAL_DECIMAL(planner.extruder_advance_K[i]);
+        }
+        SERIAL_EOL();
+      #endif
+
+    #endif
+  }
+
+}
+
+#endif // LIN_ADVANCE

Marlin/src/gcode/feature/baricuda/M126-M129.cpp

@@ -0,0 +1,58 @@
+/**
+ * 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(BARICUDA)
+
+#include "../../gcode.h"
+#include "../../../feature/baricuda.h"
+
+#if HAS_HEATER_1
+
+  /**
+   * M126: Heater 1 valve open
+   */
+  void GcodeSuite::M126() { baricuda_valve_pressure = parser.byteval('S', 255); }
+
+  /**
+   * M127: Heater 1 valve close
+   */
+  void GcodeSuite::M127() { baricuda_valve_pressure = 0; }
+
+#endif // HAS_HEATER_1
+
+#if HAS_HEATER_2
+
+  /**
+   * M128: Heater 2 valve open
+   */
+  void GcodeSuite::M128() { baricuda_e_to_p_pressure = parser.byteval('S', 255); }
+
+  /**
+   * M129: Heater 2 valve close
+   */
+  void GcodeSuite::M129() { baricuda_e_to_p_pressure = 0; }
+
+#endif // HAS_HEATER_2
+
+#endif // BARICUDA

Marlin/src/gcode/feature/camera/M240.cpp

@@ -0,0 +1,204 @@
+/**
+ * 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(PHOTO_GCODE)
+
+#include "../../gcode.h"
+#include "../../../module/motion.h" // for active_extruder and current_position
+
+#if PIN_EXISTS(CHDK)
+  millis_t chdk_timeout; // = 0
+#endif
+
+#if defined(PHOTO_POSITION) && PHOTO_DELAY_MS > 0
+  #include "../../../MarlinCore.h" // for idle()
+#endif
+
+#ifdef PHOTO_RETRACT_MM
+
+  #define _PHOTO_RETRACT_MM (PHOTO_RETRACT_MM + 0)
+
+  #include "../../../module/planner.h"
+  #include "../../../module/temperature.h"
+
+  #if ENABLED(ADVANCED_PAUSE_FEATURE)
+    #include "../../../feature/pause.h"
+  #endif
+
+  #ifdef PHOTO_RETRACT_MM
+    inline void e_move_m240(const float length, const_feedRate_t fr_mm_s) {
+      if (length && thermalManager.hotEnoughToExtrude(active_extruder))
+        unscaled_e_move(length, fr_mm_s);
+    }
+  #endif
+
+#endif
+
+#if PIN_EXISTS(PHOTOGRAPH)
+
+  FORCE_INLINE void set_photo_pin(const uint8_t state) {
+    constexpr uint32_t pulse_length = (
+      #ifdef PHOTO_PULSES_US
+        PHOTO_PULSE_DELAY_US
+      #else
+        15                    // 15.24 from _delay_ms(0.01524)
+      #endif
+    );
+    WRITE(PHOTOGRAPH_PIN, state);
+    delayMicroseconds(pulse_length);
+  }
+
+  FORCE_INLINE void tweak_photo_pin() { set_photo_pin(HIGH); set_photo_pin(LOW); }
+
+  #ifdef PHOTO_PULSES_US
+
+    inline void pulse_photo_pin(const uint32_t duration, const uint8_t state) {
+      if (state) {
+        for (const uint32_t stop = micros() + duration; micros() < stop;)
+          tweak_photo_pin();
+      }
+      else
+        delayMicroseconds(duration);
+    }
+
+    inline void spin_photo_pin() {
+      static constexpr uint32_t sequence[] = PHOTO_PULSES_US;
+      LOOP_L_N(i, COUNT(sequence))
+        pulse_photo_pin(sequence[i], !(i & 1));
+    }
+
+  #else
+
+    constexpr uint8_t NUM_PULSES = 16;
+    inline void spin_photo_pin() { for (uint8_t i = NUM_PULSES; i--;) tweak_photo_pin(); }
+
+  #endif
+#endif
+
+/**
+ * M240: Trigger a camera by...
+ *
+ *  - CHDK                  : Emulate a Canon RC-1 with a configurable ON duration.
+ *                            https://captain-slow.dk/2014/03/09/3d-printing-timelapses/
+ *  - PHOTOGRAPH_PIN        : Pulse a digital pin 16 times.
+ *                            See https://www.doc-diy.net/photo/rc-1_hacked/
+ *  - PHOTO_SWITCH_POSITION : Bump a physical switch with the X-carriage using a
+ *                            configured position, delay, and retract length.
+ *
+ * PHOTO_POSITION parameters:
+ *    A - X offset to the return position
+ *    B - Y offset to the return position
+ *    F - Override the XY movement feedrate
+ *    R - Retract/recover length (current units)
+ *    S - Retract/recover feedrate (mm/m)
+ *    X - Move to X before triggering the shutter
+ *    Y - Move to Y before triggering the shutter
+ *    Z - Raise Z by a distance before triggering the shutter
+ *
+ * PHOTO_SWITCH_POSITION parameters:
+ *    D - Duration (ms) to hold down switch (Requires PHOTO_SWITCH_MS)
+ *    P - Delay (ms) after triggering the shutter (Requires PHOTO_SWITCH_MS)
+ *    I - Switch trigger position override X
+ *    J - Switch trigger position override Y
+ */
+void GcodeSuite::M240() {
+
+  #ifdef PHOTO_POSITION
+
+    if (homing_needed_error()) return;
+
+    const xyz_pos_t old_pos = {
+      current_position.x + parser.linearval('A'),
+      current_position.y + parser.linearval('B'),
+      current_position.z
+    };
+
+    #ifdef PHOTO_RETRACT_MM
+      const float rval = parser.seenval('R') ? parser.value_linear_units() : _PHOTO_RETRACT_MM;
+      feedRate_t sval = (
+        #if ENABLED(ADVANCED_PAUSE_FEATURE)
+          PAUSE_PARK_RETRACT_FEEDRATE
+        #elif ENABLED(FWRETRACT)
+          RETRACT_FEEDRATE
+        #else
+          45
+        #endif
+      );
+      if (parser.seenval('S')) sval = parser.value_feedrate();
+      e_move_m240(-rval, sval);
+    #endif
+
+    feedRate_t fr_mm_s = MMM_TO_MMS(parser.linearval('F'));
+    if (fr_mm_s) NOLESS(fr_mm_s, 10.0f);
+
+    constexpr xyz_pos_t photo_position = PHOTO_POSITION;
+    xyz_pos_t raw = {
+       parser.seenval('X') ? RAW_X_POSITION(parser.value_linear_units()) : photo_position.x,
+       parser.seenval('Y') ? RAW_Y_POSITION(parser.value_linear_units()) : photo_position.y,
+      (parser.seenval('Z') ? parser.value_linear_units() : photo_position.z) + current_position.z
+    };
+    apply_motion_limits(raw);
+    do_blocking_move_to(raw, fr_mm_s);
+
+    #ifdef PHOTO_SWITCH_POSITION
+      constexpr xy_pos_t photo_switch_position = PHOTO_SWITCH_POSITION;
+      const xy_pos_t sraw = {
+         parser.seenval('I') ? RAW_X_POSITION(parser.value_linear_units()) : photo_switch_position.x,
+         parser.seenval('J') ? RAW_Y_POSITION(parser.value_linear_units()) : photo_switch_position.y
+      };
+      do_blocking_move_to_xy(sraw, get_homing_bump_feedrate(X_AXIS));
+      #if PHOTO_SWITCH_MS > 0
+        safe_delay(parser.intval('D', PHOTO_SWITCH_MS));
+      #endif
+      do_blocking_move_to(raw);
+    #endif
+
+  #endif
+
+  #if PIN_EXISTS(CHDK)
+
+    OUT_WRITE(CHDK_PIN, HIGH);
+    chdk_timeout = millis() + parser.intval('D', PHOTO_SWITCH_MS);
+
+  #elif HAS_PHOTOGRAPH
+
+    spin_photo_pin();
+    delay(7.33);
+    spin_photo_pin();
+
+  #endif
+
+  #ifdef PHOTO_POSITION
+    #if PHOTO_DELAY_MS > 0
+      const millis_t timeout = millis() + parser.intval('P', PHOTO_DELAY_MS);
+      while (PENDING(millis(), timeout)) idle();
+    #endif
+    do_blocking_move_to(old_pos, fr_mm_s);
+    #ifdef PHOTO_RETRACT_MM
+      e_move_m240(rval, sval);
+    #endif
+  #endif
+}
+
+#endif // PHOTO_GCODE

Marlin/src/gcode/feature/cancel/M486.cpp

@@ -0,0 +1,57 @@
+/**
+ * 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(CANCEL_OBJECTS)
+
+#include "../../gcode.h"
+#include "../../../feature/cancel_object.h"
+
+/**
+ * M486: A simple interface to cancel objects
+ *
+ *   T[count] : Reset objects and/or set the count
+ *   S<index> : Start an object with the given index
+ *   P<index> : Cancel the object with the given index
+ *   U<index> : Un-cancel object with the given index
+ *   C        : Cancel the current object (the last index given by S<index>)
+ *   S-1      : Start a non-object like a brim or purge tower that should always print
+ */
+void GcodeSuite::M486() {
+
+  if (parser.seen('T')) {
+    cancelable.reset();
+    cancelable.object_count = parser.intval('T', 1);
+  }
+
+  if (parser.seen('S'))
+    cancelable.set_active_object(parser.value_int());
+
+  if (parser.seen('C')) cancelable.cancel_active_object();
+
+  if (parser.seen('P')) cancelable.cancel_object(parser.value_int());
+
+  if (parser.seen('U')) cancelable.uncancel_object(parser.value_int());
+}
+
+#endif // CANCEL_OBJECTS

Marlin/src/gcode/feature/caselight/M355.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 ENABLED(CASE_LIGHT_ENABLE)
+
+#include "../../../feature/caselight.h"
+#include "../../gcode.h"
+
+/**
+ * M355: Turn case light on/off and set brightness
+ *
+ *   P<byte>  Set case light brightness (PWM pin required - ignored otherwise)
+ *
+ *   S<bool>  Set case light on/off
+ *
+ *   When S turns on the light on a PWM pin then the current brightness level is used/restored
+ *
+ *   M355 P200 S0 turns off the light & sets the brightness level
+ *   M355 S1 turns on the light with a brightness of 200 (assuming a PWM pin)
+ */
+void GcodeSuite::M355() {
+  bool didset = false;
+  #if CASELIGHT_USES_BRIGHTNESS
+    if (parser.seenval('P')) {
+      didset = true;
+      caselight.brightness = parser.value_byte();
+    }
+  #endif
+  const bool sflag = parser.seenval('S');
+  if (sflag) {
+    didset = true;
+    caselight.on = parser.value_bool();
+  }
+  if (didset) caselight.update(sflag);
+
+  // Always report case light status
+  SERIAL_ECHO_START();
+  SERIAL_ECHOPGM("Case light: ");
+  if (!caselight.on)
+    SERIAL_ECHOLNPGM(STR_OFF);
+  else {
+    #if CASELIGHT_USES_BRIGHTNESS
+      if (TERN(CASE_LIGHT_USE_NEOPIXEL, true, TERN0(NEED_CASE_LIGHT_PIN, PWM_PIN(CASE_LIGHT_PIN)))) {
+        SERIAL_ECHOLN(int(caselight.brightness));
+        return;
+      }
+    #endif
+    SERIAL_ECHOLNPGM(STR_ON);
+  }
+}
+
+#endif // CASE_LIGHT_ENABLE

Marlin/src/gcode/feature/clean/G12.cpp

@@ -0,0 +1,81 @@
+/**
+ * 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(NOZZLE_CLEAN_FEATURE)
+
+#include "../../../libs/nozzle.h"
+
+#include "../../gcode.h"
+#include "../../parser.h"
+#include "../../../module/motion.h"
+
+#if HAS_LEVELING
+  #include "../../../module/planner.h"
+  #include "../../../feature/bedlevel/bedlevel.h"
+#endif
+
+/**
+ * G12: Clean the nozzle
+ *
+ *  E<bool>          : 0=Never or 1=Always apply the "software endstop" limits
+ *  P0 S<strokes>    : Stroke cleaning with S strokes
+ *  P1 Sn T<objects> : Zigzag cleaning with S repeats and T zigzags
+ *  P2 Sn R<radius>  : Circle cleaning with S repeats and R radius
+ *  X, Y, Z          : Specify axes to move during cleaning. Default: ALL.
+ */
+void GcodeSuite::G12() {
+  // Don't allow nozzle cleaning without homing first
+  if (homing_needed_error()) return;
+
+  #ifdef WIPE_SEQUENCE_COMMANDS
+    if (!parser.seen_any()) {
+      gcode.process_subcommands_now_P(PSTR(WIPE_SEQUENCE_COMMANDS));
+      return;
+    }
+  #endif
+
+  const uint8_t pattern = parser.ushortval('P', 0),
+                strokes = parser.ushortval('S', NOZZLE_CLEAN_STROKES),
+                objects = parser.ushortval('T', NOZZLE_CLEAN_TRIANGLES);
+  const float radius = parser.linearval('R', NOZZLE_CLEAN_CIRCLE_RADIUS);
+
+  const bool seenxyz = parser.seen("XYZ");
+  const uint8_t cleans =  (!seenxyz || parser.boolval('X') ? _BV(X_AXIS) : 0)
+                        | (!seenxyz || parser.boolval('Y') ? _BV(Y_AXIS) : 0)
+                        | TERN(NOZZLE_CLEAN_NO_Z, 0, (!seenxyz || parser.boolval('Z') ? _BV(Z_AXIS) : 0))
+                      ;
+
+  #if HAS_LEVELING
+    // Disable bed leveling if cleaning Z
+    TEMPORARY_BED_LEVELING_STATE(!TEST(cleans, Z_AXIS) && planner.leveling_active);
+  #endif
+
+  SET_SOFT_ENDSTOP_LOOSE(!parser.boolval('E'));
+
+  nozzle.clean(pattern, strokes, radius, objects, cleans);
+
+  SET_SOFT_ENDSTOP_LOOSE(false);
+}
+
+#endif // NOZZLE_CLEAN_FEATURE

Marlin/src/gcode/feature/controllerfan/M710.cpp

@@ -0,0 +1,81 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfigPre.h"
+
+#if ENABLED(CONTROLLER_FAN_EDITABLE)
+
+#include "../../gcode.h"
+#include "../../../feature/controllerfan.h"
+
+void M710_report(const bool forReplay=true) {
+  if (!forReplay) { SERIAL_ECHOLNPGM("; Controller Fan"); SERIAL_ECHO_START(); }
+  SERIAL_ECHOLNPAIR("  M710"
+    " S", int(controllerFan.settings.active_speed),
+    " I", int(controllerFan.settings.idle_speed),
+    " A", int(controllerFan.settings.auto_mode),
+    " D", controllerFan.settings.duration,
+    " ; (", (int(controllerFan.settings.active_speed) * 100) / 255, "%"
+    " ", (int(controllerFan.settings.idle_speed) * 100) / 255, "%)"
+  );
+}
+
+/**
+ * M710: Set controller fan settings
+ *
+ *  R         : Reset to defaults
+ *  S[0-255]  : Fan speed when motors are active
+ *  I[0-255]  : Fan speed when motors are idle
+ *  A[0|1]    : Turn auto mode on or off
+ *  D         : Set auto mode idle duration
+ *
+ * Examples:
+ *   M710                   ; Report current Settings
+ *   M710 R                 ; Reset SIAD to defaults
+ *   M710 I64               ; Set controller fan Idle Speed to 25%
+ *   M710 S255              ; Set controller fan Active Speed to 100%
+ *   M710 S0                ; Set controller fan Active Speed to OFF
+ *   M710 I255 A0           ; Set controller fan Idle Speed to 100% with Auto Mode OFF
+ *   M710 I127 A1 S255 D160 ; Set controller fan idle speed 50%, AutoMode On, Fan speed 100%, duration to 160 Secs
+ */
+void GcodeSuite::M710() {
+
+  const bool seenR = parser.seen('R');
+  if (seenR) controllerFan.reset();
+
+  const bool seenS = parser.seenval('S');
+  if (seenS) controllerFan.settings.active_speed = parser.value_byte();
+
+  const bool seenI = parser.seenval('I');
+  if (seenI) controllerFan.settings.idle_speed = parser.value_byte();
+
+  const bool seenA = parser.seenval('A');
+  if (seenA) controllerFan.settings.auto_mode = parser.value_bool();
+
+  const bool seenD = parser.seenval('D');
+  if (seenD) controllerFan.settings.duration = parser.value_ushort();
+
+  if (!(seenR || seenS || seenI || seenA || seenD))
+    M710_report();
+}
+
+#endif // CONTROLLER_FAN_EDITABLE

Marlin/src/gcode/feature/digipot/M907-M910.cpp

@@ -0,0 +1,104 @@
+/**
+ * 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 ANY(HAS_MOTOR_CURRENT_SPI, HAS_MOTOR_CURRENT_PWM, HAS_MOTOR_CURRENT_I2C, HAS_MOTOR_CURRENT_DAC)
+
+#include "../../gcode.h"
+
+#if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM
+  #include "../../../module/stepper.h"
+#endif
+
+#if HAS_MOTOR_CURRENT_I2C
+  #include "../../../feature/digipot/digipot.h"
+#endif
+
+#if HAS_MOTOR_CURRENT_DAC
+  #include "../../../feature/dac/stepper_dac.h"
+#endif
+
+/**
+ * M907: Set digital trimpot motor current using axis codes X, Y, Z, E, B, S
+ */
+void GcodeSuite::M907() {
+  #if HAS_MOTOR_CURRENT_SPI
+
+    LOOP_LOGICAL_AXES(i) if (parser.seenval(axis_codes[i])) stepper.set_digipot_current(i, parser.value_int());
+    if (parser.seenval('B')) stepper.set_digipot_current(4, parser.value_int());
+    if (parser.seenval('S')) LOOP_LE_N(i, 4) stepper.set_digipot_current(i, parser.value_int());
+
+  #elif HAS_MOTOR_CURRENT_PWM
+
+    #if ANY_PIN(MOTOR_CURRENT_PWM_X, MOTOR_CURRENT_PWM_Y, MOTOR_CURRENT_PWM_XY)
+      if (parser.seenval('X') || parser.seenval('Y')) stepper.set_digipot_current(0, parser.value_int());
+    #endif
+    #if PIN_EXISTS(MOTOR_CURRENT_PWM_Z)
+      if (parser.seenval('Z')) stepper.set_digipot_current(1, parser.value_int());
+    #endif
+    #if PIN_EXISTS(MOTOR_CURRENT_PWM_E)
+      if (parser.seenval('E')) stepper.set_digipot_current(2, parser.value_int());
+    #endif
+
+  #endif
+
+  #if HAS_MOTOR_CURRENT_I2C
+    // this one uses actual amps in floating point
+    LOOP_LOGICAL_AXES(i) if (parser.seenval(axis_codes[i])) digipot_i2c.set_current(i, parser.value_float());
+    // Additional extruders use B,C,D for channels 4,5,6.
+    // TODO: Change these parameters because 'E' is used. B<index>?
+    #if HAS_EXTRUDERS
+      for (uint8_t i = E_AXIS + 1; i < DIGIPOT_I2C_NUM_CHANNELS; i++)
+        if (parser.seenval('B' + i - (E_AXIS + 1))) digipot_i2c.set_current(i, parser.value_float());
+    #endif
+  #endif
+
+  #if HAS_MOTOR_CURRENT_DAC
+    if (parser.seenval('S')) {
+      const float dac_percent = parser.value_float();
+      LOOP_LE_N(i, 4) stepper_dac.set_current_percent(i, dac_percent);
+    }
+    LOOP_LOGICAL_AXES(i) if (parser.seenval(axis_codes[i])) stepper_dac.set_current_percent(i, parser.value_float());
+  #endif
+}
+
+#if EITHER(HAS_MOTOR_CURRENT_SPI, HAS_MOTOR_CURRENT_DAC)
+
+  /**
+   * M908: Control digital trimpot directly (M908 P<pin> S<current>)
+   */
+  void GcodeSuite::M908() {
+    TERN_(HAS_MOTOR_CURRENT_SPI, stepper.set_digipot_value_spi(parser.intval('P'), parser.intval('S')));
+    TERN_(HAS_MOTOR_CURRENT_DAC, stepper_dac.set_current_value(parser.byteval('P', -1), parser.ushortval('S', 0)));
+  }
+
+  #if HAS_MOTOR_CURRENT_DAC
+
+    void GcodeSuite::M909() { stepper_dac.print_values(); }
+    void GcodeSuite::M910() { stepper_dac.commit_eeprom(); }
+
+  #endif // HAS_MOTOR_CURRENT_DAC
+
+#endif // HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_DAC
+
+#endif // HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM || HAS_MOTOR_CURRENT_I2C || HAS_MOTOR_CURRENT_DAC

Marlin/src/gcode/feature/filwidth/M404-M407.cpp

@@ -0,0 +1,71 @@
+/**
+ * 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(FILAMENT_WIDTH_SENSOR)
+
+#include "../../../feature/filwidth.h"
+#include "../../../module/planner.h"
+#include "../../../MarlinCore.h"
+#include "../../gcode.h"
+
+/**
+ * M404: Display or set (in current units) the nominal filament width (3mm, 1.75mm ) W<3.0>
+ */
+void GcodeSuite::M404() {
+  if (parser.seenval('W')) {
+    filwidth.nominal_mm = parser.value_linear_units();
+    planner.volumetric_area_nominal = CIRCLE_AREA(filwidth.nominal_mm * 0.5);
+  }
+  else
+    SERIAL_ECHOLNPAIR("Filament dia (nominal mm):", filwidth.nominal_mm);
+}
+
+/**
+ * M405: Turn on filament sensor for control
+ */
+void GcodeSuite::M405() {
+  // This is technically a linear measurement, but since it's quantized to centimeters and is a different
+  // unit than everything else, it uses parser.value_byte() instead of parser.value_linear_units().
+  if (parser.seenval('D'))
+    filwidth.set_delay_cm(parser.value_byte());
+
+  filwidth.enable(true);
+}
+
+/**
+ * M406: Turn off filament sensor for control
+ */
+void GcodeSuite::M406() {
+  filwidth.enable(false);
+  planner.calculate_volumetric_multipliers();   // Restore correct 'volumetric_multiplier' value
+}
+
+/**
+ * M407: Get measured filament diameter on serial output
+ */
+void GcodeSuite::M407() {
+  SERIAL_ECHOLNPAIR("Filament dia (measured mm):", filwidth.measured_mm);
+}
+
+#endif // FILAMENT_WIDTH_SENSOR

Marlin/src/gcode/feature/fwretract/G10_G11.cpp

@@ -0,0 +1,51 @@
+/**
+ * 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(FWRETRACT)
+
+#include "../../../feature/fwretract.h"
+#include "../../gcode.h"
+#include "../../../module/motion.h"
+
+/**
+ * G10 - Retract filament according to settings of M207
+ *       TODO: Handle 'G10 P' for tool settings and 'G10 L' for workspace settings
+ */
+void GcodeSuite::G10() {
+  #if HAS_MULTI_EXTRUDER
+    const bool rs = parser.boolval('S');
+  #endif
+  fwretract.retract(true
+    #if HAS_MULTI_EXTRUDER
+      , rs
+    #endif
+  );
+}
+
+/**
+ * G11 - Recover filament according to settings of M208
+ */
+void GcodeSuite::G11() { fwretract.retract(false); }
+
+#endif // FWRETRACT

Marlin/src/gcode/feature/fwretract/M207-M209.cpp

@@ -0,0 +1,52 @@
+/**
+ * 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(FWRETRACT)
+
+#include "../../../feature/fwretract.h"
+#include "../../gcode.h"
+
+/**
+ * M207: Set firmware retraction values
+ */
+void GcodeSuite::M207() { fwretract.M207(); }
+
+/**
+ * M208: Set firmware un-retraction values
+ */
+void GcodeSuite::M208() { fwretract.M208(); }
+
+#if ENABLED(FWRETRACT_AUTORETRACT)
+
+  /**
+   * M209: Enable automatic retract (M209 S1)
+   *
+   *   For slicers that don't support G10/11, reversed
+   *   extruder-only moves can be classified as retraction.
+   */
+  void GcodeSuite::M209() { fwretract.M209(); }
+
+#endif
+
+#endif // FWRETRACT

Marlin/src/gcode/feature/i2c/M260_M261.cpp

@@ -0,0 +1,76 @@
+/**
+ * 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(EXPERIMENTAL_I2CBUS)
+
+#include "../../gcode.h"
+
+#include "../../../feature/twibus.h"
+
+/**
+ * M260: Send data to a I2C slave device
+ *
+ * This is a PoC, the formatting and arguments for the GCODE will
+ * change to be more compatible, the current proposal is:
+ *
+ *  M260 A<slave device address base 10> ; Sets the I2C slave address the data will be sent to
+ *
+ *  M260 B<byte-1 value in base 10>
+ *  M260 B<byte-2 value in base 10>
+ *  M260 B<byte-3 value in base 10>
+ *
+ *  M260 S1 ; Send the buffered data and reset the buffer
+ *  M260 R1 ; Reset the buffer without sending data
+ */
+void GcodeSuite::M260() {
+  // Set the target address
+  if (parser.seen('A')) i2c.address(parser.value_byte());
+
+  // Add a new byte to the buffer
+  if (parser.seen('B')) i2c.addbyte(parser.value_byte());
+
+  // Flush the buffer to the bus
+  if (parser.seen('S')) i2c.send();
+
+  // Reset and rewind the buffer
+  else if (parser.seen('R')) i2c.reset();
+}
+
+/**
+ * M261: Request X bytes from I2C slave device
+ *
+ * Usage: M261 A<slave device address base 10> B<number of bytes>
+ */
+void GcodeSuite::M261() {
+  if (parser.seen('A')) i2c.address(parser.value_byte());
+
+  uint8_t bytes = parser.byteval('B', 1);
+
+  if (i2c.addr && bytes && bytes <= TWIBUS_BUFFER_SIZE)
+    i2c.relay(bytes);
+  else
+    SERIAL_ERROR_MSG("Bad i2c request");
+}
+
+#endif

Marlin/src/gcode/feature/leds/M150.cpp

@@ -0,0 +1,91 @@
+/**
+ * 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_COLOR_LEDS
+
+#include "../../gcode.h"
+#include "../../../feature/leds/leds.h"
+
+/**
+ * M150: Set Status LED Color - Use R-U-B-W for R-G-B-W
+ *       and Brightness       - Use P (for NEOPIXEL only)
+ *
+ * Always sets all 3 or 4 components. If a component is left out, set to 0.
+ *                                    If brightness is left out, no value changed
+ *
+ * With NEOPIXEL_LED:
+ *  I<index>  Set the NeoPixel index to affect. Default: All
+ *
+ * With NEOPIXEL2_SEPARATE:
+ *  S<index>  The NeoPixel strip to set. Default is index 0.
+ *
+ * Examples:
+ *
+ *   M150 R255       ; Turn LED red
+ *   M150 R255 U127  ; Turn LED orange (PWM only)
+ *   M150            ; Turn LED off
+ *   M150 R U B      ; Turn LED white
+ *   M150 W          ; Turn LED white using a white LED
+ *   M150 P127       ; Set LED 50% brightness
+ *   M150 P          ; Set LED full brightness
+ *   M150 I1 R       ; Set NEOPIXEL index 1 to red
+ *   M150 S1 I1 R    ; Set SEPARATE index 1 to red
+ */
+void GcodeSuite::M150() {
+  #if ENABLED(NEOPIXEL_LED)
+    const int8_t index = parser.intval('I', -1);
+    #if ENABLED(NEOPIXEL2_SEPARATE)
+      int8_t brightness, unit = parser.intval('S', -1);
+      switch (unit) {
+        case -1: neo2.neoindex = index; // fall-thru
+        case  0:  neo.neoindex = index; brightness =  neo.brightness(); break;
+        case  1: neo2.neoindex = index; brightness = neo2.brightness(); break;
+      }
+    #else
+      const uint8_t brightness = neo.brightness();
+      neo.neoindex = index;
+    #endif
+  #endif
+
+  const LEDColor color = LEDColor(
+    parser.seen('R') ? (parser.has_value() ? parser.value_byte() : 255) : 0,
+    parser.seen('U') ? (parser.has_value() ? parser.value_byte() : 255) : 0,
+    parser.seen('B') ? (parser.has_value() ? parser.value_byte() : 255) : 0
+    OPTARG(HAS_WHITE_LED, parser.seen('W') ? (parser.has_value() ? parser.value_byte() : 255) : 0)
+    OPTARG(NEOPIXEL_LED, parser.seen('P') ? (parser.has_value() ? parser.value_byte() : 255) : brightness)
+  );
+
+  #if ENABLED(NEOPIXEL2_SEPARATE)
+    switch (unit) {
+      case 0: leds.set_color(color); return;
+      case 1: leds2.set_color(color); return;
+    }
+  #endif
+
+  // If 'S' is not specified use both
+  leds.set_color(color);
+  TERN_(NEOPIXEL2_SEPARATE, leds2.set_color(color));
+}
+
+#endif // HAS_COLOR_LEDS

Marlin/src/gcode/feature/leds/M7219.cpp

@@ -0,0 +1,93 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfigPre.h"
+
+#if ENABLED(MAX7219_GCODE)
+
+#include "../../gcode.h"
+#include "../../../feature/max7219.h"
+
+/**
+ * M7219: Control the Max7219 LED matrix
+ *
+ *  I         - Initialize (clear) the matrix
+ *  F         - Fill the matrix (set all bits)
+ *  P         - Dump the led_line[] array values
+ *  C<column> - Set a column to the bitmask given by 'V' (Units 0-3 in portrait layout)
+ *  R<row>    - Set a row to the bitmask given by 'V' (Units 0-3 in landscape layout)
+ *  X<pos>    - X index of an LED to set or toggle
+ *  Y<pos>    - Y index of an LED to set or toggle
+ *  V<value>  - LED on/off state or row/column bitmask (8, 16, 24, or 32-bits)
+ *              ('C' / 'R' can be used to update up to 4 units at once)
+ *
+ * Directly set a native matrix row to the 8-bit value 'V':
+ *  D<line>   - Display line (0..7)
+ *  U<unit>   - Unit index (0..MAX7219_NUMBER_UNITS-1)
+ */
+void GcodeSuite::M7219() {
+  if (parser.seen('I')) {
+    max7219.register_setup();
+    max7219.clear();
+  }
+
+  if (parser.seen('F')) max7219.fill();
+
+  const uint32_t v = parser.ulongval('V');
+
+  if (parser.seenval('R')) {
+    const uint8_t r = parser.value_byte();
+    max7219.set_row(r, v);
+  }
+  else if (parser.seenval('C')) {
+    const uint8_t c = parser.value_byte();
+    max7219.set_column(c, v);
+  }
+  else if (parser.seenval('X') || parser.seenval('Y')) {
+    const uint8_t x = parser.byteval('X'), y = parser.byteval('Y');
+    if (parser.seenval('V'))
+      max7219.led_set(x, y, v > 0);
+    else
+      max7219.led_toggle(x, y);
+  }
+  else if (parser.seen('D')) {
+    const uint8_t uline = parser.value_byte() & 0x7,
+                  line = uline + (parser.byteval('U') << 3);
+    if (line < MAX7219_LINES) {
+      max7219.led_line[line] = v;
+      return max7219.refresh_line(line);
+    }
+  }
+
+  if (parser.seen('P')) {
+    LOOP_L_N(r, MAX7219_LINES) {
+      SERIAL_ECHOPGM("led_line[");
+      if (r < 10) SERIAL_CHAR(' ');
+      SERIAL_ECHO(r);
+      SERIAL_ECHOPGM("]=");
+      for (uint8_t b = 8; b--;) SERIAL_CHAR('0' + TEST(max7219.led_line[r], b));
+      SERIAL_EOL();
+    }
+  }
+}
+
+#endif // MAX7219_GCODE

Marlin/src/gcode/feature/macro/M810-M819.cpp

@@ -0,0 +1,65 @@
+/**
+ * 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(GCODE_MACROS)
+
+#include "../../gcode.h"
+#include "../../queue.h"
+#include "../../parser.h"
+
+char gcode_macros[GCODE_MACROS_SLOTS][GCODE_MACROS_SLOT_SIZE + 1] = {{ 0 }};
+
+/**
+ * M810_819: Set/execute a G-code macro.
+ *
+ * Usage:
+ *   M810 <command>|...   Set Macro 0 to the given commands, separated by the pipe character
+ *   M810                 Execute Macro 0
+ */
+void GcodeSuite::M810_819() {
+  const uint8_t index = parser.codenum - 810;
+  if (index >= GCODE_MACROS_SLOTS) return;
+
+  const size_t len = strlen(parser.string_arg);
+
+  if (len) {
+    // Set a macro
+    if (len > GCODE_MACROS_SLOT_SIZE)
+      SERIAL_ERROR_MSG("Macro too long.");
+    else {
+      char c, *s = parser.string_arg, *d = gcode_macros[index];
+      do {
+        c = *s++;
+        *d++ = c == '|' ? '\n' : c;
+      } while (c);
+    }
+  }
+  else {
+    // Execute a macro
+    char * const cmd = gcode_macros[index];
+    if (strlen(cmd)) process_subcommands_now(cmd);
+  }
+}
+
+#endif // GCODE_MACROS

Marlin/src/gcode/feature/mixing/M163-M165.cpp

@@ -0,0 +1,101 @@
+/**
+ * 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(MIXING_EXTRUDER)
+
+#include "../../gcode.h"
+#include "../../../feature/mixing.h"
+
+/**
+ * M163: Set a single mix factor for a mixing extruder
+ *       This is called "weight" by some systems.
+ *       Must be followed by M164 to normalize and commit them.
+ *
+ *   S[index]   The channel index to set
+ *   P[float]   The mix value
+ */
+void GcodeSuite::M163() {
+  const int mix_index = parser.intval('S');
+  if (mix_index < MIXING_STEPPERS)
+    mixer.set_collector(mix_index, parser.floatval('P'));
+}
+
+/**
+ * M164: Normalize and commit the mix.
+ *
+ *   S[index]   The virtual tool to store
+ *              If 'S' is omitted update the active virtual tool.
+ */
+void GcodeSuite::M164() {
+  #if MIXING_VIRTUAL_TOOLS > 1
+    const int tool_index = parser.intval('S', -1);
+  #else
+    constexpr int tool_index = 0;
+  #endif
+  if (tool_index >= 0) {
+    if (tool_index < MIXING_VIRTUAL_TOOLS)
+      mixer.normalize(tool_index);
+  }
+  else
+    mixer.normalize();
+}
+
+#if ENABLED(DIRECT_MIXING_IN_G1)
+
+  /**
+   * M165: Set multiple mix factors for a mixing extruder.
+   *       Omitted factors will be set to 0.
+   *       The mix is normalized and stored in the current virtual tool.
+   *
+   *   A[factor] Mix factor for extruder stepper 1
+   *   B[factor] Mix factor for extruder stepper 2
+   *   C[factor] Mix factor for extruder stepper 3
+   *   D[factor] Mix factor for extruder stepper 4
+   *   H[factor] Mix factor for extruder stepper 5
+   *   I[factor] Mix factor for extruder stepper 6
+   */
+  void GcodeSuite::M165() {
+    // Get mixing parameters from the GCode
+    // The total "must" be 1.0 (but it will be normalized)
+    // If no mix factors are given, the old mix is preserved
+    const char mixing_codes[] = { LIST_N(MIXING_STEPPERS, 'A', 'B', 'C', 'D', 'H', 'I') };
+    uint8_t mix_bits = 0;
+    MIXER_STEPPER_LOOP(i) {
+      if (parser.seenval(mixing_codes[i])) {
+        SBI(mix_bits, i);
+        mixer.set_collector(i, parser.value_float());
+      }
+    }
+    // If any mixing factors were included, clear the rest
+    // If none were included, preserve the last mix
+    if (mix_bits) {
+      MIXER_STEPPER_LOOP(i)
+        if (!TEST(mix_bits, i)) mixer.set_collector(i, 0.0f);
+      mixer.normalize();
+    }
+  }
+
+#endif // DIRECT_MIXING_IN_G1
+
+#endif // MIXING_EXTRUDER

Marlin/src/gcode/feature/mixing/M166.cpp

@@ -0,0 +1,103 @@
+/**
+ * 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(GRADIENT_MIX)
+
+#include "../../gcode.h"
+#include "../../../module/motion.h"
+#include "../../../module/planner.h"
+#include "../../../feature/mixing.h"
+
+inline void echo_mix() {
+  SERIAL_ECHOPAIR(" (", mixer.mix[0], "%|", mixer.mix[1], "%)");
+}
+
+inline void echo_zt(const int t, const_float_t z) {
+  mixer.update_mix_from_vtool(t);
+  SERIAL_ECHOPAIR_P(SP_Z_STR, z, SP_T_STR, t);
+  echo_mix();
+}
+
+/**
+ * M166: Set a simple gradient mix for a two-component mixer
+ *       based on the Geeetech A10M implementation by Jone Liu.
+ *
+ *   S[bool]  - Enable / disable gradients
+ *   A[float] - Starting Z for the gradient
+ *   Z[float] - Ending Z for the gradient. (Must be greater than the starting Z.)
+ *   I[index] - V-Tool to use as the starting mix.
+ *   J[index] - V-Tool to use as the ending mix.
+ *
+ *   T[index] - A V-Tool index to use as an alias for the Gradient (Requires GRADIENT_VTOOL)
+ *              T with no index clears the setting. Note: This can match the I or J value.
+ *
+ * Example: M166 S1 A0 Z20 I0 J1
+ */
+void GcodeSuite::M166() {
+  if (parser.seenval('A')) mixer.gradient.start_z = parser.value_float();
+  if (parser.seenval('Z')) mixer.gradient.end_z = parser.value_float();
+  if (parser.seenval('I')) mixer.gradient.start_vtool = (uint8_t)constrain(parser.value_int(), 0, MIXING_VIRTUAL_TOOLS);
+  if (parser.seenval('J')) mixer.gradient.end_vtool = (uint8_t)constrain(parser.value_int(), 0, MIXING_VIRTUAL_TOOLS);
+
+  #if ENABLED(GRADIENT_VTOOL)
+    if (parser.seen('T')) mixer.gradient.vtool_index = parser.byteval('T', -1);
+  #endif
+
+  if (parser.seen('S')) mixer.gradient.enabled = parser.value_bool();
+
+  mixer.refresh_gradient();
+
+  SERIAL_ECHOPGM("Gradient Mix ");
+  serialprint_onoff(mixer.gradient.enabled);
+  if (mixer.gradient.enabled) {
+
+    #if ENABLED(GRADIENT_VTOOL)
+      if (mixer.gradient.vtool_index >= 0) {
+        SERIAL_ECHOPAIR(" (T", mixer.gradient.vtool_index);
+        SERIAL_CHAR(')');
+      }
+    #endif
+
+    SERIAL_ECHOPGM(" ; Start");
+    echo_zt(mixer.gradient.start_vtool, mixer.gradient.start_z);
+
+    SERIAL_ECHOPGM(" ; End");
+    echo_zt(mixer.gradient.end_vtool, mixer.gradient.end_z);
+
+    mixer.update_mix_from_gradient();
+
+    SERIAL_ECHOPGM(" ; Current Z");
+    #if ENABLED(DELTA)
+      get_cartesian_from_steppers();
+      SERIAL_ECHO(cartes.z);
+    #else
+      SERIAL_ECHO(planner.get_axis_position_mm(Z_AXIS));
+    #endif
+    echo_mix();
+  }
+
+  SERIAL_EOL();
+}
+
+#endif // GRADIENT_MIX

Marlin/src/gcode/feature/network/M552-M554.cpp

@@ -0,0 +1,126 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfigPre.h"
+
+#if HAS_ETHERNET
+
+#include "../../../feature/ethernet.h"
+#include "../../../core/serial.h"
+#include "../../gcode.h"
+
+void say_ethernet() { SERIAL_ECHOPGM("  Ethernet "); }
+
+void ETH0_report() {
+  say_ethernet();
+  SERIAL_ECHO_TERNARY(ethernet.hardware_enabled, "port ", "en", "dis", "abled.\n");
+  if (ethernet.hardware_enabled) {
+    say_ethernet();
+    SERIAL_ECHO_TERNARY(ethernet.have_telnet_client, "client ", "en", "dis", "abled.\n");
+  }
+  else
+    SERIAL_ECHOLNPGM("Send 'M552 S1' to enable.");
+}
+
+void MAC_report() {
+  uint8_t mac[6];
+  if (ethernet.hardware_enabled) {
+    Ethernet.MACAddress(mac);
+    SERIAL_ECHOPGM("  MAC: ");
+    LOOP_L_N(i, 6) {
+      if (mac[i] < 16) SERIAL_CHAR('0');
+      SERIAL_PRINT(mac[i], PrintBase::Hex);
+      if (i < 5) SERIAL_CHAR(':');
+    }
+  }
+  SERIAL_EOL();
+}
+
+// Display current values when the link is active,
+// otherwise show the stored values
+void ip_report(const uint16_t cmd, PGM_P const post, const IPAddress &ipo) {
+  SERIAL_CHAR('M'); SERIAL_ECHO(cmd); SERIAL_CHAR(' ');
+  LOOP_L_N(i, 4) {
+    SERIAL_ECHO(ipo[i]);
+    if (i < 3) SERIAL_CHAR('.');
+  }
+  SERIAL_ECHOPGM(" ; ");
+  SERIAL_ECHOPGM_P(post);
+  SERIAL_EOL();
+}
+void M552_report() {
+  ip_report(552, PSTR("ip address"), Ethernet.linkStatus() == LinkON ? Ethernet.localIP() : ethernet.ip);
+}
+void M553_report() {
+  ip_report(553, PSTR("subnet mask"), Ethernet.linkStatus() == LinkON ? Ethernet.subnetMask() : ethernet.subnet);
+}
+void M554_report() {
+  ip_report(554, PSTR("gateway"), Ethernet.linkStatus() == LinkON ? Ethernet.gatewayIP() : ethernet.gateway);
+}
+
+/**
+ * M552: Set IP address, enable/disable network interface
+ *
+ *  S0   : disable networking
+ *  S1   : enable networking
+ *  S-1  : reset network interface
+ *
+ *  Pnnn : Set IP address, 0.0.0.0 means acquire an IP address using DHCP
+ */
+void GcodeSuite::M552() {
+  const bool seenP = parser.seenval('P');
+  if (seenP) ethernet.ip.fromString(parser.value_string());
+
+  const bool seenS = parser.seenval('S');
+  if (seenS) {
+    switch (parser.value_int()) {
+      case -1:
+        if (ethernet.telnetClient) ethernet.telnetClient.stop();
+        ethernet.init();
+        break;
+      case 0: ethernet.hardware_enabled = false; break;
+      case 1: ethernet.hardware_enabled = true; break;
+      default: break;
+    }
+  }
+  const bool nopar = !seenS && !seenP;
+  if (nopar || seenS) ETH0_report();
+  if (nopar || seenP) M552_report();
+}
+
+/**
+ * M553 Pnnn - Set netmask
+ */
+void GcodeSuite::M553() {
+  if (parser.seenval('P')) ethernet.subnet.fromString(parser.value_string());
+  M553_report();
+}
+
+/**
+ * M554 Pnnn - Set Gateway
+ */
+void GcodeSuite::M554() {
+  if (parser.seenval('P')) ethernet.gateway.fromString(parser.value_string());
+  M554_report();
+}
+
+#endif // HAS_ETHERNET

Marlin/src/gcode/feature/password/M510-M512.cpp

@@ -0,0 +1,83 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfigPre.h"
+
+#if ENABLED(PASSWORD_FEATURE)
+
+#include "../../../feature/password/password.h"
+#include "../../../core/serial.h"
+#include "../../gcode.h"
+
+//
+// M510: Lock Printer
+//
+void GcodeSuite::M510() {
+  password.lock_machine();
+}
+
+//
+// M511: Unlock Printer
+//
+#if ENABLED(PASSWORD_UNLOCK_GCODE)
+
+  void GcodeSuite::M511() {
+    if (password.is_locked) {
+      password.value_entry = parser.ulongval('P');
+      password.authentication_check();
+    }
+  }
+
+#endif // PASSWORD_UNLOCK_GCODE
+
+//
+// M512: Set/Change/Remove Password
+//
+#if ENABLED(PASSWORD_CHANGE_GCODE)
+
+  void GcodeSuite::M512() {
+    if (password.is_set && parser.ulongval('P') != password.value) {
+      SERIAL_ECHOLNPGM(STR_WRONG_PASSWORD);
+      return;
+    }
+
+    if (parser.seenval('S')) {
+      password.value_entry = parser.ulongval('S');
+
+      if (password.value_entry < CAT(1e, PASSWORD_LENGTH)) {
+        password.is_set = true;
+        password.value = password.value_entry;
+        SERIAL_ECHOLNPAIR(STR_PASSWORD_SET, password.value); // TODO: Update password.string
+      }
+      else
+        SERIAL_ECHOLNPGM(STR_PASSWORD_TOO_LONG);
+    }
+    else {
+      password.is_set = false;
+      SERIAL_ECHOLNPGM(STR_PASSWORD_REMOVED);
+    }
+    SERIAL_ECHOLNPGM(STR_REMINDER_SAVE_SETTINGS);
+  }
+
+#endif // PASSWORD_CHANGE_GCODE
+
+#endif // PASSWORD_FEATURE

Marlin/src/gcode/feature/pause/G27.cpp

@@ -0,0 +1,41 @@
+/**
+ * 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(NOZZLE_PARK_FEATURE)
+
+#include "../../gcode.h"
+#include "../../../libs/nozzle.h"
+#include "../../../module/motion.h"
+
+/**
+ * G27: Park the nozzle
+ */
+void GcodeSuite::G27() {
+  // Don't allow nozzle parking without homing first
+  if (homing_needed_error()) return;
+  nozzle.park(parser.ushortval('P'));
+}
+
+#endif // NOZZLE_PARK_FEATURE

Marlin/src/gcode/feature/pause/G60.cpp

@@ -0,0 +1,60 @@
+/**
+ * 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 SAVED_POSITIONS
+
+#include "../../gcode.h"
+#include "../../../module/motion.h"
+
+#define DEBUG_OUT ENABLED(SAVED_POSITIONS_DEBUG)
+#include "../../../core/debug_out.h"
+
+/**
+ * G60: Save current position
+ *
+ *   S<slot> - Memory slot # (0-based) to save into (default 0)
+ */
+void GcodeSuite::G60() {
+  const uint8_t slot = parser.byteval('S');
+
+  if (slot >= SAVED_POSITIONS) {
+    SERIAL_ERROR_MSG(STR_INVALID_POS_SLOT STRINGIFY(SAVED_POSITIONS));
+    return;
+  }
+
+  stored_position[slot] = current_position;
+  SBI(saved_slots[slot >> 3], slot & 0x07);
+
+  #if ENABLED(SAVED_POSITIONS_DEBUG)
+    DEBUG_ECHOPAIR(STR_SAVED_POS " S", slot);
+    const xyze_pos_t &pos = stored_position[slot];
+    DEBUG_ECHOLNPAIR_F_P(
+      LIST_N(DOUBLE(LOGICAL_AXES), SP_E_STR, pos.e,
+      PSTR(" : X"), pos.x, SP_Y_STR, pos.y, SP_Z_STR, pos.z,
+      SP_I_STR, pos.i, SP_J_STR, pos.j, SP_K_STR, pos.k)
+    );
+  #endif
+}
+
+#endif // SAVED_POSITIONS

Marlin/src/gcode/feature/pause/G61.cpp

@@ -0,0 +1,95 @@
+/**
+ * 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 SAVED_POSITIONS
+
+#include "../../../module/planner.h"
+#include "../../gcode.h"
+#include "../../../module/motion.h"
+#include "../../../module/planner.h"
+
+#define DEBUG_OUT ENABLED(SAVED_POSITIONS_DEBUG)
+#include "../../../core/debug_out.h"
+
+/**
+ * G61: Return to saved position
+ *
+ *   F<rate>  - Feedrate (optional) for the move back.
+ *   S<slot>  - Slot # (0-based) to restore from (default 0).
+ *   X Y Z E  - Axes to restore. At least one is required.
+ *
+ *   If XYZE are not given, default restore uses the smart blocking move.
+ */
+void GcodeSuite::G61(void) {
+
+  const uint8_t slot = parser.byteval('S');
+
+  #define SYNC_E(POINT) TERN_(HAS_EXTRUDERS, planner.set_e_position_mm((destination.e = current_position.e = (POINT))))
+
+  #if SAVED_POSITIONS < 256
+    if (slot >= SAVED_POSITIONS) {
+      SERIAL_ERROR_MSG(STR_INVALID_POS_SLOT STRINGIFY(SAVED_POSITIONS));
+      return;
+    }
+  #endif
+
+  // No saved position? No axes being restored?
+  if (!TEST(saved_slots[slot >> 3], slot & 0x07)) return;
+
+  // Apply any given feedrate over 0.0
+  feedRate_t saved_feedrate = feedrate_mm_s;
+  const float fr = parser.linearval('F');
+  if (fr > 0.0) feedrate_mm_s = MMM_TO_MMS(fr);
+
+  if (!parser.seen_axis()) {
+    DEBUG_ECHOLNPGM("Default position restore");
+    do_blocking_move_to(stored_position[slot], feedrate_mm_s);
+    SYNC_E(stored_position[slot].e);
+  }
+  else {
+    if (parser.seen(LINEAR_AXIS_GANG("X", "Y", "Z", AXIS4_STR, AXIS5_STR, AXIS6_STR))) {
+      DEBUG_ECHOPAIR(STR_RESTORING_POS " S", slot);
+      LOOP_LINEAR_AXES(i) {
+        destination[i] = parser.seen(AXIS_CHAR(i))
+          ? stored_position[slot][i] + parser.value_axis_units((AxisEnum)i)
+          : current_position[i];
+        DEBUG_CHAR(' ', AXIS_CHAR(i));
+        DEBUG_ECHO_F(destination[i]);
+      }
+      DEBUG_EOL();
+      // Move to the saved position
+      prepare_line_to_destination();
+    }
+    #if HAS_EXTRUDERS
+      if (parser.seen_test('E')) {
+        DEBUG_ECHOLNPAIR(STR_RESTORING_POS " S", slot, " E", current_position.e, "=>", stored_position[slot].e);
+        SYNC_E(stored_position[slot].e);
+      }
+    #endif
+  }
+
+  feedrate_mm_s = saved_feedrate;
+}
+
+#endif // SAVED_POSITIONS

Marlin/src/gcode/feature/pause/M125.cpp

@@ -0,0 +1,89 @@
+/**
+ * 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(PARK_HEAD_ON_PAUSE)
+
+#include "../../gcode.h"
+#include "../../parser.h"
+#include "../../../feature/pause.h"
+#include "../../../lcd/marlinui.h"
+#include "../../../module/motion.h"
+#include "../../../module/printcounter.h"
+#include "../../../sd/cardreader.h"
+
+#if ENABLED(POWER_LOSS_RECOVERY)
+  #include "../../../feature/powerloss.h"
+#endif
+
+/**
+ * M125: Store current position and move to parking position.
+ *       Called on pause (by M25) to prevent material leaking onto the
+ *       object. On resume (M24) the head will be moved back and the
+ *       print will resume.
+ *
+ *       When not actively SD printing, M125 simply moves to the park
+ *       position and waits, resuming with a button click or M108.
+ *       Without PARK_HEAD_ON_PAUSE the M125 command does nothing.
+ *
+ *    L<linear> = Override retract Length
+ *    X<pos>    = Override park position X
+ *    Y<pos>    = Override park position Y
+ *    Z<linear> = Override Z raise
+ *
+ *  With an LCD menu:
+ *    P<bool>   = Always show a prompt and await a response
+ */
+void GcodeSuite::M125() {
+  // Initial retract before move to filament change position
+  const float retract = TERN0(HAS_EXTRUDERS, -ABS(parser.axisunitsval('L', E_AXIS, PAUSE_PARK_RETRACT_LENGTH)));
+
+  xyz_pos_t park_point = NOZZLE_PARK_POINT;
+
+  // Move XY axes to filament change position or given position
+  if (parser.seenval('X')) park_point.x = RAW_X_POSITION(parser.linearval('X'));
+  if (parser.seenval('Y')) park_point.y = RAW_X_POSITION(parser.linearval('Y'));
+
+  // Lift Z axis
+  if (parser.seenval('Z')) park_point.z = parser.linearval('Z');
+
+  #if HAS_HOTEND_OFFSET && NONE(DUAL_X_CARRIAGE, DELTA)
+    park_point += hotend_offset[active_extruder];
+  #endif
+
+  const bool sd_printing = TERN0(SDSUPPORT, IS_SD_PRINTING());
+
+  ui.pause_show_message(PAUSE_MESSAGE_PARKING, PAUSE_MODE_PAUSE_PRINT);
+
+  // If possible, show an LCD prompt with the 'P' flag
+  const bool show_lcd = TERN0(HAS_LCD_MENU, parser.boolval('P'));
+
+  if (pause_print(retract, park_point, show_lcd, 0)) {
+    if (ENABLED(EXTENSIBLE_UI) || BOTH(EMERGENCY_PARSER, HOST_PROMPT_SUPPORT) || !sd_printing || show_lcd) {
+      wait_for_confirmation(false, 0);
+      resume_print(0, 0, -retract, 0);
+    }
+  }
+}
+
+#endif // PARK_HEAD_ON_PAUSE

Marlin/src/gcode/feature/pause/M600.cpp

@@ -0,0 +1,168 @@
+/**
+ * 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(ADVANCED_PAUSE_FEATURE)
+
+#include "../../gcode.h"
+#include "../../../feature/pause.h"
+#include "../../../module/motion.h"
+#include "../../../module/printcounter.h"
+#include "../../../lcd/marlinui.h"
+
+#if HAS_MULTI_EXTRUDER
+  #include "../../../module/tool_change.h"
+#endif
+
+#if ENABLED(MMU2_MENUS)
+  #include "../../../lcd/menu/menu_mmu2.h"
+#endif
+
+#if ENABLED(MIXING_EXTRUDER)
+  #include "../../../feature/mixing.h"
+#endif
+
+#if HAS_FILAMENT_SENSOR
+  #include "../../../feature/runout.h"
+#endif
+
+/**
+ * M600: Pause for filament change
+ *
+ *  E[distance] - Retract the filament this far
+ *  Z[distance] - Move the Z axis by this distance
+ *  X[position] - Move to this X position, with Y
+ *  Y[position] - Move to this Y position, with X
+ *  U[distance] - Retract distance for removal (manual reload)
+ *  L[distance] - Extrude distance for insertion (manual reload)
+ *  B[count]    - Number of times to beep, -1 for indefinite (if equipped with a buzzer)
+ *  T[toolhead] - Select extruder for filament change
+ *  R[temp]     - Resume temperature (in current units)
+ *
+ *  Default values are used for omitted arguments.
+ */
+void GcodeSuite::M600() {
+
+  #if ENABLED(MIXING_EXTRUDER)
+    const int8_t target_e_stepper = get_target_e_stepper_from_command();
+    if (target_e_stepper < 0) return;
+
+    const uint8_t old_mixing_tool = mixer.get_current_vtool();
+    mixer.T(MIXER_DIRECT_SET_TOOL);
+
+    MIXER_STEPPER_LOOP(i) mixer.set_collector(i, i == uint8_t(target_e_stepper) ? 1.0 : 0.0);
+    mixer.normalize();
+
+    const int8_t target_extruder = active_extruder;
+  #else
+    const int8_t target_extruder = get_target_extruder_from_command();
+    if (target_extruder < 0) return;
+  #endif
+
+  #if ENABLED(DUAL_X_CARRIAGE)
+    int8_t DXC_ext = target_extruder;
+    if (!parser.seen_test('T')) {  // If no tool index is specified, M600 was (probably) sent in response to filament runout.
+                                   // In this case, for duplicating modes set DXC_ext to the extruder that ran out.
+      #if MULTI_FILAMENT_SENSOR
+        if (idex_is_duplicating())
+          DXC_ext = (READ(FIL_RUNOUT2_PIN) == FIL_RUNOUT2_STATE) ? 1 : 0;
+      #else
+        DXC_ext = active_extruder;
+      #endif
+    }
+  #endif
+
+  // Show initial "wait for start" message
+  #if DISABLED(MMU2_MENUS)
+    ui.pause_show_message(PAUSE_MESSAGE_CHANGING, PAUSE_MODE_PAUSE_PRINT, target_extruder);
+  #endif
+
+  #if ENABLED(HOME_BEFORE_FILAMENT_CHANGE)
+    // If needed, home before parking for filament change
+    home_if_needed(true);
+  #endif
+
+  #if HAS_MULTI_EXTRUDER
+    // Change toolhead if specified
+    const uint8_t active_extruder_before_filament_change = active_extruder;
+    if (active_extruder != target_extruder && TERN1(DUAL_X_CARRIAGE, !idex_is_duplicating()))
+      tool_change(target_extruder, false);
+  #endif
+
+  // Initial retract before move to filament change position
+  const float retract = -ABS(parser.axisunitsval('E', E_AXIS, PAUSE_PARK_RETRACT_LENGTH));
+
+  xyz_pos_t park_point NOZZLE_PARK_POINT;
+
+  // Lift Z axis
+  if (parser.seenval('Z')) park_point.z = parser.linearval('Z');
+
+  // Move XY axes to filament change position or given position
+  if (parser.seenval('X')) park_point.x = parser.linearval('X');
+  if (parser.seenval('Y')) park_point.y = parser.linearval('Y');
+
+  #if HAS_HOTEND_OFFSET && NONE(DUAL_X_CARRIAGE, DELTA)
+    park_point += hotend_offset[active_extruder];
+  #endif
+
+  #if ENABLED(MMU2_MENUS)
+    // For MMU2 reset retract and load/unload values so they don't mess with MMU filament handling
+    constexpr float unload_length = 0.5f,
+                    slow_load_length = 0.0f,
+                    fast_load_length = 0.0f;
+  #else
+    // Unload filament
+    const float unload_length = -ABS(parser.axisunitsval('U', E_AXIS, fc_settings[active_extruder].unload_length));
+    // Slow load filament
+    constexpr float slow_load_length = FILAMENT_CHANGE_SLOW_LOAD_LENGTH;
+    // Fast load filament
+    const float fast_load_length = ABS(parser.axisunitsval('L', E_AXIS, fc_settings[active_extruder].load_length));
+  #endif
+
+  const int beep_count = parser.intval('B', -1
+    #ifdef FILAMENT_CHANGE_ALERT_BEEPS
+      + 1 + FILAMENT_CHANGE_ALERT_BEEPS
+    #endif
+  );
+
+  if (pause_print(retract, park_point, true, unload_length DXC_PASS)) {
+    #if ENABLED(MMU2_MENUS)
+      mmu2_M600();
+      resume_print(slow_load_length, fast_load_length, 0, beep_count DXC_PASS);
+    #else
+      wait_for_confirmation(true, beep_count DXC_PASS);
+      resume_print(slow_load_length, fast_load_length, ADVANCED_PAUSE_PURGE_LENGTH,
+                   beep_count, (parser.seenval('R') ? parser.value_celsius() : 0) DXC_PASS);
+    #endif
+  }
+
+  #if HAS_MULTI_EXTRUDER
+    // Restore toolhead if it was changed
+    if (active_extruder_before_filament_change != active_extruder)
+      tool_change(active_extruder_before_filament_change, false);
+  #endif
+
+  TERN_(MIXING_EXTRUDER, mixer.T(old_mixing_tool)); // Restore original mixing tool
+}
+
+#endif // ADVANCED_PAUSE_FEATURE

Marlin/src/gcode/feature/pause/M603.cpp

@@ -0,0 +1,65 @@
+/**
+ * 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(ADVANCED_PAUSE_FEATURE)
+
+#include "../../gcode.h"
+#include "../../../feature/pause.h"
+#include "../../../module/motion.h"
+#include "../../../module/printcounter.h"
+
+#if HAS_MULTI_EXTRUDER
+  #include "../../../module/tool_change.h"
+#endif
+
+/**
+ * M603: Configure filament change
+ *
+ *  T[toolhead] - Select extruder to configure, active extruder if not specified
+ *  U[distance] - Retract distance for removal, for the specified extruder
+ *  L[distance] - Extrude distance for insertion, for the specified extruder
+ */
+void GcodeSuite::M603() {
+
+  const int8_t target_extruder = get_target_extruder_from_command();
+  if (target_extruder < 0) return;
+
+  // Unload length
+  if (parser.seen('U')) {
+    fc_settings[target_extruder].unload_length = ABS(parser.value_axis_units(E_AXIS));
+    #if ENABLED(PREVENT_LENGTHY_EXTRUDE)
+      NOMORE(fc_settings[target_extruder].unload_length, EXTRUDE_MAXLENGTH);
+    #endif
+  }
+
+  // Load length
+  if (parser.seen('L')) {
+    fc_settings[target_extruder].load_length = ABS(parser.value_axis_units(E_AXIS));
+    #if ENABLED(PREVENT_LENGTHY_EXTRUDE)
+      NOMORE(fc_settings[target_extruder].load_length, EXTRUDE_MAXLENGTH);
+    #endif
+  }
+}
+
+#endif // ADVANCED_PAUSE_FEATURE

Marlin/src/gcode/feature/pause/M701_M702.cpp

@@ -0,0 +1,242 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfigPre.h"
+
+#if ENABLED(FILAMENT_LOAD_UNLOAD_GCODES)
+
+#include "../../gcode.h"
+#include "../../../MarlinCore.h"
+#include "../../../module/motion.h"
+#include "../../../module/temperature.h"
+#include "../../../feature/pause.h"
+#include "../../../lcd/marlinui.h"
+
+#if HAS_MULTI_EXTRUDER
+  #include "../../../module/tool_change.h"
+#endif
+
+#if HAS_PRUSA_MMU2
+  #include "../../../feature/mmu/mmu2.h"
+#endif
+
+#if ENABLED(MIXING_EXTRUDER)
+  #include "../../../feature/mixing.h"
+#endif
+
+/**
+ * M701: Load filament
+ *
+ *  T<extruder> - Extruder number. Required for mixing extruder.
+ *                For non-mixing, current extruder if omitted.
+ *  Z<distance> - Move the Z axis by this distance
+ *  L<distance> - Extrude distance for insertion (positive value) (manual reload)
+ *
+ *  Default values are used for omitted arguments.
+ */
+void GcodeSuite::M701() {
+  xyz_pos_t park_point = NOZZLE_PARK_POINT;
+
+  // Don't raise Z if the machine isn't homed
+  if (TERN0(NO_MOTION_BEFORE_HOMING, axes_should_home())) park_point.z = 0;
+
+  #if ENABLED(MIXING_EXTRUDER)
+    const int8_t target_e_stepper = get_target_e_stepper_from_command();
+    if (target_e_stepper < 0) return;
+
+    const uint8_t old_mixing_tool = mixer.get_current_vtool();
+    mixer.T(MIXER_DIRECT_SET_TOOL);
+
+    MIXER_STEPPER_LOOP(i) mixer.set_collector(i, (i == (uint8_t)target_e_stepper) ? 1.0 : 0.0);
+    mixer.normalize();
+
+    const int8_t target_extruder = active_extruder;
+  #else
+    const int8_t target_extruder = get_target_extruder_from_command();
+    if (target_extruder < 0) return;
+  #endif
+
+  // Z axis lift
+  if (parser.seenval('Z')) park_point.z = parser.linearval('Z');
+
+  // Show initial "wait for load" message
+  ui.pause_show_message(PAUSE_MESSAGE_LOAD, PAUSE_MODE_LOAD_FILAMENT, target_extruder);
+
+  #if HAS_MULTI_EXTRUDER && (HAS_PRUSA_MMU1 || !HAS_MMU)
+    // Change toolhead if specified
+    uint8_t active_extruder_before_filament_change = active_extruder;
+    if (active_extruder != target_extruder)
+      tool_change(target_extruder, false);
+  #endif
+
+  auto move_z_by = [](const_float_t zdist) {
+    if (zdist) {
+      destination = current_position;
+      destination.z += zdist;
+      prepare_internal_move_to_destination(NOZZLE_PARK_Z_FEEDRATE);
+    }
+  };
+
+  // Raise the Z axis (with max limit)
+  const float park_raise = _MIN(park_point.z, (Z_MAX_POS) - current_position.z);
+  move_z_by(park_raise);
+
+  // Load filament
+  #if HAS_PRUSA_MMU2
+    mmu2.load_filament_to_nozzle(target_extruder);
+  #else
+    constexpr float     purge_length = ADVANCED_PAUSE_PURGE_LENGTH,
+                    slow_load_length = FILAMENT_CHANGE_SLOW_LOAD_LENGTH;
+        const float fast_load_length = ABS(parser.seen('L') ? parser.value_axis_units(E_AXIS)
+                                                            : fc_settings[active_extruder].load_length);
+    load_filament(
+      slow_load_length, fast_load_length, purge_length,
+      FILAMENT_CHANGE_ALERT_BEEPS,
+      true,                                           // show_lcd
+      thermalManager.still_heating(target_extruder),  // pause_for_user
+      PAUSE_MODE_LOAD_FILAMENT                        // pause_mode
+      #if ENABLED(DUAL_X_CARRIAGE)
+        , target_extruder                             // Dual X target
+      #endif
+    );
+  #endif
+
+  // Restore Z axis
+  move_z_by(-park_raise);
+
+  #if HAS_MULTI_EXTRUDER && (HAS_PRUSA_MMU1 || !HAS_MMU)
+    // Restore toolhead if it was changed
+    if (active_extruder_before_filament_change != active_extruder)
+      tool_change(active_extruder_before_filament_change, false);
+  #endif
+
+  TERN_(MIXING_EXTRUDER, mixer.T(old_mixing_tool)); // Restore original mixing tool
+
+  // Show status screen
+  ui.pause_show_message(PAUSE_MESSAGE_STATUS);
+}
+
+/**
+ * M702: Unload filament
+ *
+ *  T<extruder> - Extruder number. Required for mixing extruder.
+ *                For non-mixing, if omitted, current extruder
+ *                (or ALL extruders with FILAMENT_UNLOAD_ALL_EXTRUDERS).
+ *  Z<distance> - Move the Z axis by this distance
+ *  U<distance> - Retract distance for removal (manual reload)
+ *
+ *  Default values are used for omitted arguments.
+ */
+void GcodeSuite::M702() {
+  xyz_pos_t park_point = NOZZLE_PARK_POINT;
+
+  // Don't raise Z if the machine isn't homed
+  if (TERN0(NO_MOTION_BEFORE_HOMING, axes_should_home())) park_point.z = 0;
+
+  #if ENABLED(MIXING_EXTRUDER)
+    const uint8_t old_mixing_tool = mixer.get_current_vtool();
+
+    #if ENABLED(FILAMENT_UNLOAD_ALL_EXTRUDERS)
+      float mix_multiplier = 1.0;
+      const bool seenT = parser.seenval('T');
+      if (!seenT) {
+        mixer.T(MIXER_AUTORETRACT_TOOL);
+        mix_multiplier = MIXING_STEPPERS;
+      }
+    #else
+      constexpr bool seenT = true;
+    #endif
+
+    if (seenT) {
+      const int8_t target_e_stepper = get_target_e_stepper_from_command();
+      if (target_e_stepper < 0) return;
+      mixer.T(MIXER_DIRECT_SET_TOOL);
+      MIXER_STEPPER_LOOP(i) mixer.set_collector(i, (i == (uint8_t)target_e_stepper) ? 1.0 : 0.0);
+      mixer.normalize();
+    }
+
+    const int8_t target_extruder = active_extruder;
+  #else
+    const int8_t target_extruder = get_target_extruder_from_command();
+    if (target_extruder < 0) return;
+  #endif
+
+  // Z axis lift
+  if (parser.seenval('Z')) park_point.z = parser.linearval('Z');
+
+  // Show initial "wait for unload" message
+  ui.pause_show_message(PAUSE_MESSAGE_UNLOAD, PAUSE_MODE_UNLOAD_FILAMENT, target_extruder);
+
+  #if HAS_MULTI_EXTRUDER && (HAS_PRUSA_MMU1 || !HAS_MMU)
+    // Change toolhead if specified
+    uint8_t active_extruder_before_filament_change = active_extruder;
+    if (active_extruder != target_extruder)
+      tool_change(target_extruder, false);
+  #endif
+
+  // Lift Z axis
+  if (park_point.z > 0)
+    do_blocking_move_to_z(_MIN(current_position.z + park_point.z, Z_MAX_POS), feedRate_t(NOZZLE_PARK_Z_FEEDRATE));
+
+  // Unload filament
+  #if HAS_PRUSA_MMU2
+    mmu2.unload();
+  #else
+    #if BOTH(HAS_MULTI_EXTRUDER, FILAMENT_UNLOAD_ALL_EXTRUDERS)
+      if (!parser.seenval('T')) {
+        HOTEND_LOOP() {
+          if (e != active_extruder) tool_change(e, false);
+          unload_filament(-fc_settings[e].unload_length, true, PAUSE_MODE_UNLOAD_FILAMENT);
+        }
+      }
+      else
+    #endif
+    {
+      // Unload length
+      const float unload_length = -ABS(parser.seen('U') ? parser.value_axis_units(E_AXIS)
+                                                        : fc_settings[target_extruder].unload_length);
+
+      unload_filament(unload_length, true, PAUSE_MODE_UNLOAD_FILAMENT
+        #if ALL(FILAMENT_UNLOAD_ALL_EXTRUDERS, MIXING_EXTRUDER)
+          , mix_multiplier
+        #endif
+      );
+    }
+  #endif
+
+  // Restore Z axis
+  if (park_point.z > 0)
+    do_blocking_move_to_z(_MAX(current_position.z - park_point.z, 0), feedRate_t(NOZZLE_PARK_Z_FEEDRATE));
+
+  #if HAS_MULTI_EXTRUDER && (HAS_PRUSA_MMU1 || !HAS_MMU)
+    // Restore toolhead if it was changed
+    if (active_extruder_before_filament_change != active_extruder)
+      tool_change(active_extruder_before_filament_change, false);
+  #endif
+
+  TERN_(MIXING_EXTRUDER, mixer.T(old_mixing_tool)); // Restore original mixing tool
+
+  // Show status screen
+  ui.pause_show_message(PAUSE_MESSAGE_STATUS);
+}
+
+#endif // ADVANCED_PAUSE_FEATURE

Marlin/src/gcode/feature/power_monitor/M430.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/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if HAS_POWER_MONITOR
+
+#include "../../../feature/power_monitor.h"
+#include "../../../MarlinCore.h"
+#include "../../gcode.h"
+
+/**
+ * M430: Enable/disable current LCD display
+ *       With no parameters report the system current draw (in Amps)
+ *
+ *  I[bool] - Set Display of current on the LCD
+ *  V[bool] - Set Display of voltage on the LCD
+ *  W[bool] - Set Display of power on the LCD
+ */
+void GcodeSuite::M430() {
+  bool do_report = true;
+  #if HAS_WIRED_LCD
+    #if ENABLED(POWER_MONITOR_CURRENT)
+      if (parser.seen('I')) { power_monitor.set_current_display(parser.value_bool()); do_report = false; }
+    #endif
+    #if ENABLED(POWER_MONITOR_VOLTAGE)
+      if (parser.seen('V')) { power_monitor.set_voltage_display(parser.value_bool()); do_report = false; }
+    #endif
+    #if HAS_POWER_MONITOR_WATTS
+      if (parser.seen('W')) { power_monitor.set_power_display(parser.value_bool()); do_report = false; }
+    #endif
+  #endif
+  if (do_report) {
+    SERIAL_ECHOLNPAIR(
+      #if ENABLED(POWER_MONITOR_CURRENT)
+        "Current: ", power_monitor.getAmps(), "A"
+        #if ENABLED(POWER_MONITOR_VOLTAGE)
+          "  "
+        #endif
+      #endif
+      #if ENABLED(POWER_MONITOR_VOLTAGE)
+        "Voltage: ", power_monitor.getVolts(), "V"
+      #endif
+      #if HAS_POWER_MONITOR_WATTS
+        "  Power: ", power_monitor.getPower(), "W"
+      #endif
+    );
+  }
+}
+
+#endif // HAS_POWER_MONITOR

Marlin/src/gcode/feature/powerloss/M1000.cpp

@@ -0,0 +1,89 @@
+/**
+ * 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(POWER_LOSS_RECOVERY)
+
+#include "../../gcode.h"
+#include "../../../feature/powerloss.h"
+#include "../../../module/motion.h"
+#include "../../../lcd/marlinui.h"
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../../../lcd/extui/ui_api.h"
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_POWER_LOSS_RECOVERY)
+#include "../../../core/debug_out.h"
+
+void menu_job_recovery();
+
+inline void plr_error(PGM_P const prefix) {
+  #if ENABLED(DEBUG_POWER_LOSS_RECOVERY)
+    DEBUG_ECHO_START();
+    DEBUG_ECHOPGM_P(prefix);
+    DEBUG_ECHOLNPGM(" Job Recovery Data");
+  #else
+    UNUSED(prefix);
+  #endif
+}
+
+#if HAS_LCD_MENU
+  void lcd_power_loss_recovery_cancel();
+#endif
+
+/**
+ * M1000: Resume from power-loss (undocumented)
+ *   - With 'S' go to the Resume/Cancel menu
+ *   - With no parameters, run recovery commands
+ */
+void GcodeSuite::M1000() {
+
+  if (recovery.valid()) {
+    if (parser.seen_test('S')) {
+      #if HAS_LCD_MENU
+        ui.goto_screen(menu_job_recovery);
+      #elif ENABLED(DWIN_CREALITY_LCD)
+        recovery.dwin_flag = true;
+      #elif ENABLED(EXTENSIBLE_UI)
+        ExtUI::onPowerLossResume();
+      #else
+        SERIAL_ECHO_MSG("Resume requires LCD.");
+      #endif
+    }
+    else if (parser.seen_test('C')) {
+      #if HAS_LCD_MENU
+        lcd_power_loss_recovery_cancel();
+      #else
+        recovery.cancel();
+      #endif
+      TERN_(EXTENSIBLE_UI, ExtUI::onPrintTimerStopped());
+    }
+    else
+      recovery.resume();
+  }
+  else
+    plr_error(recovery.info.valid_head ? PSTR("No") : PSTR("Invalid"));
+
+}
+
+#endif // POWER_LOSS_RECOVERY

Marlin/src/gcode/feature/powerloss/M413.cpp

@@ -0,0 +1,62 @@
+/**
+ * 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(POWER_LOSS_RECOVERY)
+
+#include "../../gcode.h"
+#include "../../../feature/powerloss.h"
+#include "../../../module/motion.h"
+#include "../../../lcd/marlinui.h"
+
+/**
+ * M413: Enable / Disable power-loss recovery
+ *
+ * Parameters
+ *   S[bool] - Flag to enable / disable.
+ *             If omitted, report current state.
+ */
+void GcodeSuite::M413() {
+
+  if (parser.seen('S'))
+    recovery.enable(parser.value_bool());
+  else {
+    SERIAL_ECHO_START();
+    SERIAL_ECHOPGM("Power-loss recovery ");
+    serialprintln_onoff(recovery.enabled);
+  }
+
+  #if ENABLED(DEBUG_POWER_LOSS_RECOVERY)
+    if (parser.seen("RL")) recovery.load();
+    if (parser.seen_test('W')) recovery.save(true);
+    if (parser.seen_test('P')) recovery.purge();
+    if (parser.seen_test('D')) recovery.debug(PSTR("M413"));
+    #if PIN_EXISTS(POWER_LOSS)
+      if (parser.seen_test('O')) recovery._outage();
+    #endif
+    if (parser.seen_test('E')) SERIAL_ECHOPGM_P(recovery.exists() ? PSTR("PLR Exists\n") : PSTR("No PLR\n"));
+    if (parser.seen_test('V')) SERIAL_ECHOPGM_P(recovery.valid() ? PSTR("Valid\n") : PSTR("Invalid\n"));
+  #endif
+}
+
+#endif // POWER_LOSS_RECOVERY

Marlin/src/gcode/feature/prusa_MMU2/M403.cpp

@@ -0,0 +1,49 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfigPre.h"
+
+#if HAS_PRUSA_MMU2
+
+#include "../../gcode.h"
+#include "../../../feature/mmu/mmu2.h"
+
+/**
+ * M403: Set filament type for MMU2
+ *
+ * Valid filament type values:
+ *
+ *  0   Default
+ *  1   Flexible
+ *  2   PVA
+ */
+void GcodeSuite::M403() {
+  int8_t index = parser.intval('E', -1),
+         type = parser.intval('F', -1);
+
+  if (WITHIN(index, 0, EXTRUDERS - 1) && WITHIN(type, 0, 2))
+    mmu2.set_filament_type(index, type);
+  else
+    SERIAL_ECHO_MSG("M403 - bad arguments.");
+}
+
+#endif // HAS_PRUSA_MMU2

Marlin/src/gcode/feature/runout/M412.cpp

@@ -0,0 +1,69 @@
+/**
+ * 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_FILAMENT_SENSOR
+
+#include "../../gcode.h"
+#include "../../../feature/runout.h"
+
+/**
+ * M412: Enable / Disable filament runout detection
+ *
+ * Parameters
+ *  R         : Reset the runout sensor
+ *  S<bool>   : Reset and enable/disable the runout sensor
+ *  H<bool>   : Enable/disable host handling of filament runout
+ *  D<linear> : Extra distance to continue after runout is triggered
+ */
+void GcodeSuite::M412() {
+  if (parser.seen("RS"
+    TERN_(HAS_FILAMENT_RUNOUT_DISTANCE, "D")
+    TERN_(HOST_ACTION_COMMANDS, "H")
+  )) {
+    #if ENABLED(HOST_ACTION_COMMANDS)
+      if (parser.seen('H')) runout.host_handling = parser.value_bool();
+    #endif
+    const bool seenR = parser.seen_test('R'), seenS = parser.seen('S');
+    if (seenR || seenS) runout.reset();
+    if (seenS) runout.enabled = parser.value_bool();
+    #if HAS_FILAMENT_RUNOUT_DISTANCE
+      if (parser.seen('D')) runout.set_runout_distance(parser.value_linear_units());
+    #endif
+  }
+  else {
+    SERIAL_ECHO_START();
+    SERIAL_ECHOPGM("Filament runout ");
+    serialprint_onoff(runout.enabled);
+    #if HAS_FILAMENT_RUNOUT_DISTANCE
+      SERIAL_ECHOPAIR(" ; Distance ", runout.runout_distance(), "mm");
+    #endif
+    #if ENABLED(HOST_ACTION_COMMANDS)
+      SERIAL_ECHOPGM(" ; Host handling ");
+      serialprint_onoff(runout.host_handling);
+    #endif
+    SERIAL_EOL();
+  }
+}
+
+#endif // HAS_FILAMENT_SENSOR

Marlin/src/gcode/feature/trinamic/M122.cpp

@@ -0,0 +1,64 @@
+/**
+ * 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_TRINAMIC_CONFIG
+
+#include "../../gcode.h"
+#include "../../../feature/tmc_util.h"
+#include "../../../module/stepper/indirection.h"
+
+/**
+ * M122: Debug TMC drivers
+ */
+void GcodeSuite::M122() {
+  xyze_bool_t print_axis = ARRAY_N_1(LOGICAL_AXES, false);
+
+  bool print_all = true;
+  LOOP_LOGICAL_AXES(i) if (parser.seen_test(axis_codes[i])) { print_axis[i] = true; print_all = false; }
+
+  if (print_all) LOOP_LOGICAL_AXES(i) print_axis[i] = true;
+
+  if (parser.boolval('I')) restore_stepper_drivers();
+
+  #if ENABLED(TMC_DEBUG)
+    #if ENABLED(MONITOR_DRIVER_STATUS)
+      const bool sflag = parser.seen_test('S'), sval = sflag && parser.value_bool();
+      if (sflag && !sval)
+        tmc_set_report_interval(0);
+      else if (parser.seenval('P'))
+        tmc_set_report_interval(_MAX(250, parser.value_ushort()));
+      else if (sval)
+        tmc_set_report_interval(MONITOR_DRIVER_STATUS_INTERVAL_MS);
+    #endif
+
+    if (parser.seen_test('V'))
+      tmc_get_registers(LOGICAL_AXIS_ELEM(print_axis));
+    else
+      tmc_report_all(LOGICAL_AXIS_ELEM(print_axis));
+  #endif
+
+  test_tmc_connection(LOGICAL_AXIS_ELEM(print_axis));
+}
+
+#endif // HAS_TRINAMIC_CONFIG

Marlin/src/gcode/feature/trinamic/M569.cpp

@@ -0,0 +1,141 @@
+/**
+ * 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_STEALTHCHOP
+
+#include "../../gcode.h"
+#include "../../../feature/tmc_util.h"
+#include "../../../module/stepper/indirection.h"
+
+template<typename TMC>
+void tmc_say_stealth_status(TMC &st) {
+  st.printLabel();
+  SERIAL_ECHOPGM(" driver mode:\t");
+  SERIAL_ECHOLNPGM_P(st.get_stealthChop() ? PSTR("stealthChop") : PSTR("spreadCycle"));
+}
+template<typename TMC>
+void tmc_set_stealthChop(TMC &st, const bool enable) {
+  st.stored.stealthChop_enabled = enable;
+  st.refresh_stepping_mode();
+}
+
+static void set_stealth_status(const bool enable, const int8_t target_e_stepper) {
+  #define TMC_SET_STEALTH(Q) tmc_set_stealthChop(stepper##Q, enable)
+
+  #if    X_HAS_STEALTHCHOP  || Y_HAS_STEALTHCHOP  || Z_HAS_STEALTHCHOP \
+      || I_HAS_STEALTHCHOP  || J_HAS_STEALTHCHOP  || K_HAS_STEALTHCHOP \
+      || X2_HAS_STEALTHCHOP || Y2_HAS_STEALTHCHOP || Z2_HAS_STEALTHCHOP || Z3_HAS_STEALTHCHOP || Z4_HAS_STEALTHCHOP
+    const uint8_t index = parser.byteval('I');
+  #endif
+
+  LOOP_LOGICAL_AXES(i) if (parser.seen(axis_codes[i])) {
+    switch (i) {
+      case X_AXIS:
+        TERN_(X_HAS_STEALTHCHOP,  if (index == 0) TMC_SET_STEALTH(X));
+        TERN_(X2_HAS_STEALTHCHOP, if (index == 1) TMC_SET_STEALTH(X2));
+        break;
+
+      #if HAS_Y_AXIS
+        case Y_AXIS:
+          TERN_(Y_HAS_STEALTHCHOP,  if (index == 0) TMC_SET_STEALTH(Y));
+          TERN_(Y2_HAS_STEALTHCHOP, if (index == 1) TMC_SET_STEALTH(Y2));
+          break;
+      #endif
+
+      #if HAS_Z_AXIS
+        case Z_AXIS:
+          TERN_(Z_HAS_STEALTHCHOP,  if (index == 0) TMC_SET_STEALTH(Z));
+          TERN_(Z2_HAS_STEALTHCHOP, if (index == 1) TMC_SET_STEALTH(Z2));
+          TERN_(Z3_HAS_STEALTHCHOP, if (index == 2) TMC_SET_STEALTH(Z3));
+          TERN_(Z4_HAS_STEALTHCHOP, if (index == 3) TMC_SET_STEALTH(Z4));
+          break;
+      #endif
+
+      #if I_HAS_STEALTHCHOP
+        case I_AXIS: TMC_SET_STEALTH(I); break;
+      #endif
+      #if J_HAS_STEALTHCHOP
+        case J_AXIS: TMC_SET_STEALTH(J); break;
+      #endif
+      #if K_HAS_STEALTHCHOP
+        case K_AXIS: TMC_SET_STEALTH(K); break;
+      #endif
+
+      #if E_STEPPERS
+        case E_AXIS: {
+          if (target_e_stepper < 0) return;
+          switch (target_e_stepper) {
+            TERN_(E0_HAS_STEALTHCHOP, case 0: TMC_SET_STEALTH(E0); break;)
+            TERN_(E1_HAS_STEALTHCHOP, case 1: TMC_SET_STEALTH(E1); break;)
+            TERN_(E2_HAS_STEALTHCHOP, case 2: TMC_SET_STEALTH(E2); break;)
+            TERN_(E3_HAS_STEALTHCHOP, case 3: TMC_SET_STEALTH(E3); break;)
+            TERN_(E4_HAS_STEALTHCHOP, case 4: TMC_SET_STEALTH(E4); break;)
+            TERN_(E5_HAS_STEALTHCHOP, case 5: TMC_SET_STEALTH(E5); break;)
+            TERN_(E6_HAS_STEALTHCHOP, case 6: TMC_SET_STEALTH(E6); break;)
+            TERN_(E7_HAS_STEALTHCHOP, case 7: TMC_SET_STEALTH(E7); break;)
+          }
+        } break;
+      #endif
+    }
+  }
+}
+
+static void say_stealth_status() {
+  #define TMC_SAY_STEALTH_STATUS(Q) tmc_say_stealth_status(stepper##Q)
+  OPTCODE( X_HAS_STEALTHCHOP, TMC_SAY_STEALTH_STATUS(X))
+  OPTCODE(X2_HAS_STEALTHCHOP, TMC_SAY_STEALTH_STATUS(X2))
+  OPTCODE( Y_HAS_STEALTHCHOP, TMC_SAY_STEALTH_STATUS(Y))
+  OPTCODE(Y2_HAS_STEALTHCHOP, TMC_SAY_STEALTH_STATUS(Y2))
+  OPTCODE( Z_HAS_STEALTHCHOP, TMC_SAY_STEALTH_STATUS(Z))
+  OPTCODE(Z2_HAS_STEALTHCHOP, TMC_SAY_STEALTH_STATUS(Z2))
+  OPTCODE(Z3_HAS_STEALTHCHOP, TMC_SAY_STEALTH_STATUS(Z3))
+  OPTCODE(Z4_HAS_STEALTHCHOP, TMC_SAY_STEALTH_STATUS(Z4))
+  OPTCODE( I_HAS_STEALTHCHOP, TMC_SAY_STEALTH_STATUS(I))
+  OPTCODE( J_HAS_STEALTHCHOP, TMC_SAY_STEALTH_STATUS(J))
+  OPTCODE( K_HAS_STEALTHCHOP, TMC_SAY_STEALTH_STATUS(K))
+  OPTCODE(E0_HAS_STEALTHCHOP, TMC_SAY_STEALTH_STATUS(E0))
+  OPTCODE(E1_HAS_STEALTHCHOP, TMC_SAY_STEALTH_STATUS(E1))
+  OPTCODE(E2_HAS_STEALTHCHOP, TMC_SAY_STEALTH_STATUS(E2))
+  OPTCODE(E3_HAS_STEALTHCHOP, TMC_SAY_STEALTH_STATUS(E3))
+  OPTCODE(E4_HAS_STEALTHCHOP, TMC_SAY_STEALTH_STATUS(E4))
+  OPTCODE(E5_HAS_STEALTHCHOP, TMC_SAY_STEALTH_STATUS(E5))
+  OPTCODE(E6_HAS_STEALTHCHOP, TMC_SAY_STEALTH_STATUS(E6))
+  OPTCODE(E7_HAS_STEALTHCHOP, TMC_SAY_STEALTH_STATUS(E7))
+}
+
+/**
+ * M569: Enable stealthChop on an axis
+ *
+ *   S[1|0] to enable or disable
+ *   XYZE to target an axis
+ *   No arguments reports the stealthChop status of all capable drivers.
+ */
+void GcodeSuite::M569() {
+  if (parser.seen('S'))
+    set_stealth_status(parser.value_bool(), get_target_e_stepper_from_command());
+  else
+    say_stealth_status();
+}
+
+#endif // HAS_STEALTHCHOP

Marlin/src/gcode/feature/trinamic/M906.cpp

@@ -0,0 +1,201 @@
+/**
+ * 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_TRINAMIC_CONFIG
+
+#include "../../gcode.h"
+#include "../../../feature/tmc_util.h"
+#include "../../../module/stepper/indirection.h"
+
+/**
+ * M906: Set motor current in milliamps.
+ *
+ * Parameters:
+ *   X[current]  - Set mA current for X driver(s)
+ *   Y[current]  - Set mA current for Y driver(s)
+ *   Z[current]  - Set mA current for Z driver(s)
+ *   E[current]  - Set mA current for E driver(s)
+ *
+ *   I[index]    - Axis sub-index (Omit or 0 for X, Y, Z; 1 for X2, Y2, Z2; 2 for Z3; 3 for Z4.)
+ *   T[index]    - Extruder index (Zero-based. Omit for E0 only.)
+ *
+ * With no parameters report driver currents.
+ */
+void GcodeSuite::M906() {
+  #define TMC_SAY_CURRENT(Q) tmc_print_current(stepper##Q)
+  #define TMC_SET_CURRENT(Q) stepper##Q.rms_current(value)
+
+  bool report = true;
+
+  #if AXIS_IS_TMC(X) || AXIS_IS_TMC(X2) || AXIS_IS_TMC(Y) || AXIS_IS_TMC(Y2) || AXIS_IS_TMC(Z) || AXIS_IS_TMC(Z2) || AXIS_IS_TMC(Z3) || AXIS_IS_TMC(Z4) || AXIS_IS_TMC(I) || AXIS_IS_TMC(J) || AXIS_IS_TMC(K)
+    const uint8_t index = parser.byteval('I');
+  #endif
+
+  LOOP_LOGICAL_AXES(i) if (uint16_t value = parser.intval(axis_codes[i])) {
+    report = false;
+    switch (i) {
+      case X_AXIS:
+        #if AXIS_IS_TMC(X)
+          if (index == 0) TMC_SET_CURRENT(X);
+        #endif
+        #if AXIS_IS_TMC(X2)
+          if (index == 1) TMC_SET_CURRENT(X2);
+        #endif
+        break;
+
+      #if HAS_Y_AXIS
+        case Y_AXIS:
+          #if AXIS_IS_TMC(Y)
+            if (index == 0) TMC_SET_CURRENT(Y);
+          #endif
+          #if AXIS_IS_TMC(Y2)
+            if (index == 1) TMC_SET_CURRENT(Y2);
+          #endif
+          break;
+      #endif
+
+      #if HAS_Z_AXIS
+        case Z_AXIS:
+          #if AXIS_IS_TMC(Z)
+            if (index == 0) TMC_SET_CURRENT(Z);
+          #endif
+          #if AXIS_IS_TMC(Z2)
+            if (index == 1) TMC_SET_CURRENT(Z2);
+          #endif
+          #if AXIS_IS_TMC(Z3)
+            if (index == 2) TMC_SET_CURRENT(Z3);
+          #endif
+          #if AXIS_IS_TMC(Z4)
+            if (index == 3) TMC_SET_CURRENT(Z4);
+          #endif
+          break;
+      #endif
+
+      #if AXIS_IS_TMC(I)
+        case I_AXIS: TMC_SET_CURRENT(I); break;
+      #endif
+      #if AXIS_IS_TMC(J)
+        case J_AXIS: TMC_SET_CURRENT(J); break;
+      #endif
+      #if AXIS_IS_TMC(K)
+        case K_AXIS: TMC_SET_CURRENT(K); break;
+      #endif
+
+      #if E_STEPPERS
+        case E_AXIS: {
+          const int8_t target_e_stepper = get_target_e_stepper_from_command();
+          if (target_e_stepper < 0) return;
+          switch (target_e_stepper) {
+            #if AXIS_IS_TMC(E0)
+              case 0: TMC_SET_CURRENT(E0); break;
+            #endif
+            #if AXIS_IS_TMC(E1)
+              case 1: TMC_SET_CURRENT(E1); break;
+            #endif
+            #if AXIS_IS_TMC(E2)
+              case 2: TMC_SET_CURRENT(E2); break;
+            #endif
+            #if AXIS_IS_TMC(E3)
+              case 3: TMC_SET_CURRENT(E3); break;
+            #endif
+            #if AXIS_IS_TMC(E4)
+              case 4: TMC_SET_CURRENT(E4); break;
+            #endif
+            #if AXIS_IS_TMC(E5)
+              case 5: TMC_SET_CURRENT(E5); break;
+            #endif
+            #if AXIS_IS_TMC(E6)
+              case 6: TMC_SET_CURRENT(E6); break;
+            #endif
+            #if AXIS_IS_TMC(E7)
+              case 7: TMC_SET_CURRENT(E7); break;
+            #endif
+          }
+        } break;
+      #endif
+    }
+  }
+
+  if (report) {
+    #if AXIS_IS_TMC(X)
+      TMC_SAY_CURRENT(X);
+    #endif
+    #if AXIS_IS_TMC(X2)
+      TMC_SAY_CURRENT(X2);
+    #endif
+    #if AXIS_IS_TMC(Y)
+      TMC_SAY_CURRENT(Y);
+    #endif
+    #if AXIS_IS_TMC(Y2)
+      TMC_SAY_CURRENT(Y2);
+    #endif
+    #if AXIS_IS_TMC(Z)
+      TMC_SAY_CURRENT(Z);
+    #endif
+    #if AXIS_IS_TMC(Z2)
+      TMC_SAY_CURRENT(Z2);
+    #endif
+    #if AXIS_IS_TMC(Z3)
+      TMC_SAY_CURRENT(Z3);
+    #endif
+    #if AXIS_IS_TMC(Z4)
+      TMC_SAY_CURRENT(Z4);
+    #endif
+    #if AXIS_IS_TMC(I)
+      TMC_SAY_CURRENT(I);
+    #endif
+    #if AXIS_IS_TMC(J)
+      TMC_SAY_CURRENT(J);
+    #endif
+    #if AXIS_IS_TMC(K)
+      TMC_SAY_CURRENT(K);
+    #endif
+    #if AXIS_IS_TMC(E0)
+      TMC_SAY_CURRENT(E0);
+    #endif
+    #if AXIS_IS_TMC(E1)
+      TMC_SAY_CURRENT(E1);
+    #endif
+    #if AXIS_IS_TMC(E2)
+      TMC_SAY_CURRENT(E2);
+    #endif
+    #if AXIS_IS_TMC(E3)
+      TMC_SAY_CURRENT(E3);
+    #endif
+    #if AXIS_IS_TMC(E4)
+      TMC_SAY_CURRENT(E4);
+    #endif
+    #if AXIS_IS_TMC(E5)
+      TMC_SAY_CURRENT(E5);
+    #endif
+    #if AXIS_IS_TMC(E6)
+      TMC_SAY_CURRENT(E6);
+    #endif
+    #if AXIS_IS_TMC(E7)
+      TMC_SAY_CURRENT(E7);
+    #endif
+  }
+}
+
+#endif // HAS_TRINAMIC_CONFIG

Marlin/src/gcode/feature/trinamic/M911-M914.cpp

@@ -0,0 +1,417 @@
+/**
+ * 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_TRINAMIC_CONFIG
+
+#include "../../gcode.h"
+#include "../../../feature/tmc_util.h"
+#include "../../../module/stepper/indirection.h"
+#include "../../../module/planner.h"
+#include "../../queue.h"
+
+#if ENABLED(MONITOR_DRIVER_STATUS)
+
+  #define M91x_USE(ST) (AXIS_DRIVER_TYPE(ST, TMC2130) || AXIS_DRIVER_TYPE(ST, TMC2160) || AXIS_DRIVER_TYPE(ST, TMC2208) || AXIS_DRIVER_TYPE(ST, TMC2209) || AXIS_DRIVER_TYPE(ST, TMC2660) || AXIS_DRIVER_TYPE(ST, TMC5130) || AXIS_DRIVER_TYPE(ST, TMC5160))
+  #define M91x_USE_E(N) (E_STEPPERS > N && M91x_USE(E##N))
+
+  #if M91x_USE(X) || M91x_USE(X2)
+    #define M91x_SOME_X 1
+  #endif
+  #if LINEAR_AXES >= 2 && (M91x_USE(Y) || M91x_USE(Y2))
+    #define M91x_SOME_Y 1
+  #endif
+  #if HAS_Z_AXIS && (M91x_USE(Z) || M91x_USE(Z2) || M91x_USE(Z3) || M91x_USE(Z4))
+    #define M91x_SOME_Z 1
+  #endif
+  #if LINEAR_AXES >= 4 && M91x_USE(I)
+    #define M91x_USE_I 1
+  #endif
+  #if LINEAR_AXES >= 5 && M91x_USE(J)
+    #define M91x_USE_J 1
+  #endif
+  #if LINEAR_AXES >= 6 && M91x_USE(K)
+    #define M91x_USE_K 1
+  #endif
+
+  #if M91x_USE_E(0) || M91x_USE_E(1) || M91x_USE_E(2) || M91x_USE_E(3) || M91x_USE_E(4) || M91x_USE_E(5) || M91x_USE_E(6) || M91x_USE_E(7)
+    #define M91x_SOME_E 1
+  #endif
+
+  #if !M91x_SOME_X && !M91x_SOME_Y && !M91x_SOME_Z && !M91x_USE_I && !M91x_USE_J && !M91x_USE_K && !M91x_SOME_E
+    #error "MONITOR_DRIVER_STATUS requires at least one TMC2130, 2160, 2208, 2209, 2660, 5130, or 5160."
+  #endif
+
+  /**
+   * M911: Report TMC stepper driver overtemperature pre-warn flag
+   *       This flag is held by the library, persisting until cleared by M912
+   */
+  void GcodeSuite::M911() {
+    #if M91x_USE(X)
+      tmc_report_otpw(stepperX);
+    #endif
+    #if M91x_USE(X2)
+      tmc_report_otpw(stepperX2);
+    #endif
+    #if M91x_USE(Y)
+      tmc_report_otpw(stepperY);
+    #endif
+    #if M91x_USE(Y2)
+      tmc_report_otpw(stepperY2);
+    #endif
+    #if M91x_USE(Z)
+      tmc_report_otpw(stepperZ);
+    #endif
+    #if M91x_USE(Z2)
+      tmc_report_otpw(stepperZ2);
+    #endif
+    #if M91x_USE(Z3)
+      tmc_report_otpw(stepperZ3);
+    #endif
+    #if M91x_USE(Z4)
+      tmc_report_otpw(stepperZ4);
+    #endif
+    TERN_(M91x_USE_I, tmc_report_otpw(stepperI));
+    TERN_(M91x_USE_J, tmc_report_otpw(stepperJ));
+    TERN_(M91x_USE_K, tmc_report_otpw(stepperK));
+    #if M91x_USE_E(0)
+      tmc_report_otpw(stepperE0);
+    #endif
+    #if M91x_USE_E(1)
+      tmc_report_otpw(stepperE1);
+    #endif
+    #if M91x_USE_E(2)
+      tmc_report_otpw(stepperE2);
+    #endif
+    #if M91x_USE_E(3)
+      tmc_report_otpw(stepperE3);
+    #endif
+    #if M91x_USE_E(4)
+      tmc_report_otpw(stepperE4);
+    #endif
+    #if M91x_USE_E(5)
+      tmc_report_otpw(stepperE5);
+    #endif
+    #if M91x_USE_E(6)
+      tmc_report_otpw(stepperE6);
+    #endif
+    #if M91x_USE_E(7)
+      tmc_report_otpw(stepperE7);
+    #endif
+  }
+
+  /**
+   * M912: Clear TMC stepper driver overtemperature pre-warn flag held by the library
+   *       Specify one or more axes with X, Y, Z, X1, Y1, Z1, X2, Y2, Z2, Z3, Z4 and E[index].
+   *       If no axes are given, clear all.
+   *
+   * Examples:
+   *       M912 X   ; clear X and X2
+   *       M912 X1  ; clear X1 only
+   *       M912 X2  ; clear X2 only
+   *       M912 X E ; clear X, X2, and all E
+   *       M912 E1  ; clear E1 only
+   */
+  void GcodeSuite::M912() {
+    const bool hasX = TERN0(M91x_SOME_X, parser.seen(axis_codes.x)),
+               hasY = TERN0(M91x_SOME_Y, parser.seen(axis_codes.y)),
+               hasZ = TERN0(M91x_SOME_Z, parser.seen(axis_codes.z)),
+               hasI = TERN0(M91x_USE_I,  parser.seen(axis_codes.i)),
+               hasJ = TERN0(M91x_USE_J,  parser.seen(axis_codes.j)),
+               hasK = TERN0(M91x_USE_K,  parser.seen(axis_codes.k)),
+               hasE = TERN0(M91x_SOME_E, parser.seen(axis_codes.e));
+
+    const bool hasNone = !hasE && !hasX && !hasY && !hasZ && !hasI && !hasJ && !hasK;
+
+    #if M91x_SOME_X
+      const int8_t xval = int8_t(parser.byteval(axis_codes.x, 0xFF));
+      #if M91x_USE(X)
+        if (hasNone || xval == 1 || (hasX && xval < 0)) tmc_clear_otpw(stepperX);
+      #endif
+      #if M91x_USE(X2)
+        if (hasNone || xval == 2 || (hasX && xval < 0)) tmc_clear_otpw(stepperX2);
+      #endif
+    #endif
+
+    #if M91x_SOME_Y
+      const int8_t yval = int8_t(parser.byteval(axis_codes.y, 0xFF));
+      #if M91x_USE(Y)
+        if (hasNone || yval == 1 || (hasY && yval < 0)) tmc_clear_otpw(stepperY);
+      #endif
+      #if M91x_USE(Y2)
+        if (hasNone || yval == 2 || (hasY && yval < 0)) tmc_clear_otpw(stepperY2);
+      #endif
+    #endif
+
+    #if M91x_SOME_Z
+      const int8_t zval = int8_t(parser.byteval(axis_codes.z, 0xFF));
+      #if M91x_USE(Z)
+        if (hasNone || zval == 1 || (hasZ && zval < 0)) tmc_clear_otpw(stepperZ);
+      #endif
+      #if M91x_USE(Z2)
+        if (hasNone || zval == 2 || (hasZ && zval < 0)) tmc_clear_otpw(stepperZ2);
+      #endif
+      #if M91x_USE(Z3)
+        if (hasNone || zval == 3 || (hasZ && zval < 0)) tmc_clear_otpw(stepperZ3);
+      #endif
+      #if M91x_USE(Z4)
+        if (hasNone || zval == 4 || (hasZ && zval < 0)) tmc_clear_otpw(stepperZ4);
+      #endif
+    #endif
+
+    #if M91x_USE_I
+      const int8_t ival = int8_t(parser.byteval(axis_codes.i, 0xFF));
+      if (hasNone || ival == 1 || (hasI && ival < 0)) tmc_clear_otpw(stepperI);
+    #endif
+    #if M91x_USE_J
+      const int8_t jval = int8_t(parser.byteval(axis_codes.j, 0xFF));
+      if (hasNone || jval == 1 || (hasJ && jval < 0)) tmc_clear_otpw(stepperJ);
+    #endif
+    #if M91x_USE_K
+      const int8_t kval = int8_t(parser.byteval(axis_codes.k, 0xFF));
+      if (hasNone || kval == 1 || (hasK && kval < 0)) tmc_clear_otpw(stepperK);
+    #endif
+
+    #if M91x_SOME_E
+      const int8_t eval = int8_t(parser.byteval(axis_codes.e, 0xFF));
+      #if M91x_USE_E(0)
+        if (hasNone || eval == 0 || (hasE && eval < 0)) tmc_clear_otpw(stepperE0);
+      #endif
+      #if M91x_USE_E(1)
+        if (hasNone || eval == 1 || (hasE && eval < 0)) tmc_clear_otpw(stepperE1);
+      #endif
+      #if M91x_USE_E(2)
+        if (hasNone || eval == 2 || (hasE && eval < 0)) tmc_clear_otpw(stepperE2);
+      #endif
+      #if M91x_USE_E(3)
+        if (hasNone || eval == 3 || (hasE && eval < 0)) tmc_clear_otpw(stepperE3);
+      #endif
+      #if M91x_USE_E(4)
+        if (hasNone || eval == 4 || (hasE && eval < 0)) tmc_clear_otpw(stepperE4);
+      #endif
+      #if M91x_USE_E(5)
+        if (hasNone || eval == 5 || (hasE && eval < 0)) tmc_clear_otpw(stepperE5);
+      #endif
+      #if M91x_USE_E(6)
+        if (hasNone || eval == 6 || (hasE && eval < 0)) tmc_clear_otpw(stepperE6);
+      #endif
+      #if M91x_USE_E(7)
+        if (hasNone || eval == 7 || (hasE && eval < 0)) tmc_clear_otpw(stepperE7);
+      #endif
+    #endif
+  }
+
+#endif // MONITOR_DRIVER_STATUS
+
+/**
+ * M913: Set HYBRID_THRESHOLD speed.
+ */
+#if ENABLED(HYBRID_THRESHOLD)
+  void GcodeSuite::M913() {
+    #define TMC_SAY_PWMTHRS(A,Q) tmc_print_pwmthrs(stepper##Q)
+    #define TMC_SET_PWMTHRS(A,Q) stepper##Q.set_pwm_thrs(value)
+    #define TMC_SAY_PWMTHRS_E(E) tmc_print_pwmthrs(stepperE##E)
+    #define TMC_SET_PWMTHRS_E(E) stepperE##E.set_pwm_thrs(value)
+
+    bool report = true;
+    #if AXIS_IS_TMC(X) || AXIS_IS_TMC(X2) || AXIS_IS_TMC(Y) || AXIS_IS_TMC(Y2) || AXIS_IS_TMC(Z) || AXIS_IS_TMC(Z2) || AXIS_IS_TMC(Z3) || AXIS_IS_TMC(Z4) || AXIS_IS_TMC(I) || AXIS_IS_TMC(J) || AXIS_IS_TMC(K)
+      const uint8_t index = parser.byteval('I');
+    #endif
+    LOOP_LOGICAL_AXES(i) if (int32_t value = parser.longval(axis_codes[i])) {
+      report = false;
+      switch (i) {
+        case X_AXIS:
+          TERN_(X_HAS_STEALTHCHOP,  if (index < 2) TMC_SET_PWMTHRS(X,X));
+          TERN_(X2_HAS_STEALTHCHOP, if (!(index & 1)) TMC_SET_PWMTHRS(X,X2));
+          break;
+        case Y_AXIS:
+          TERN_(Y_HAS_STEALTHCHOP,  if (index < 2) TMC_SET_PWMTHRS(Y,Y));
+          TERN_(Y2_HAS_STEALTHCHOP, if (!(index & 1)) TMC_SET_PWMTHRS(Y,Y2));
+          break;
+
+        #if I_HAS_STEALTHCHOP
+          case I_AXIS: TMC_SET_PWMTHRS(I,I); break;
+        #endif
+        #if J_HAS_STEALTHCHOP
+          case J_AXIS: TMC_SET_PWMTHRS(J,J); break;
+        #endif
+        #if K_HAS_STEALTHCHOP
+          case K_AXIS: TMC_SET_PWMTHRS(K,K); break;
+        #endif
+
+        case Z_AXIS:
+          TERN_(Z_HAS_STEALTHCHOP, if (index < 2) TMC_SET_PWMTHRS(Z,Z));
+          TERN_(Z2_HAS_STEALTHCHOP, if (index == 0 || index == 2) TMC_SET_PWMTHRS(Z,Z2));
+          TERN_(Z3_HAS_STEALTHCHOP, if (index == 0 || index == 3) TMC_SET_PWMTHRS(Z,Z3));
+          TERN_(Z4_HAS_STEALTHCHOP, if (index == 0 || index == 4) TMC_SET_PWMTHRS(Z,Z4));
+          break;
+        #if E_STEPPERS
+          case E_AXIS: {
+            const int8_t target_e_stepper = get_target_e_stepper_from_command();
+            if (target_e_stepper < 0) return;
+            switch (target_e_stepper) {
+              TERN_(E0_HAS_STEALTHCHOP, case 0: TMC_SET_PWMTHRS_E(0); break;)
+              TERN_(E1_HAS_STEALTHCHOP, case 1: TMC_SET_PWMTHRS_E(1); break;)
+              TERN_(E2_HAS_STEALTHCHOP, case 2: TMC_SET_PWMTHRS_E(2); break;)
+              TERN_(E3_HAS_STEALTHCHOP, case 3: TMC_SET_PWMTHRS_E(3); break;)
+              TERN_(E4_HAS_STEALTHCHOP, case 4: TMC_SET_PWMTHRS_E(4); break;)
+              TERN_(E5_HAS_STEALTHCHOP, case 5: TMC_SET_PWMTHRS_E(5); break;)
+              TERN_(E6_HAS_STEALTHCHOP, case 6: TMC_SET_PWMTHRS_E(6); break;)
+              TERN_(E7_HAS_STEALTHCHOP, case 7: TMC_SET_PWMTHRS_E(7); break;)
+            }
+          } break;
+        #endif // E_STEPPERS
+      }
+    }
+
+    if (report) {
+      TERN_( X_HAS_STEALTHCHOP, TMC_SAY_PWMTHRS(X,X));
+      TERN_(X2_HAS_STEALTHCHOP, TMC_SAY_PWMTHRS(X,X2));
+      TERN_( Y_HAS_STEALTHCHOP, TMC_SAY_PWMTHRS(Y,Y));
+      TERN_(Y2_HAS_STEALTHCHOP, TMC_SAY_PWMTHRS(Y,Y2));
+      TERN_( Z_HAS_STEALTHCHOP, TMC_SAY_PWMTHRS(Z,Z));
+      TERN_(Z2_HAS_STEALTHCHOP, TMC_SAY_PWMTHRS(Z,Z2));
+      TERN_(Z3_HAS_STEALTHCHOP, TMC_SAY_PWMTHRS(Z,Z3));
+      TERN_(Z4_HAS_STEALTHCHOP, TMC_SAY_PWMTHRS(Z,Z4));
+
+      TERN_( I_HAS_STEALTHCHOP, TMC_SAY_PWMTHRS(I,I));
+      TERN_( J_HAS_STEALTHCHOP, TMC_SAY_PWMTHRS(J,J));
+      TERN_( K_HAS_STEALTHCHOP, TMC_SAY_PWMTHRS(K,K));
+
+      TERN_(E0_HAS_STEALTHCHOP, TMC_SAY_PWMTHRS_E(0));
+      TERN_(E1_HAS_STEALTHCHOP, TMC_SAY_PWMTHRS_E(1));
+      TERN_(E2_HAS_STEALTHCHOP, TMC_SAY_PWMTHRS_E(2));
+      TERN_(E3_HAS_STEALTHCHOP, TMC_SAY_PWMTHRS_E(3));
+      TERN_(E4_HAS_STEALTHCHOP, TMC_SAY_PWMTHRS_E(4));
+      TERN_(E5_HAS_STEALTHCHOP, TMC_SAY_PWMTHRS_E(5));
+      TERN_(E6_HAS_STEALTHCHOP, TMC_SAY_PWMTHRS_E(6));
+      TERN_(E7_HAS_STEALTHCHOP, TMC_SAY_PWMTHRS_E(7));
+    }
+  }
+#endif // HYBRID_THRESHOLD
+
+/**
+ * M914: Set StallGuard sensitivity.
+ */
+#if USE_SENSORLESS
+  void GcodeSuite::M914() {
+
+    bool report = true;
+    const uint8_t index = parser.byteval('I');
+    LOOP_LINEAR_AXES(i) if (parser.seen(AXIS_CHAR(i))) {
+      const int16_t value = parser.value_int();
+      report = false;
+      switch (i) {
+        #if X_SENSORLESS
+          case X_AXIS:
+            #if AXIS_HAS_STALLGUARD(X)
+              if (index < 2) stepperX.homing_threshold(value);
+            #endif
+            #if AXIS_HAS_STALLGUARD(X2)
+              if (!(index & 1)) stepperX2.homing_threshold(value);
+            #endif
+            break;
+        #endif
+        #if Y_SENSORLESS
+          case Y_AXIS:
+            #if AXIS_HAS_STALLGUARD(Y)
+              if (index < 2) stepperY.homing_threshold(value);
+            #endif
+            #if AXIS_HAS_STALLGUARD(Y2)
+              if (!(index & 1)) stepperY2.homing_threshold(value);
+            #endif
+            break;
+        #endif
+        #if Z_SENSORLESS
+          case Z_AXIS:
+            #if AXIS_HAS_STALLGUARD(Z)
+              if (index < 2) stepperZ.homing_threshold(value);
+            #endif
+            #if AXIS_HAS_STALLGUARD(Z2)
+              if (index == 0 || index == 2) stepperZ2.homing_threshold(value);
+            #endif
+            #if AXIS_HAS_STALLGUARD(Z3)
+              if (index == 0 || index == 3) stepperZ3.homing_threshold(value);
+            #endif
+            #if AXIS_HAS_STALLGUARD(Z4)
+              if (index == 0 || index == 4) stepperZ4.homing_threshold(value);
+            #endif
+            break;
+        #endif
+        #if I_SENSORLESS && AXIS_HAS_STALLGUARD(I)
+          case I_AXIS: stepperI.homing_threshold(value); break;
+        #endif
+        #if J_SENSORLESS && AXIS_HAS_STALLGUARD(J)
+          case J_AXIS: stepperJ.homing_threshold(value); break;
+        #endif
+        #if K_SENSORLESS && AXIS_HAS_STALLGUARD(K)
+          case K_AXIS: stepperK.homing_threshold(value); break;
+        #endif
+      }
+    }
+
+    if (report) {
+      #if X_SENSORLESS
+        #if AXIS_HAS_STALLGUARD(X)
+          tmc_print_sgt(stepperX);
+        #endif
+        #if AXIS_HAS_STALLGUARD(X2)
+          tmc_print_sgt(stepperX2);
+        #endif
+      #endif
+      #if Y_SENSORLESS
+        #if AXIS_HAS_STALLGUARD(Y)
+          tmc_print_sgt(stepperY);
+        #endif
+        #if AXIS_HAS_STALLGUARD(Y2)
+          tmc_print_sgt(stepperY2);
+        #endif
+      #endif
+      #if Z_SENSORLESS
+        #if AXIS_HAS_STALLGUARD(Z)
+          tmc_print_sgt(stepperZ);
+        #endif
+        #if AXIS_HAS_STALLGUARD(Z2)
+          tmc_print_sgt(stepperZ2);
+        #endif
+        #if AXIS_HAS_STALLGUARD(Z3)
+          tmc_print_sgt(stepperZ3);
+        #endif
+        #if AXIS_HAS_STALLGUARD(Z4)
+          tmc_print_sgt(stepperZ4);
+        #endif
+      #endif
+      #if I_SENSORLESS && AXIS_HAS_STALLGUARD(I)
+        tmc_print_sgt(stepperI);
+      #endif
+      #if J_SENSORLESS && AXIS_HAS_STALLGUARD(J)
+        tmc_print_sgt(stepperJ);
+      #endif
+      #if K_SENSORLESS && AXIS_HAS_STALLGUARD(K)
+        tmc_print_sgt(stepperK);
+      #endif
+    }
+  }
+#endif // USE_SENSORLESS
+
+#endif // HAS_TRINAMIC_CONFIG