Outdent UBL code

Marlin/src/feature/bedlevel/ubl/ubl.cpp

@@ -24,234 +24,234 @@
 
 #if ENABLED(AUTO_BED_LEVELING_UBL)
 
-  #include "../bedlevel.h"
+#include "../bedlevel.h"
 
-  unified_bed_leveling ubl;
+unified_bed_leveling ubl;
 
-  #include "../../../MarlinCore.h"
-  #include "../../../gcode/gcode.h"
+#include "../../../MarlinCore.h"
+#include "../../../gcode/gcode.h"
 
-  #include "../../../module/settings.h"
-  #include "../../../module/planner.h"
-  #include "../../../module/motion.h"
-  #include "../../../module/probe.h"
+#include "../../../module/settings.h"
+#include "../../../module/planner.h"
+#include "../../../module/motion.h"
+#include "../../../module/probe.h"
 
-  #if ENABLED(EXTENSIBLE_UI)
-    #include "../../../lcd/extui/ui_api.h"
-  #endif
-
-  #include "math.h"
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../../../lcd/extui/ui_api.h"
+#endif
 
-  void unified_bed_leveling::echo_name() { SERIAL_ECHOPGM("Unified Bed Leveling"); }
+#include "math.h"
 
-  void unified_bed_leveling::report_current_mesh() {
-    if (!leveling_is_valid()) return;
-    SERIAL_ECHO_MSG("  G29 I999");
-    GRID_LOOP(x, y)
-      if (!isnan(z_values[x][y])) {
-        SERIAL_ECHO_START();
-        SERIAL_ECHOPAIR("  M421 I", x, " J", y);
-        SERIAL_ECHOLNPAIR_F_P(SP_Z_STR, z_values[x][y], 4);
-        serial_delay(75); // Prevent Printrun from exploding
-      }
-  }
+void unified_bed_leveling::echo_name() { SERIAL_ECHOPGM("Unified Bed Leveling"); }
 
-  void unified_bed_leveling::report_state() {
-    echo_name();
-    SERIAL_ECHO_TERNARY(planner.leveling_active, " System v" UBL_VERSION " ", "", "in", "active\n");
-    serial_delay(50);
+void unified_bed_leveling::report_current_mesh() {
+  if (!leveling_is_valid()) return;
+  SERIAL_ECHO_MSG("  G29 I999");
+  GRID_LOOP(x, y)
+    if (!isnan(z_values[x][y])) {
+      SERIAL_ECHO_START();
+      SERIAL_ECHOPAIR("  M421 I", x, " J", y);
+      SERIAL_ECHOLNPAIR_F_P(SP_Z_STR, z_values[x][y], 4);
+      serial_delay(75); // Prevent Printrun from exploding
+    }
+}
+
+void unified_bed_leveling::report_state() {
+  echo_name();
+  SERIAL_ECHO_TERNARY(planner.leveling_active, " System v" UBL_VERSION " ", "", "in", "active\n");
+  serial_delay(50);
+}
+
+int8_t unified_bed_leveling::storage_slot;
+
+float unified_bed_leveling::z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
+
+#define _GRIDPOS(A,N) (MESH_MIN_##A + N * (MESH_##A##_DIST))
+
+const float
+unified_bed_leveling::_mesh_index_to_xpos[GRID_MAX_POINTS_X] PROGMEM = ARRAY_N(GRID_MAX_POINTS_X,
+  _GRIDPOS(X,  0), _GRIDPOS(X,  1), _GRIDPOS(X,  2), _GRIDPOS(X,  3),
+  _GRIDPOS(X,  4), _GRIDPOS(X,  5), _GRIDPOS(X,  6), _GRIDPOS(X,  7),
+  _GRIDPOS(X,  8), _GRIDPOS(X,  9), _GRIDPOS(X, 10), _GRIDPOS(X, 11),
+  _GRIDPOS(X, 12), _GRIDPOS(X, 13), _GRIDPOS(X, 14), _GRIDPOS(X, 15)
+),
+unified_bed_leveling::_mesh_index_to_ypos[GRID_MAX_POINTS_Y] PROGMEM = ARRAY_N(GRID_MAX_POINTS_Y,
+  _GRIDPOS(Y,  0), _GRIDPOS(Y,  1), _GRIDPOS(Y,  2), _GRIDPOS(Y,  3),
+  _GRIDPOS(Y,  4), _GRIDPOS(Y,  5), _GRIDPOS(Y,  6), _GRIDPOS(Y,  7),
+  _GRIDPOS(Y,  8), _GRIDPOS(Y,  9), _GRIDPOS(Y, 10), _GRIDPOS(Y, 11),
+  _GRIDPOS(Y, 12), _GRIDPOS(Y, 13), _GRIDPOS(Y, 14), _GRIDPOS(Y, 15)
+);
+
+volatile int16_t unified_bed_leveling::encoder_diff;
+
+unified_bed_leveling::unified_bed_leveling() { reset(); }
+
+void unified_bed_leveling::reset() {
+  const bool was_enabled = planner.leveling_active;
+  set_bed_leveling_enabled(false);
+  storage_slot = -1;
+  ZERO(z_values);
+  #if ENABLED(EXTENSIBLE_UI)
+    GRID_LOOP(x, y) ExtUI::onMeshUpdate(x, y, 0);
+  #endif
+  if (was_enabled) report_current_position();
+}
+
+void unified_bed_leveling::invalidate() {
+  set_bed_leveling_enabled(false);
+  set_all_mesh_points_to_value(NAN);
+}
+
+void unified_bed_leveling::set_all_mesh_points_to_value(const float value) {
+  GRID_LOOP(x, y) {
+    z_values[x][y] = value;
+    TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, value));
   }
-
-  int8_t unified_bed_leveling::storage_slot;
-
-  float unified_bed_leveling::z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
-
-  #define _GRIDPOS(A,N) (MESH_MIN_##A + N * (MESH_##A##_DIST))
-
-  const float
-  unified_bed_leveling::_mesh_index_to_xpos[GRID_MAX_POINTS_X] PROGMEM = ARRAY_N(GRID_MAX_POINTS_X,
-    _GRIDPOS(X,  0), _GRIDPOS(X,  1), _GRIDPOS(X,  2), _GRIDPOS(X,  3),
-    _GRIDPOS(X,  4), _GRIDPOS(X,  5), _GRIDPOS(X,  6), _GRIDPOS(X,  7),
-    _GRIDPOS(X,  8), _GRIDPOS(X,  9), _GRIDPOS(X, 10), _GRIDPOS(X, 11),
-    _GRIDPOS(X, 12), _GRIDPOS(X, 13), _GRIDPOS(X, 14), _GRIDPOS(X, 15)
-  ),
-  unified_bed_leveling::_mesh_index_to_ypos[GRID_MAX_POINTS_Y] PROGMEM = ARRAY_N(GRID_MAX_POINTS_Y,
-    _GRIDPOS(Y,  0), _GRIDPOS(Y,  1), _GRIDPOS(Y,  2), _GRIDPOS(Y,  3),
-    _GRIDPOS(Y,  4), _GRIDPOS(Y,  5), _GRIDPOS(Y,  6), _GRIDPOS(Y,  7),
-    _GRIDPOS(Y,  8), _GRIDPOS(Y,  9), _GRIDPOS(Y, 10), _GRIDPOS(Y, 11),
-    _GRIDPOS(Y, 12), _GRIDPOS(Y, 13), _GRIDPOS(Y, 14), _GRIDPOS(Y, 15)
-  );
-
-  volatile int16_t unified_bed_leveling::encoder_diff;
-
-  unified_bed_leveling::unified_bed_leveling() { reset(); }
-
-  void unified_bed_leveling::reset() {
-    const bool was_enabled = planner.leveling_active;
-    set_bed_leveling_enabled(false);
-    storage_slot = -1;
-    ZERO(z_values);
-    #if ENABLED(EXTENSIBLE_UI)
-      GRID_LOOP(x, y) ExtUI::onMeshUpdate(x, y, 0);
-    #endif
-    if (was_enabled) report_current_position();
+}
+
+#if ENABLED(OPTIMIZED_MESH_STORAGE)
+
+  constexpr float mesh_store_scaling = 1000;
+  constexpr int16_t Z_STEPS_NAN = INT16_MAX;
+
+  void unified_bed_leveling::set_store_from_mesh(const bed_mesh_t &in_values, mesh_store_t &stored_values) {
+    auto z_to_store = [](const float &z) {
+      if (isnan(z)) return Z_STEPS_NAN;
+      const int32_t z_scaled = TRUNC(z * mesh_store_scaling);
+      if (z_scaled == Z_STEPS_NAN || !WITHIN(z_scaled, INT16_MIN, INT16_MAX))
+        return Z_STEPS_NAN; // If Z is out of range, return our custom 'NaN'
+      return int16_t(z_scaled);
+    };
+    GRID_LOOP(x, y) stored_values[x][y] = z_to_store(in_values[x][y]);
   }
 
-  void unified_bed_leveling::invalidate() {
-    set_bed_leveling_enabled(false);
-    set_all_mesh_points_to_value(NAN);
+  void unified_bed_leveling::set_mesh_from_store(const mesh_store_t &stored_values, bed_mesh_t &out_values) {
+    auto store_to_z = [](const int16_t z_scaled) {
+      return z_scaled == Z_STEPS_NAN ? NAN : z_scaled / mesh_store_scaling;
+    };
+    GRID_LOOP(x, y) out_values[x][y] = store_to_z(stored_values[x][y]);
   }
 
-  void unified_bed_leveling::set_all_mesh_points_to_value(const float value) {
-    GRID_LOOP(x, y) {
-      z_values[x][y] = value;
-      TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, value));
-    }
+#endif // OPTIMIZED_MESH_STORAGE
+
+static void serial_echo_xy(const uint8_t sp, const int16_t x, const int16_t y) {
+  SERIAL_ECHO_SP(sp);
+  SERIAL_CHAR('(');
+  if (x < 100) { SERIAL_CHAR(' '); if (x < 10) SERIAL_CHAR(' '); }
+  SERIAL_ECHO(x);
+  SERIAL_CHAR(',');
+  if (y < 100) { SERIAL_CHAR(' '); if (y < 10) SERIAL_CHAR(' '); }
+  SERIAL_ECHO(y);
+  SERIAL_CHAR(')');
+  serial_delay(5);
+}
+
+static void serial_echo_column_labels(const uint8_t sp) {
+  SERIAL_ECHO_SP(7);
+  LOOP_L_N(i, GRID_MAX_POINTS_X) {
+    if (i < 10) SERIAL_CHAR(' ');
+    SERIAL_ECHO(i);
+    SERIAL_ECHO_SP(sp);
   }
+  serial_delay(10);
+}
 
-  #if ENABLED(OPTIMIZED_MESH_STORAGE)
-
-    constexpr float mesh_store_scaling = 1000;
-    constexpr int16_t Z_STEPS_NAN = INT16_MAX;
-
-    void unified_bed_leveling::set_store_from_mesh(const bed_mesh_t &in_values, mesh_store_t &stored_values) {
-      auto z_to_store = [](const float &z) {
-        if (isnan(z)) return Z_STEPS_NAN;
-        const int32_t z_scaled = TRUNC(z * mesh_store_scaling);
-        if (z_scaled == Z_STEPS_NAN || !WITHIN(z_scaled, INT16_MIN, INT16_MAX))
-          return Z_STEPS_NAN; // If Z is out of range, return our custom 'NaN'
-        return int16_t(z_scaled);
-      };
-      GRID_LOOP(x, y) stored_values[x][y] = z_to_store(in_values[x][y]);
-    }
+/**
+ * Produce one of these mesh maps:
+ *   0: Human-readable
+ *   1: CSV format for spreadsheet import
+ *   2: TODO: Display on Graphical LCD
+ *   4: Compact Human-Readable
+ */
+void unified_bed_leveling::display_map(const int map_type) {
+  const bool was = gcode.set_autoreport_paused(true);
 
-    void unified_bed_leveling::set_mesh_from_store(const mesh_store_t &stored_values, bed_mesh_t &out_values) {
-      auto store_to_z = [](const int16_t z_scaled) {
-        return z_scaled == Z_STEPS_NAN ? NAN : z_scaled / mesh_store_scaling;
-      };
-      GRID_LOOP(x, y) out_values[x][y] = store_to_z(stored_values[x][y]);
-    }
+  constexpr uint8_t eachsp = 1 + 6 + 1,                           // [-3.567]
+                    twixt = eachsp * (GRID_MAX_POINTS_X) - 9 * 2; // Leading 4sp, Coordinates 9sp each
 
-  #endif // OPTIMIZED_MESH_STORAGE
+  const bool human = !(map_type & 0x3), csv = map_type == 1, lcd = map_type == 2, comp = map_type & 0x4;
 
-  static void serial_echo_xy(const uint8_t sp, const int16_t x, const int16_t y) {
-    SERIAL_ECHO_SP(sp);
-    SERIAL_CHAR('(');
-    if (x < 100) { SERIAL_CHAR(' '); if (x < 10) SERIAL_CHAR(' '); }
-    SERIAL_ECHO(x);
-    SERIAL_CHAR(',');
-    if (y < 100) { SERIAL_CHAR(' '); if (y < 10) SERIAL_CHAR(' '); }
-    SERIAL_ECHO(y);
-    SERIAL_CHAR(')');
-    serial_delay(5);
+  SERIAL_ECHOPGM("\nBed Topography Report");
+  if (human) {
+    SERIAL_ECHOLNPGM(":\n");
+    serial_echo_xy(4, MESH_MIN_X, MESH_MAX_Y);
+    serial_echo_xy(twixt, MESH_MAX_X, MESH_MAX_Y);
+    SERIAL_EOL();
+    serial_echo_column_labels(eachsp - 2);
   }
-
-  static void serial_echo_column_labels(const uint8_t sp) {
-    SERIAL_ECHO_SP(7);
-    LOOP_L_N(i, GRID_MAX_POINTS_X) {
-      if (i < 10) SERIAL_CHAR(' ');
-      SERIAL_ECHO(i);
-      SERIAL_ECHO_SP(sp);
-    }
-    serial_delay(10);
+  else {
+    SERIAL_ECHOPGM(" for ");
+    serialprintPGM(csv ? PSTR("CSV:\n") : PSTR("LCD:\n"));
   }
 
-  /**
-   * Produce one of these mesh maps:
-   *   0: Human-readable
-   *   1: CSV format for spreadsheet import
-   *   2: TODO: Display on Graphical LCD
-   *   4: Compact Human-Readable
-   */
-  void unified_bed_leveling::display_map(const int map_type) {
-    const bool was = gcode.set_autoreport_paused(true);
+  // Add XY probe offset from extruder because probe.probe_at_point() subtracts them when
+  // moving to the XY position to be measured. This ensures better agreement between
+  // the current Z position after G28 and the mesh values.
+  const xy_int8_t curr = closest_indexes(xy_pos_t(current_position) + probe.offset_xy);
 
-    constexpr uint8_t eachsp = 1 + 6 + 1,                           // [-3.567]
-                      twixt = eachsp * (GRID_MAX_POINTS_X) - 9 * 2; // Leading 4sp, Coordinates 9sp each
+  if (!lcd) SERIAL_EOL();
+  for (int8_t j = GRID_MAX_POINTS_Y - 1; j >= 0; j--) {
 
-    const bool human = !(map_type & 0x3), csv = map_type == 1, lcd = map_type == 2, comp = map_type & 0x4;
-
-    SERIAL_ECHOPGM("\nBed Topography Report");
+    // Row Label (J index)
     if (human) {
-      SERIAL_ECHOLNPGM(":\n");
-      serial_echo_xy(4, MESH_MIN_X, MESH_MAX_Y);
-      serial_echo_xy(twixt, MESH_MAX_X, MESH_MAX_Y);
-      SERIAL_EOL();
-      serial_echo_column_labels(eachsp - 2);
-    }
-    else {
-      SERIAL_ECHOPGM(" for ");
-      serialprintPGM(csv ? PSTR("CSV:\n") : PSTR("LCD:\n"));
+      if (j < 10) SERIAL_CHAR(' ');
+      SERIAL_ECHO(j);
+      SERIAL_ECHOPGM(" |");
     }
 
-    // Add XY probe offset from extruder because probe.probe_at_point() subtracts them when
-    // moving to the XY position to be measured. This ensures better agreement between
-    // the current Z position after G28 and the mesh values.
-    const xy_int8_t curr = closest_indexes(xy_pos_t(current_position) + probe.offset_xy);
+    // Row Values (I indexes)
+    LOOP_L_N(i, GRID_MAX_POINTS_X) {
 
-    if (!lcd) SERIAL_EOL();
-    for (int8_t j = GRID_MAX_POINTS_Y - 1; j >= 0; j--) {
+      // Opening Brace or Space
+      const bool is_current = i == curr.x && j == curr.y;
+      if (human) SERIAL_CHAR(is_current ? '[' : ' ');
 
-      // Row Label (J index)
-      if (human) {
-        if (j < 10) SERIAL_CHAR(' ');
-        SERIAL_ECHO(j);
-        SERIAL_ECHOPGM(" |");
+      // Z Value at current I, J
+      const float f = z_values[i][j];
+      if (lcd) {
+        // TODO: Display on Graphical LCD
       }
-
-      // Row Values (I indexes)
-      LOOP_L_N(i, GRID_MAX_POINTS_X) {
-
-        // Opening Brace or Space
-        const bool is_current = i == curr.x && j == curr.y;
-        if (human) SERIAL_CHAR(is_current ? '[' : ' ');
-
-        // Z Value at current I, J
-        const float f = z_values[i][j];
-        if (lcd) {
-          // TODO: Display on Graphical LCD
-        }
-        else if (isnan(f))
-          serialprintPGM(human ? PSTR("  .   ") : PSTR("NAN"));
-        else if (human || csv) {
-          if (human && f >= 0.0) SERIAL_CHAR(f > 0 ? '+' : ' ');  // Display sign also for positive numbers (' ' for 0)
-          SERIAL_ECHO_F(f, 3);                                    // Positive: 5 digits, Negative: 6 digits
-        }
-        if (csv && i < GRID_MAX_POINTS_X - 1) SERIAL_CHAR('\t');
-
-        // Closing Brace or Space
-        if (human) SERIAL_CHAR(is_current ? ']' : ' ');
-
-        SERIAL_FLUSHTX();
-        idle_no_sleep();
+      else if (isnan(f))
+        serialprintPGM(human ? PSTR("  .   ") : PSTR("NAN"));
+      else if (human || csv) {
+        if (human && f >= 0.0) SERIAL_CHAR(f > 0 ? '+' : ' ');  // Display sign also for positive numbers (' ' for 0)
+        SERIAL_ECHO_F(f, 3);                                    // Positive: 5 digits, Negative: 6 digits
       }
-      if (!lcd) SERIAL_EOL();
+      if (csv && i < GRID_MAX_POINTS_X - 1) SERIAL_CHAR('\t');
 
-      // A blank line between rows (unless compact)
-      if (j && human && !comp) SERIAL_ECHOLNPGM("   |");
-    }
+      // Closing Brace or Space
+      if (human) SERIAL_CHAR(is_current ? ']' : ' ');
 
-    if (human) {
-      serial_echo_column_labels(eachsp - 2);
-      SERIAL_EOL();
-      serial_echo_xy(4, MESH_MIN_X, MESH_MIN_Y);
-      serial_echo_xy(twixt, MESH_MAX_X, MESH_MIN_Y);
-      SERIAL_EOL();
-      SERIAL_EOL();
+      SERIAL_FLUSHTX();
+      idle_no_sleep();
     }
+    if (!lcd) SERIAL_EOL();
 
-    gcode.set_autoreport_paused(was);
+    // A blank line between rows (unless compact)
+    if (j && human && !comp) SERIAL_ECHOLNPGM("   |");
   }
 
-  bool unified_bed_leveling::sanity_check() {
-    uint8_t error_flag = 0;
+  if (human) {
+    serial_echo_column_labels(eachsp - 2);
+    SERIAL_EOL();
+    serial_echo_xy(4, MESH_MIN_X, MESH_MIN_Y);
+    serial_echo_xy(twixt, MESH_MAX_X, MESH_MIN_Y);
+    SERIAL_EOL();
+    SERIAL_EOL();
+  }
 
-    if (settings.calc_num_meshes() < 1) {
-      SERIAL_ECHOLNPGM("?Mesh too big for EEPROM.");
-      error_flag++;
-    }
+  gcode.set_autoreport_paused(was);
+}
 
-    return !!error_flag;
+bool unified_bed_leveling::sanity_check() {
+  uint8_t error_flag = 0;
+
+  if (settings.calc_num_meshes() < 1) {
+    SERIAL_ECHOLNPGM("?Mesh too big for EEPROM.");
+    error_flag++;
   }
 
+  return !!error_flag;
+}
+
 #endif // AUTO_BED_LEVELING_UBL

Marlin/src/feature/bedlevel/ubl/ubl.h

@@ -46,275 +46,275 @@ struct mesh_index_pair;
 #endif
 
 class unified_bed_leveling {
-  private:
-
-    static int    g29_verbose_level,
-                  g29_phase_value,
-                  g29_repetition_cnt,
-                  g29_storage_slot,
-                  g29_map_type;
-    static bool   g29_c_flag;
-    static float  g29_card_thickness,
-                  g29_constant;
-    static xy_pos_t g29_pos;
-    static xy_bool_t xy_seen;
-
-    #if HAS_BED_PROBE
-      static int  g29_grid_size;
-    #endif
-
-    #if IS_NEWPANEL
-      static void move_z_with_encoder(const float &multiplier);
-      static float measure_point_with_encoder();
-      static float measure_business_card_thickness();
-      static void manually_probe_remaining_mesh(const xy_pos_t&, const float&, const float&, const bool) _O0;
-      static void fine_tune_mesh(const xy_pos_t &pos, const bool do_ubl_mesh_map) _O0;
-    #endif
+private:
+
+  static int    g29_verbose_level,
+                g29_phase_value,
+                g29_repetition_cnt,
+                g29_storage_slot,
+                g29_map_type;
+  static bool   g29_c_flag;
+  static float  g29_card_thickness,
+                g29_constant;
+  static xy_pos_t g29_pos;
+  static xy_bool_t xy_seen;
+
+  #if HAS_BED_PROBE
+    static int  g29_grid_size;
+  #endif
+
+  #if IS_NEWPANEL
+    static void move_z_with_encoder(const float &multiplier);
+    static float measure_point_with_encoder();
+    static float measure_business_card_thickness();
+    static void manually_probe_remaining_mesh(const xy_pos_t&, const float&, const float&, const bool) _O0;
+    static void fine_tune_mesh(const xy_pos_t &pos, const bool do_ubl_mesh_map) _O0;
+  #endif
+
+  static bool g29_parameter_parsing() _O0;
+  static void shift_mesh_height();
+  static void probe_entire_mesh(const xy_pos_t &near, const bool do_ubl_mesh_map, const bool stow_probe, const bool do_furthest) _O0;
+  static void tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3);
+  static void tilt_mesh_based_on_probed_grid(const bool do_ubl_mesh_map);
+  static bool smart_fill_one(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir);
+  static inline bool smart_fill_one(const xy_uint8_t &pos, const xy_uint8_t &dir) {
+    return smart_fill_one(pos.x, pos.y, dir.x, dir.y);
+  }
+  static void smart_fill_mesh();
+
+  #if ENABLED(UBL_DEVEL_DEBUGGING)
+    static void g29_what_command();
+    static void g29_eeprom_dump();
+    static void g29_compare_current_mesh_to_stored_mesh();
+  #endif
+
+public:
+
+  static void echo_name();
+  static void report_current_mesh();
+  static void report_state();
+  static void save_ubl_active_state_and_disable();
+  static void restore_ubl_active_state_and_leave();
+  static void display_map(const int) _O0;
+  static mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const xy_pos_t&, const bool=false, MeshFlags *done_flags=nullptr) _O0;
+  static mesh_index_pair find_furthest_invalid_mesh_point() _O0;
+  static void reset();
+  static void invalidate();
+  static void set_all_mesh_points_to_value(const float value);
+  static void adjust_mesh_to_mean(const bool cflag, const float value);
+  static bool sanity_check();
+
+  static void G29() _O0;                          // O0 for no optimization
+  static void smart_fill_wlsf(const float &) _O2; // O2 gives smaller code than Os on A2560
+
+  static int8_t storage_slot;
+
+  static bed_mesh_t z_values;
+  #if ENABLED(OPTIMIZED_MESH_STORAGE)
+    static void set_store_from_mesh(const bed_mesh_t &in_values, mesh_store_t &stored_values);
+    static void set_mesh_from_store(const mesh_store_t &stored_values, bed_mesh_t &out_values);
+  #endif
+  static const float _mesh_index_to_xpos[GRID_MAX_POINTS_X],
+                     _mesh_index_to_ypos[GRID_MAX_POINTS_Y];
+
+  #if HAS_LCD_MENU
+    static bool lcd_map_control;
+    static void steppers_were_disabled();
+  #else
+    static inline void steppers_were_disabled() {}
+  #endif
+
+  static volatile int16_t encoder_diff; // Volatile because buttons may changed it at interrupt time
+
+  unified_bed_leveling();
+
+  FORCE_INLINE static void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; }
+
+  static int8_t cell_index_x_raw(const float &x) {
+    return FLOOR((x - (MESH_MIN_X)) * RECIPROCAL(MESH_X_DIST));
+  }
+
+  static int8_t cell_index_y_raw(const float &y) {
+    return FLOOR((y - (MESH_MIN_Y)) * RECIPROCAL(MESH_Y_DIST));
+  }
+
+  static int8_t cell_index_x_valid(const float &x) {
+    return WITHIN(cell_index_x_raw(x), 0, (GRID_MAX_POINTS_X - 2));
+  }
+
+  static int8_t cell_index_y_valid(const float &y) {
+    return WITHIN(cell_index_y_raw(y), 0, (GRID_MAX_POINTS_Y - 2));
+  }
+
+  static int8_t cell_index_x(const float &x) {
+    return constrain(cell_index_x_raw(x), 0, (GRID_MAX_POINTS_X) - 2);
+  }
+
+  static int8_t cell_index_y(const float &y) {
+    return constrain(cell_index_y_raw(y), 0, (GRID_MAX_POINTS_Y) - 2);
+  }
+
+  static inline xy_int8_t cell_indexes(const float &x, const float &y) {
+    return { cell_index_x(x), cell_index_y(y) };
+  }
+  static inline xy_int8_t cell_indexes(const xy_pos_t &xy) { return cell_indexes(xy.x, xy.y); }
+
+  static int8_t closest_x_index(const float &x) {
+    const int8_t px = (x - (MESH_MIN_X) + (MESH_X_DIST) * 0.5) * RECIPROCAL(MESH_X_DIST);
+    return WITHIN(px, 0, GRID_MAX_POINTS_X - 1) ? px : -1;
+  }
+  static int8_t closest_y_index(const float &y) {
+    const int8_t py = (y - (MESH_MIN_Y) + (MESH_Y_DIST) * 0.5) * RECIPROCAL(MESH_Y_DIST);
+    return WITHIN(py, 0, GRID_MAX_POINTS_Y - 1) ? py : -1;
+  }
+  static inline xy_int8_t closest_indexes(const xy_pos_t &xy) {
+    return { closest_x_index(xy.x), closest_y_index(xy.y) };
+  }
+
+  /**
+   *                           z2   --|
+   *                 z0        |      |
+   *                  |        |      + (z2-z1)
+   *   z1             |        |      |
+   * ---+-------------+--------+--  --|
+   *   a1            a0        a2
+   *    |<---delta_a---------->|
+   *
+   *  calc_z0 is the basis for all the Mesh Based correction. It is used to
+   *  find the expected Z Height at a position between two known Z-Height locations.
+   *
+   *  It is fairly expensive with its 4 floating point additions and 2 floating point
+   *  multiplications.
+   */
+  FORCE_INLINE static float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) {
+    return z1 + (z2 - z1) * (a0 - a1) / (a2 - a1);
+  }
+
+  #ifdef UBL_Z_RAISE_WHEN_OFF_MESH
+    #define _UBL_OUTER_Z_RAISE UBL_Z_RAISE_WHEN_OFF_MESH
+  #else
+    #define _UBL_OUTER_Z_RAISE NAN
+  #endif
+
+  /**
+   * z_correction_for_x_on_horizontal_mesh_line is an optimization for
+   * the case where the printer is making a vertical line that only crosses horizontal mesh lines.
+   */
+  static inline float z_correction_for_x_on_horizontal_mesh_line(const float &rx0, const int x1_i, const int yi) {
+    if (!WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(yi, 0, GRID_MAX_POINTS_Y - 1)) {
+
+      if (DEBUGGING(LEVELING)) {
+        if (WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 1)) DEBUG_ECHOPGM("yi"); else DEBUG_ECHOPGM("x1_i");
+        DEBUG_ECHOLNPAIR(" out of bounds in z_correction_for_x_on_horizontal_mesh_line(rx0=", rx0, ",x1_i=", x1_i, ",yi=", yi, ")");
+      }
 
-    static bool g29_parameter_parsing() _O0;
-    static void shift_mesh_height();
-    static void probe_entire_mesh(const xy_pos_t &near, const bool do_ubl_mesh_map, const bool stow_probe, const bool do_furthest) _O0;
-    static void tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3);
-    static void tilt_mesh_based_on_probed_grid(const bool do_ubl_mesh_map);
-    static bool smart_fill_one(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir);
-    static inline bool smart_fill_one(const xy_uint8_t &pos, const xy_uint8_t &dir) {
-      return smart_fill_one(pos.x, pos.y, dir.x, dir.y);
+      // The requested location is off the mesh. Return UBL_Z_RAISE_WHEN_OFF_MESH or NAN.
+      return _UBL_OUTER_Z_RAISE;
     }
-    static void smart_fill_mesh();
-
-    #if ENABLED(UBL_DEVEL_DEBUGGING)
-      static void g29_what_command();
-      static void g29_eeprom_dump();
-      static void g29_compare_current_mesh_to_stored_mesh();
-    #endif
 
-  public:
-
-    static void echo_name();
-    static void report_current_mesh();
-    static void report_state();
-    static void save_ubl_active_state_and_disable();
-    static void restore_ubl_active_state_and_leave();
-    static void display_map(const int) _O0;
-    static mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const xy_pos_t&, const bool=false, MeshFlags *done_flags=nullptr) _O0;
-    static mesh_index_pair find_furthest_invalid_mesh_point() _O0;
-    static void reset();
-    static void invalidate();
-    static void set_all_mesh_points_to_value(const float value);
-    static void adjust_mesh_to_mean(const bool cflag, const float value);
-    static bool sanity_check();
-
-    static void G29() _O0;                          // O0 for no optimization
-    static void smart_fill_wlsf(const float &) _O2; // O2 gives smaller code than Os on A2560
-
-    static int8_t storage_slot;
-
-    static bed_mesh_t z_values;
-    #if ENABLED(OPTIMIZED_MESH_STORAGE)
-      static void set_store_from_mesh(const bed_mesh_t &in_values, mesh_store_t &stored_values);
-      static void set_mesh_from_store(const mesh_store_t &stored_values, bed_mesh_t &out_values);
-    #endif
-    static const float _mesh_index_to_xpos[GRID_MAX_POINTS_X],
-                       _mesh_index_to_ypos[GRID_MAX_POINTS_Y];
-
-    #if HAS_LCD_MENU
-      static bool lcd_map_control;
-      static void steppers_were_disabled();
-    #else
-      static inline void steppers_were_disabled() {}
-    #endif
-
-    static volatile int16_t encoder_diff; // Volatile because buttons may changed it at interrupt time
+    const float xratio = (rx0 - mesh_index_to_xpos(x1_i)) * RECIPROCAL(MESH_X_DIST),
+                z1 = z_values[x1_i][yi];
 
-    unified_bed_leveling();
+    return z1 + xratio * (z_values[_MIN(x1_i, GRID_MAX_POINTS_X - 2) + 1][yi] - z1); // Don't allow x1_i+1 to be past the end of the array
+                                                                                    // If it is, it is clamped to the last element of the
+                                                                                    // z_values[][] array and no correction is applied.
+  }
 
-    FORCE_INLINE static void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; }
+  //
+  // See comments above for z_correction_for_x_on_horizontal_mesh_line
+  //
+  static inline float z_correction_for_y_on_vertical_mesh_line(const float &ry0, const int xi, const int y1_i) {
+    if (!WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(y1_i, 0, GRID_MAX_POINTS_Y - 1)) {
 
-    static int8_t cell_index_x_raw(const float &x) {
-      return FLOOR((x - (MESH_MIN_X)) * RECIPROCAL(MESH_X_DIST));
-    }
-
-    static int8_t cell_index_y_raw(const float &y) {
-      return FLOOR((y - (MESH_MIN_Y)) * RECIPROCAL(MESH_Y_DIST));
-    }
-
-    static int8_t cell_index_x_valid(const float &x) {
-      return WITHIN(cell_index_x_raw(x), 0, (GRID_MAX_POINTS_X - 2));
-    }
+      if (DEBUGGING(LEVELING)) {
+        if (WITHIN(xi, 0, GRID_MAX_POINTS_X - 1)) DEBUG_ECHOPGM("y1_i"); else DEBUG_ECHOPGM("xi");
+        DEBUG_ECHOLNPAIR(" out of bounds in z_correction_for_y_on_vertical_mesh_line(ry0=", ry0, ", xi=", xi, ", y1_i=", y1_i, ")");
+      }
 
-    static int8_t cell_index_y_valid(const float &y) {
-      return WITHIN(cell_index_y_raw(y), 0, (GRID_MAX_POINTS_Y - 2));
+      // The requested location is off the mesh. Return UBL_Z_RAISE_WHEN_OFF_MESH or NAN.
+      return _UBL_OUTER_Z_RAISE;
     }
 
-    static int8_t cell_index_x(const float &x) {
-      return constrain(cell_index_x_raw(x), 0, (GRID_MAX_POINTS_X) - 2);
-    }
+    const float yratio = (ry0 - mesh_index_to_ypos(y1_i)) * RECIPROCAL(MESH_Y_DIST),
+                z1 = z_values[xi][y1_i];
 
-    static int8_t cell_index_y(const float &y) {
-      return constrain(cell_index_y_raw(y), 0, (GRID_MAX_POINTS_Y) - 2);
-    }
+    return z1 + yratio * (z_values[xi][_MIN(y1_i, GRID_MAX_POINTS_Y - 2) + 1] - z1); // Don't allow y1_i+1 to be past the end of the array
+                                                                                    // If it is, it is clamped to the last element of the
+                                                                                    // z_values[][] array and no correction is applied.
+  }
 
-    static inline xy_int8_t cell_indexes(const float &x, const float &y) {
-      return { cell_index_x(x), cell_index_y(y) };
-    }
-    static inline xy_int8_t cell_indexes(const xy_pos_t &xy) { return cell_indexes(xy.x, xy.y); }
-
-    static int8_t closest_x_index(const float &x) {
-      const int8_t px = (x - (MESH_MIN_X) + (MESH_X_DIST) * 0.5) * RECIPROCAL(MESH_X_DIST);
-      return WITHIN(px, 0, GRID_MAX_POINTS_X - 1) ? px : -1;
-    }
-    static int8_t closest_y_index(const float &y) {
-      const int8_t py = (y - (MESH_MIN_Y) + (MESH_Y_DIST) * 0.5) * RECIPROCAL(MESH_Y_DIST);
-      return WITHIN(py, 0, GRID_MAX_POINTS_Y - 1) ? py : -1;
-    }
-    static inline xy_int8_t closest_indexes(const xy_pos_t &xy) {
-      return { closest_x_index(xy.x), closest_y_index(xy.y) };
-    }
+  /**
+   * This is the generic Z-Correction. It works anywhere within a Mesh Cell. It first
+   * does a linear interpolation along both of the bounding X-Mesh-Lines to find the
+   * Z-Height at both ends. Then it does a linear interpolation of these heights based
+   * on the Y position within the cell.
+   */
+  static float get_z_correction(const float &rx0, const float &ry0) {
+    const int8_t cx = cell_index_x(rx0), cy = cell_index_y(ry0); // return values are clamped
 
     /**
-     *                           z2   --|
-     *                 z0        |      |
-     *                  |        |      + (z2-z1)
-     *   z1             |        |      |
-     * ---+-------------+--------+--  --|
-     *   a1            a0        a2
-     *    |<---delta_a---------->|
-     *
-     *  calc_z0 is the basis for all the Mesh Based correction. It is used to
-     *  find the expected Z Height at a position between two known Z-Height locations.
-     *
-     *  It is fairly expensive with its 4 floating point additions and 2 floating point
-     *  multiplications.
+     * Check if the requested location is off the mesh.  If so, and
+     * UBL_Z_RAISE_WHEN_OFF_MESH is specified, that value is returned.
      */
-    FORCE_INLINE static float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) {
-      return z1 + (z2 - z1) * (a0 - a1) / (a2 - a1);
-    }
-
     #ifdef UBL_Z_RAISE_WHEN_OFF_MESH
-      #define _UBL_OUTER_Z_RAISE UBL_Z_RAISE_WHEN_OFF_MESH
-    #else
-      #define _UBL_OUTER_Z_RAISE NAN
+      if (!WITHIN(rx0, MESH_MIN_X, MESH_MAX_X) || !WITHIN(ry0, MESH_MIN_Y, MESH_MAX_Y))
+        return UBL_Z_RAISE_WHEN_OFF_MESH;
     #endif
 
-    /**
-     * z_correction_for_x_on_horizontal_mesh_line is an optimization for
-     * the case where the printer is making a vertical line that only crosses horizontal mesh lines.
-     */
-    static inline float z_correction_for_x_on_horizontal_mesh_line(const float &rx0, const int x1_i, const int yi) {
-      if (!WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(yi, 0, GRID_MAX_POINTS_Y - 1)) {
-
-        if (DEBUGGING(LEVELING)) {
-          if (WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 1)) DEBUG_ECHOPGM("yi"); else DEBUG_ECHOPGM("x1_i");
-          DEBUG_ECHOLNPAIR(" out of bounds in z_correction_for_x_on_horizontal_mesh_line(rx0=", rx0, ",x1_i=", x1_i, ",yi=", yi, ")");
-        }
+    const float z1 = calc_z0(rx0,
+                             mesh_index_to_xpos(cx), z_values[cx][cy],
+                             mesh_index_to_xpos(cx + 1), z_values[_MIN(cx, GRID_MAX_POINTS_X - 2) + 1][cy]);
 
-        // The requested location is off the mesh. Return UBL_Z_RAISE_WHEN_OFF_MESH or NAN.
-        return _UBL_OUTER_Z_RAISE;
-      }
+    const float z2 = calc_z0(rx0,
+                             mesh_index_to_xpos(cx), z_values[cx][_MIN(cy, GRID_MAX_POINTS_Y - 2) + 1],
+                             mesh_index_to_xpos(cx + 1), z_values[_MIN(cx, GRID_MAX_POINTS_X - 2) + 1][_MIN(cy, GRID_MAX_POINTS_Y - 2) + 1]);
 
-      const float xratio = (rx0 - mesh_index_to_xpos(x1_i)) * RECIPROCAL(MESH_X_DIST),
-                  z1 = z_values[x1_i][yi];
+    float z0 = calc_z0(ry0,
+                       mesh_index_to_ypos(cy), z1,
+                       mesh_index_to_ypos(cy + 1), z2);
 
-      return z1 + xratio * (z_values[_MIN(x1_i, GRID_MAX_POINTS_X - 2) + 1][yi] - z1); // Don't allow x1_i+1 to be past the end of the array
-                                                                                      // If it is, it is clamped to the last element of the
-                                                                                      // z_values[][] array and no correction is applied.
+    if (DEBUGGING(MESH_ADJUST)) {
+      DEBUG_ECHOPAIR(" raw get_z_correction(", rx0);
+      DEBUG_CHAR(','); DEBUG_ECHO(ry0);
+      DEBUG_ECHOPAIR_F(") = ", z0, 6);
+      DEBUG_ECHOLNPAIR_F(" >>>---> ", z0, 6);
     }
 
-    //
-    // See comments above for z_correction_for_x_on_horizontal_mesh_line
-    //
-    static inline float z_correction_for_y_on_vertical_mesh_line(const float &ry0, const int xi, const int y1_i) {
-      if (!WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(y1_i, 0, GRID_MAX_POINTS_Y - 1)) {
-
-        if (DEBUGGING(LEVELING)) {
-          if (WITHIN(xi, 0, GRID_MAX_POINTS_X - 1)) DEBUG_ECHOPGM("y1_i"); else DEBUG_ECHOPGM("xi");
-          DEBUG_ECHOLNPAIR(" out of bounds in z_correction_for_y_on_vertical_mesh_line(ry0=", ry0, ", xi=", xi, ", y1_i=", y1_i, ")");
-        }
-
-        // The requested location is off the mesh. Return UBL_Z_RAISE_WHEN_OFF_MESH or NAN.
-        return _UBL_OUTER_Z_RAISE;
-      }
-
-      const float yratio = (ry0 - mesh_index_to_ypos(y1_i)) * RECIPROCAL(MESH_Y_DIST),
-                  z1 = z_values[xi][y1_i];
-
-      return z1 + yratio * (z_values[xi][_MIN(y1_i, GRID_MAX_POINTS_Y - 2) + 1] - z1); // Don't allow y1_i+1 to be past the end of the array
-                                                                                      // If it is, it is clamped to the last element of the
-                                                                                      // z_values[][] array and no correction is applied.
-    }
-
-    /**
-     * This is the generic Z-Correction. It works anywhere within a Mesh Cell. It first
-     * does a linear interpolation along both of the bounding X-Mesh-Lines to find the
-     * Z-Height at both ends. Then it does a linear interpolation of these heights based
-     * on the Y position within the cell.
-     */
-    static float get_z_correction(const float &rx0, const float &ry0) {
-      const int8_t cx = cell_index_x(rx0), cy = cell_index_y(ry0); // return values are clamped
-
-      /**
-       * Check if the requested location is off the mesh.  If so, and
-       * UBL_Z_RAISE_WHEN_OFF_MESH is specified, that value is returned.
-       */
-      #ifdef UBL_Z_RAISE_WHEN_OFF_MESH
-        if (!WITHIN(rx0, MESH_MIN_X, MESH_MAX_X) || !WITHIN(ry0, MESH_MIN_Y, MESH_MAX_Y))
-          return UBL_Z_RAISE_WHEN_OFF_MESH;
-      #endif
-
-      const float z1 = calc_z0(rx0,
-                               mesh_index_to_xpos(cx), z_values[cx][cy],
-                               mesh_index_to_xpos(cx + 1), z_values[_MIN(cx, GRID_MAX_POINTS_X - 2) + 1][cy]);
-
-      const float z2 = calc_z0(rx0,
-                               mesh_index_to_xpos(cx), z_values[cx][_MIN(cy, GRID_MAX_POINTS_Y - 2) + 1],
-                               mesh_index_to_xpos(cx + 1), z_values[_MIN(cx, GRID_MAX_POINTS_X - 2) + 1][_MIN(cy, GRID_MAX_POINTS_Y - 2) + 1]);
-
-      float z0 = calc_z0(ry0,
-                         mesh_index_to_ypos(cy), z1,
-                         mesh_index_to_ypos(cy + 1), z2);
+    if (isnan(z0)) { // if part of the Mesh is undefined, it will show up as NAN
+      z0 = 0.0;      // in ubl.z_values[][] and propagate through the
+                     // calculations. If our correction is NAN, we throw it out
+                     // because part of the Mesh is undefined and we don't have the
+                     // information we need to complete the height correction.
 
       if (DEBUGGING(MESH_ADJUST)) {
-        DEBUG_ECHOPAIR(" raw get_z_correction(", rx0);
-        DEBUG_CHAR(','); DEBUG_ECHO(ry0);
-        DEBUG_ECHOPAIR_F(") = ", z0, 6);
-        DEBUG_ECHOLNPAIR_F(" >>>---> ", z0, 6);
-      }
-
-      if (isnan(z0)) { // if part of the Mesh is undefined, it will show up as NAN
-        z0 = 0.0;      // in ubl.z_values[][] and propagate through the
-                       // calculations. If our correction is NAN, we throw it out
-                       // because part of the Mesh is undefined and we don't have the
-                       // information we need to complete the height correction.
-
-        if (DEBUGGING(MESH_ADJUST)) {
-          DEBUG_ECHOPAIR("??? Yikes!  NAN in get_z_correction(", rx0);
-          DEBUG_CHAR(',');
-          DEBUG_ECHO(ry0);
-          DEBUG_CHAR(')');
-          DEBUG_EOL();
-        }
+        DEBUG_ECHOPAIR("??? Yikes!  NAN in get_z_correction(", rx0);
+        DEBUG_CHAR(',');
+        DEBUG_ECHO(ry0);
+        DEBUG_CHAR(')');
+        DEBUG_EOL();
       }
-      return z0;
-    }
-    static inline float get_z_correction(const xy_pos_t &pos) { return get_z_correction(pos.x, pos.y); }
-
-    static inline float mesh_index_to_xpos(const uint8_t i) {
-      return i < GRID_MAX_POINTS_X ? pgm_read_float(&_mesh_index_to_xpos[i]) : MESH_MIN_X + i * (MESH_X_DIST);
-    }
-    static inline float mesh_index_to_ypos(const uint8_t i) {
-      return i < GRID_MAX_POINTS_Y ? pgm_read_float(&_mesh_index_to_ypos[i]) : MESH_MIN_Y + i * (MESH_Y_DIST);
-    }
-
-    #if UBL_SEGMENTED
-      static bool line_to_destination_segmented(const feedRate_t &scaled_fr_mm_s);
-    #else
-      static void line_to_destination_cartesian(const feedRate_t &scaled_fr_mm_s, const uint8_t e);
-    #endif
-
-    static inline bool mesh_is_valid() {
-      GRID_LOOP(x, y) if (isnan(z_values[x][y])) return false;
-      return true;
     }
+    return z0;
+  }
+  static inline float get_z_correction(const xy_pos_t &pos) { return get_z_correction(pos.x, pos.y); }
+
+  static inline float mesh_index_to_xpos(const uint8_t i) {
+    return i < GRID_MAX_POINTS_X ? pgm_read_float(&_mesh_index_to_xpos[i]) : MESH_MIN_X + i * (MESH_X_DIST);
+  }
+  static inline float mesh_index_to_ypos(const uint8_t i) {
+    return i < GRID_MAX_POINTS_Y ? pgm_read_float(&_mesh_index_to_ypos[i]) : MESH_MIN_Y + i * (MESH_Y_DIST);
+  }
+
+  #if UBL_SEGMENTED
+    static bool line_to_destination_segmented(const feedRate_t &scaled_fr_mm_s);
+  #else
+    static void line_to_destination_cartesian(const feedRate_t &scaled_fr_mm_s, const uint8_t e);
+  #endif
+
+  static inline bool mesh_is_valid() {
+    GRID_LOOP(x, y) if (isnan(z_values[x][y])) return false;
+    return true;
+  }
 
 }; // class unified_bed_leveling