Merge pull request #6099 from thinkyhead/rc_cleanup_ubl_1

Patch till UBL is integrated with planner-based leveling

Marlin/Conditionals_post.h

   #define ABL_GRID   (ENABLED(AUTO_BED_LEVELING_LINEAR) || ENABLED(AUTO_BED_LEVELING_BILINEAR))
   #define HAS_ABL    (ABL_PLANAR || ABL_GRID || ENABLED(AUTO_BED_LEVELING_UBL))
 
-  #define PLANNER_LEVELING      ((HAS_ABL && DISABLED(AUTO_BED_LEVELING_UBL)) || ENABLED(MESH_BED_LEVELING))
+  #define PLANNER_LEVELING      (HAS_ABL || ENABLED(MESH_BED_LEVELING))
   #define HAS_PROBING_PROCEDURE (HAS_ABL || ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST))
 
   #if HAS_PROBING_PROCEDURE

Marlin/Configuration.h

   #define UBL_PROBE_PT_2_Y 20
   #define UBL_PROBE_PT_3_X 180
   #define UBL_PROBE_PT_3_Y 20
+  #define UBL_MESH_EDIT_ENABLED     // Enable G26 mesh editing
 
 #elif ENABLED(MESH_BED_LEVELING)
 

Marlin/G26_Mesh_Validation_Tool.cpp

  * Marlin Firmware -- G26 - Mesh Validation Tool
  */
 
-#define EXTRUSION_MULTIPLIER 1.0    // This is too much clutter for the main Configuration.h file  But
-#define RETRACTION_MULTIPLIER 1.0   // some user have expressed an interest in being able to customize
-#define NOZZLE 0.3                  // these numbers for thier printer so they don't need to type all
-#define FILAMENT 1.75               // the options every time they do a Mesh Validation Print.
-#define LAYER_HEIGHT 0.2
-#define PRIME_LENGTH 10.0           // So, we put these number in an easy to find and change place.
-#define BED_TEMP 60.0
-#define HOTEND_TEMP 205.0
-#define OOZE_AMOUNT 0.3
-
-#include "Marlin.h"
-#include "Configuration.h"
-#include "planner.h"
-#include "stepper.h"
-#include "temperature.h"
-#include "UBL.h"
-#include "ultralcd.h"
-
-#if ENABLED(AUTO_BED_LEVELING_UBL)
+#include "MarlinConfig.h"
+
+#if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(UBL_MESH_EDIT_ENABLED)
+
+  #include "Marlin.h"
+  #include "Configuration.h"
+  #include "planner.h"
+  #include "stepper.h"
+  #include "temperature.h"
+  #include "UBL.h"
+  #include "ultralcd.h"
+
+  #define EXTRUSION_MULTIPLIER 1.0    // This is too much clutter for the main Configuration.h file  But
+  #define RETRACTION_MULTIPLIER 1.0   // some user have expressed an interest in being able to customize
+  #define NOZZLE 0.3                  // these numbers for thier printer so they don't need to type all
+  #define FILAMENT 1.75               // the options every time they do a Mesh Validation Print.
+  #define LAYER_HEIGHT 0.2
+  #define PRIME_LENGTH 10.0           // So, we put these number in an easy to find and change place.
+  #define BED_TEMP 60.0
+  #define HOTEND_TEMP 205.0
+  #define OOZE_AMOUNT 0.3
 
   #define SIZE_OF_INTERSECTION_CIRCLES 5
   #define SIZE_OF_CROSS_HAIRS 3 // cross hairs inside the circle.  This number should be
 
   /**
    *   Roxy's 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 behaviour 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.
-   *  
+   *
    *   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.
-   *  
+   *
    *   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
-   *  
+   *
    *   N #  Nozzle    Used to control the size of nozzle diameter.  If not specified, a .4mm nozzle is assumed.
-   *  
+   *
    *   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 'cicle' 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.
-   *  
+   *
    *   R #  Random    Randomize the order that the circles are drawn on the bed.  The search for the closest
    *        undrawn cicle is still done.  But the distance to the location for each circle has a
    *        random number of the size specified added to it.  Specifying R50 will give an interesting
    *        deviation from the normal behaviour on a 10 x 10 Mesh.
-   *  
+   *
    *   X #  X coordinate  Specify the starting location of the drawing activity.
-   *  
+   *
    *   Y #  Y coordinate  Specify the starting location of the drawing activity.
    */
 
 
   float valid_trig_angle(float);
   mesh_index_pair find_closest_circle_to_print(float, float);
-  void debug_current_and_destination(char *title);
   void ubl_line_to_destination(const float&, const float&, const float&, const float&, const float&, uint8_t);
   //uint16_t x_splits = 0xFFFF, uint16_t y_splits = 0xFFFF);  /* needed for the old mesh_buffer_line() routine */
 
 
   int8_t prime_flag = 0;
 
-  bool keep_heaters_on = false;
-
-  bool g26_debug_flag = false;
-
-  /**
-   * These support functions allow the use of large bit arrays of flags that take very
-   * little RAM. Currently they are limited to being 16x16 in size. Changing the declaration
-   * to unsigned long will allow us to go to 32x32 if higher resolution Mesh's are needed
-   * in the future.
-   */
-  void bit_clear(uint16_t bits[16], uint8_t x, uint8_t y) { CBI(bits[y], x); }
-  void bit_set(uint16_t bits[16], uint8_t x, uint8_t y) { SBI(bits[y], x); }
-  bool is_bit_set(uint16_t bits[16], uint8_t x, uint8_t y) { return TEST(bits[y], x); }
+  bool keep_heaters_on = false,
+       g26_debug_flag = false;
 
   /**
    * G26: Mesh Validation Pattern generation.
     }
   }
 
-  void debug_current_and_destination(char *title) {
-    float dx, dy, de, xy_dist, fpmm;
-
-    // if the title message starts with a '!' it is so important, we are going to
-    // ignore the status of the g26_debug_flag
-    if (*title != '!' && !g26_debug_flag) return;
-
-    dx = current_position[X_AXIS] - destination[X_AXIS];
-    dy = current_position[Y_AXIS] - destination[Y_AXIS];
-    de = destination[E_AXIS] - current_position[E_AXIS];
-    if (de == 0.0) return;
-
-    xy_dist = HYPOT(dx, dy);
-    if (xy_dist == 0.0) {
-      return;
-      //SERIAL_ECHOPGM("   FPMM=");
-      //fpmm = de;
-      //SERIAL_PROTOCOL_F(fpmm, 6);
-    }
-    else {
-      SERIAL_ECHOPGM("   fpmm=");
-      fpmm = de / xy_dist;
-      SERIAL_ECHO_F(fpmm, 6);
-    }
-
-    SERIAL_ECHOPGM("    current=( ");
-    SERIAL_ECHO_F(current_position[X_AXIS], 6);
-    SERIAL_ECHOPGM(", ");
-    SERIAL_ECHO_F(current_position[Y_AXIS], 6);
-    SERIAL_ECHOPGM(", ");
-    SERIAL_ECHO_F(current_position[Z_AXIS], 6);
-    SERIAL_ECHOPGM(", ");
-    SERIAL_ECHO_F(current_position[E_AXIS], 6);
-    SERIAL_ECHOPGM(" )   destination=( ");
-    if (current_position[X_AXIS] == destination[X_AXIS])
-      SERIAL_ECHOPGM("-------------");
-    else
-      SERIAL_ECHO_F(destination[X_AXIS], 6);
-
-    SERIAL_ECHOPGM(", ");
-
-    if (current_position[Y_AXIS] == destination[Y_AXIS])
-      SERIAL_ECHOPGM("-------------");
-    else
-      SERIAL_ECHO_F(destination[Y_AXIS], 6);
-
-    SERIAL_ECHOPGM(", ");
-
-    if (current_position[Z_AXIS] == destination[Z_AXIS])
-      SERIAL_ECHOPGM("-------------");
-    else
-      SERIAL_ECHO_F(destination[Z_AXIS], 6);
-
-    SERIAL_ECHOPGM(", ");
-
-    if (current_position[E_AXIS] == destination[E_AXIS])
-      SERIAL_ECHOPGM("-------------");
-    else
-      SERIAL_ECHO_F(destination[E_AXIS], 6);
-
-    SERIAL_ECHOPGM(" )   ");
-    SERIAL_ECHO(title);
-    SERIAL_EOL;
-
-    SET_INPUT_PULLUP(66); // Roxy's Left Switch is on pin 66.  Right Switch is on pin 65
-
-    //if (been_to_2_6) {
-    //while ((digitalRead(66) & 0x01) != 0)
-    //  idle();
-    //}
-  }
-
   void move_to(const float &x, const float &y, const float &z, const float &e_delta) {
     float feed_value;
     static float last_z = -999.99;
     return UBL_OK;
   }
 
-#endif // AUTO_BED_LEVELING_UBL
+#endif // AUTO_BED_LEVELING_UBL && UBL_MESH_EDIT_ENABLED

Marlin/Marlin.h

 #include "fastio.h"
 #include "utility.h"
 
-
 #ifdef USBCON
   #include "HardwareSerial.h"
   #if ENABLED(BLUETOOTH)

Marlin/Marlin_main.cpp

 #include "duration_t.h"
 #include "types.h"
 
-#if ENABLED(AUTO_BED_LEVELING_UBL)
-  #include "UBL.h"
-#endif
-
 #if HAS_ABL
   #include "vector_3.h"
   #if ENABLED(AUTO_BED_LEVELING_LINEAR)
 #endif
 
 #if ENABLED(AUTO_BED_LEVELING_UBL)
+  #include "UBL.h"
   unified_bed_leveling ubl;
+#define UBL_MESH_VALID !(   z_values[0][0] == z_values[0][1] && z_values[0][1] == z_values[0][2] \
+                         && z_values[1][0] == z_values[1][1] && z_values[1][1] == z_values[1][2] \
+                         && z_values[2][0] == z_values[2][1] && z_values[2][1] == z_values[2][2] \
+                         && z_values[0][0] == 0 && z_values[1][0] == 0 && z_values[2][0] == 0    \
+                         || isnan(z_values[0][0]))
 #endif
 
 bool Running = true;
       safe_delay(375);
     }
 
-    FORCE_INLINE void set_bltouch_deployed(const bool &deploy) {
+    void set_bltouch_deployed(const bool deploy) {
       bltouch_command(deploy ? BLTOUCH_DEPLOY : BLTOUCH_STOW);
       #if ENABLED(DEBUG_LEVELING_FEATURE)
         if (DEBUGGING(LEVELING)) {
 
 #endif // HAS_BED_PROBE
 
-#if PLANNER_LEVELING || ENABLED(AUTO_BED_LEVELING_UBL)
+#if PLANNER_LEVELING
   /**
    * Turn bed leveling on or off, fixing the current
    * position as-needed.
 
         mbl.set_active(enable && mbl.has_mesh());
 
-        if (enable) planner.unapply_leveling(current_position);
+        if (enable && mbl.has_mesh()) planner.unapply_leveling(current_position);
       }
 
     #elif HAS_ABL && !ENABLED(AUTO_BED_LEVELING_UBL)
           planner.unapply_leveling(current_position);
       }
     #elif ENABLED(AUTO_BED_LEVELING_UBL)
-        ubl.state.active = enable;
+      ubl.state.active = enable;
+      //set_current_from_steppers_for_axis(Z_AXIS);
     #endif
   }
 
  *
  */
 inline void gcode_G28() {
-  #if ENABLED(AUTO_BED_LEVELING_UBL)
-  bool bed_leveling_state_at_entry=0;
-    bed_leveling_state_at_entry = ubl.state.active;
-    set_bed_leveling_enabled(false);
-  #endif
 
   #if ENABLED(DEBUG_LEVELING_FEATURE)
     if (DEBUGGING(LEVELING)) {
   stepper.synchronize();
 
   // Disable the leveling matrix before homing
-  #if PLANNER_LEVELING || ENABLED(MESH_BED_LEVELING)
+  #if PLANNER_LEVELING
+    #if ENABLED(AUTO_BED_LEVELING_UBL)
+      const bool bed_leveling_state_at_entry = ubl.state.active;
+    #endif
     set_bed_leveling_enabled(false);
   #endif
 
 
 #endif // Z_MIN_PROBE_REPEATABILITY_TEST
 
+#if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(UBL_MESH_EDIT_ENABLED)
+
+  inline void gcode_M49() {
+    SERIAL_PROTOCOLPGM("UBL Debug Flag turned ");
+    if ((g26_debug_flag = !g26_debug_flag))
+      SERIAL_PROTOCOLLNPGM("on.");
+    else
+      SERIAL_PROTOCOLLNPGM("off.");
+  }
+
+#endif // AUTO_BED_LEVELING_UBL && UBL_MESH_EDIT_ENABLED
+
 /**
  * M75: Start print timer
  */
           break;
       #endif // INCH_MODE_SUPPORT
 
-      #if ENABLED(AUTO_BED_LEVELING_UBL)
+      #if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(UBL_MESH_EDIT_ENABLED)
         case 26: // G26: Mesh Validation Pattern generation
           gcode_G26();
           break;
         gcode_G28();
         break;
 
-      #if PLANNER_LEVELING || HAS_ABL
+      #if PLANNER_LEVELING
         case 29: // G29 Detailed Z probe, probes the bed at 3 or more points,
                  // or provides access to the UBL System if enabled.
           gcode_G29();
           break;
       #endif // Z_MIN_PROBE_REPEATABILITY_TEST
 
-      #if ENABLED(AUTO_BED_LEVELING_UBL)
+      #if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(UBL_MESH_EDIT_ENABLED)
         case 49: // M49: Turn on or off g26_debug_flag for verbose output
-    if (g26_debug_flag) {
-            SERIAL_PROTOCOLPGM("UBL Debug Flag turned off.\n");
-            g26_debug_flag = 0; }
-    else {
-            SERIAL_PROTOCOLPGM("UBL Debug Flag turned on.\n");
-            g26_debug_flag++; }
+          gcode_M49();
           break;
-      #endif // Z_MIN_PROBE_REPEATABILITY_TEST
+      #endif // AUTO_BED_LEVELING_UBL && UBL_MESH_EDIT_ENABLED
 
       case 75: // M75: Start print timer
         gcode_M75(); break;
  */
 void set_current_from_steppers_for_axis(const AxisEnum axis) {
   get_cartesian_from_steppers();
-  #if PLANNER_LEVELING
+  #if PLANNER_LEVELING && DISABLED(AUTO_BED_LEVELING_UBL)
     planner.unapply_leveling(cartes);
   #endif
   if (axis == ALL_AXES)
     float normalized_dist, end[XYZE];
 
     // Split at the left/front border of the right/top square
-    int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2);
+    const int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2);
     if (cx2 != cx1 && TEST(x_splits, gcx)) {
       COPY(end, destination);
       destination[X_AXIS] = LOGICAL_X_POSITION(mbl.get_probe_x(gcx));
     float normalized_dist, end[XYZE];
 
     // Split at the left/front border of the right/top square
-    int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2);
+    const int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2);
     if (cx2 != cx1 && TEST(x_splits, gcx)) {
       COPY(end, destination);
       destination[X_AXIS] = LOGICAL_X_POSITION(bilinear_start[X_AXIS] + ABL_BG_SPACING(X_AXIS) * gcx);
 
 float calculate_volumetric_multiplier(float diameter) {
   if (!volumetric_enabled || diameter == 0) return 1.0;
-  return 1.0 / (M_PI * diameter * 0.5 * diameter * 0.5);
+  return 1.0 / (M_PI * sq(diameter * 0.5));
 }
 
 void calculate_volumetric_multipliers() {

Marlin/UBL.h

 
 #include "Marlin.h"
 #include "math.h"
+#include "vector_3.h"
 
 #ifndef UNIFIED_BED_LEVELING_H
 #define UNIFIED_BED_LEVELING_H
     #define UBL_ERR true
 
     typedef struct {
-      int x_index, y_index;
-      float distance; // Not always used. But when populated, it is the distance
-                      // from the search location
+      int8_t x_index, y_index;
+      float distance; // When populated, the distance from the search location
     } mesh_index_pair;
 
-    typedef struct { double dx, dy, dz; } vector;
-
     enum MeshPointType { INVALID, REAL, SET_IN_BITMAP };
 
     bool axis_unhomed_error(bool, bool, bool);
-    void dump(char *str, float f);
+    void dump(char * const str, const float &f);
     bool ubl_lcd_clicked();
-    void probe_entire_mesh(float, float, bool, bool);
+    void probe_entire_mesh(const float&, const float&, const bool, const bool);
+    void debug_current_and_destination(char *title);
     void ubl_line_to_destination(const float&, const float&, const float&, const float&, const float&, uint8_t);
-    void manually_probe_remaining_mesh(float, float, float, float, bool);
-    vector tilt_mesh_based_on_3pts(float, float, float);
-    void new_set_bed_level_equation_3pts(float, float, float);
-    float measure_business_card_thickness(float);
-    mesh_index_pair find_closest_mesh_point_of_type(MeshPointType, float, float, bool, unsigned int[16]);
+    void manually_probe_remaining_mesh(const float&, const float&, const float&, const float&, const bool);
+    vector_3 tilt_mesh_based_on_3pts(const float&, const float&, const float&);
+    float measure_business_card_thickness(const float&);
+    mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const float&, const float&, const bool, unsigned int[16]);
     void find_mean_mesh_height();
     void shift_mesh_height();
     bool g29_parameter_parsing();
     void g29_what_command();
     void g29_eeprom_dump();
     void g29_compare_current_mesh_to_stored_mesh();
-    void fine_tune_mesh(float, float, bool);
+    void fine_tune_mesh(const float&, const float&, const bool);
     void bit_clear(uint16_t bits[16], uint8_t x, uint8_t y);
     void bit_set(uint16_t bits[16], uint8_t x, uint8_t y);
     bool is_bit_set(uint16_t bits[16], uint8_t x, uint8_t y);
     #define MESH_X_DIST ((float(UBL_MESH_MAX_X) - float(UBL_MESH_MIN_X)) / (float(UBL_MESH_NUM_X_POINTS) - 1.0))
     #define MESH_Y_DIST ((float(UBL_MESH_MAX_Y) - float(UBL_MESH_MIN_Y)) / (float(UBL_MESH_NUM_Y_POINTS) - 1.0))
 
-    extern bool g26_debug_flag;
+    #if ENABLED(UBL_MESH_EDIT_ENABLED)
+      extern bool g26_debug_flag;
+    #else
+      constexpr bool g26_debug_flag = false;
+    #endif
     extern float last_specified_z;
     extern float fade_scaling_factor_for_current_height;
     extern float z_values[UBL_MESH_NUM_X_POINTS][UBL_MESH_NUM_Y_POINTS];
               mesh_x_max = UBL_MESH_MAX_X,
               mesh_y_max = UBL_MESH_MAX_Y,
               mesh_x_dist = MESH_X_DIST,
-              mesh_y_dist = MESH_Y_DIST,
-              g29_correction_fade_height = 10.0,
-              g29_fade_height_multiplier = 1.0 / 10.0; // It is cheaper to do a floating point multiply than a floating
-                                                       // point divide. So, we keep this number in both forms. The first
-                                                       // is for the user. The second one is the one that is actually used
-                                                       // again and again and again during the correction calculations.
+              mesh_y_dist = MESH_Y_DIST;
+
+        #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+          float g29_correction_fade_height = 10.0,
+                g29_fade_height_multiplier = 1.0 / 10.0; // It is cheaper to do a floating point multiply than a floating
+                                                         // point divide. So, we keep this number in both forms. The first
+                                                         // is for the user. The second one is the one that is actually used
+                                                         // again and again and again during the correction calculations.
+        #endif
 
         unsigned char padding[24];  // This is just to allow room to add state variables without
                                     // changing the location of data structures in the EEPROM.
       unified_bed_leveling();
       //  ~unified_bed_leveling();  // No destructor because this object never goes away!
 
-      void display_map(int);
+      void display_map(const int);
 
       void reset();
       void invalidate();
 
       void store_state();
       void load_state();
-      void store_mesh(int);
-      void load_mesh(int);
+      void store_mesh(const int16_t);
+      void load_mesh(const int16_t);
 
       bool sanity_check();
 
-      FORCE_INLINE float map_x_index_to_bed_location(int8_t i){ return ((float) UBL_MESH_MIN_X) + (((float) MESH_X_DIST) * (float) i); };
-      FORCE_INLINE float map_y_index_to_bed_location(int8_t i){ return ((float) UBL_MESH_MIN_Y) + (((float) MESH_Y_DIST) * (float) i); };
+      FORCE_INLINE static float map_x_index_to_bed_location(const int8_t i) { return ((float) UBL_MESH_MIN_X) + (((float) MESH_X_DIST) * (float) i); };
+      FORCE_INLINE static float map_y_index_to_bed_location(const int8_t i) { return ((float) UBL_MESH_MIN_Y) + (((float) MESH_Y_DIST) * (float) i); };
 
-      void set_z(const int8_t px, const int8_t py, const float z) { z_values[px][py] = z; }
+      FORCE_INLINE void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; }
 
-      int8_t get_cell_index_x(float x) {
-        int8_t cx = (x - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST));
+      static int8_t get_cell_index_x(const float &x) {
+        const int8_t cx = (x - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST));
         return constrain(cx, 0, (UBL_MESH_NUM_X_POINTS) - 1);   // -1 is appropriate if we want all movement to the X_MAX
       }                                                         // position. But with this defined this way, it is possible
                                                                 // to extrapolate off of this point even further out. Probably
                                                                 // that is OK because something else should be keeping that from
                                                                 // happening and should not be worried about at this level.
-      int8_t get_cell_index_y(float y) {
-        int8_t cy = (y - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_Y_DIST));
+      static int8_t get_cell_index_y(const float &y) {
+        const int8_t cy = (y - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_Y_DIST));
         return constrain(cy, 0, (UBL_MESH_NUM_Y_POINTS) - 1);   // -1 is appropriate if we want all movement to the Y_MAX
       }                                                         // position. But with this defined this way, it is possible
                                                                 // to extrapolate off of this point even further out. Probably
                                                                 // that is OK because something else should be keeping that from
                                                                 // happening and should not be worried about at this level.
 
-      int8_t find_closest_x_index(float x) {
-        int8_t px = (x - (UBL_MESH_MIN_X) + (MESH_X_DIST) * 0.5) * (1.0 / (MESH_X_DIST));
+      static int8_t find_closest_x_index(const float &x) {
+        const int8_t px = (x - (UBL_MESH_MIN_X) + (MESH_X_DIST) * 0.5) * (1.0 / (MESH_X_DIST));
         return (px >= 0 && px < (UBL_MESH_NUM_X_POINTS)) ? px : -1;
       }
 
-      int8_t find_closest_y_index(float y) {
-        int8_t py = (y - (UBL_MESH_MIN_Y) + (MESH_Y_DIST) * 0.5) * (1.0 / (MESH_Y_DIST));
+      static int8_t find_closest_y_index(const float &y) {
+        const int8_t py = (y - (UBL_MESH_MIN_Y) + (MESH_Y_DIST) * 0.5) * (1.0 / (MESH_Y_DIST));
         return (py >= 0 && py < (UBL_MESH_NUM_Y_POINTS)) ? py : -1;
       }
 
        *    |<---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
+       *  find the expected Z Height at a position between two known Z-Height locations.
        *
-       *  It is farly expensive with its 4 floating point additions and 2 floating point
+       *  It is fairly expensive with its 4 floating point additions and 2 floating point
        *  multiplications.
        */
-      inline float calc_z0(float a0, float a1, float z1, float a2, float z2) {
-        float delta_z = (z2 - z1);
-        float delta_a = (a0 - a1) / (a2 - a1);
+      static FORCE_INLINE float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) {
+        const float delta_z = (z2 - z1),
+                    delta_a = (a0 - a1) / (a2 - a1);
         return z1 + delta_a * delta_z;
       }
 
        * the X index of the x0 intersection available and we don't want to perform any extra floating
        * point operations.
        */
-      inline float get_z_correction_along_horizontal_mesh_line_at_specific_X(float x0, int x1_i, int yi) {
+      inline float get_z_correction_along_horizontal_mesh_line_at_specific_X(const float &x0, const int x1_i, const int yi) {
         if (x1_i < 0 || yi < 0 || x1_i >= UBL_MESH_NUM_X_POINTS || yi >= UBL_MESH_NUM_Y_POINTS) {
           SERIAL_ECHOPAIR("? in get_z_correction_along_horizontal_mesh_line_at_specific_X(x0=", x0);
           SERIAL_ECHOPAIR(",x1_i=", x1_i);
           return NAN;
         }
 
-        const float a0ma1diva2ma1 = (x0 - mesh_index_to_x_location[x1_i]) * (1.0 / (MESH_X_DIST)),
+        const float xratio = (RAW_X_POSITION(x0) - mesh_index_to_x_location[x1_i]) * (1.0 / (MESH_X_DIST)),
                     z1 = z_values[x1_i][yi],
                     z2 = z_values[x1_i + 1][yi],
                     dz = (z2 - z1);
 
-        return z1 + a0ma1diva2ma1 * dz;
+        return z1 + xratio * dz;
       }
 
       //
       // See comments above for get_z_correction_along_horizontal_mesh_line_at_specific_X
       //
-      inline float get_z_correction_along_vertical_mesh_line_at_specific_Y(float y0, int xi, int y1_i) {
+      inline float get_z_correction_along_vertical_mesh_line_at_specific_Y(const float &y0, const int xi, const int y1_i) {
         if (xi < 0 || y1_i < 0 || xi >= UBL_MESH_NUM_X_POINTS || y1_i >= UBL_MESH_NUM_Y_POINTS) {
           SERIAL_ECHOPAIR("? in get_z_correction_along_vertical_mesh_line_at_specific_X(y0=", y0);
           SERIAL_ECHOPAIR(", x1_i=", xi);
           return NAN;
         }
 
-        const float a0ma1diva2ma1 = (y0 - mesh_index_to_y_location[y1_i]) * (1.0 / (MESH_Y_DIST)),
+        const float yratio = (RAW_Y_POSITION(y0) - mesh_index_to_y_location[y1_i]) * (1.0 / (MESH_Y_DIST)),
                     z1 = z_values[xi][y1_i],
                     z2 = z_values[xi][y1_i + 1],
                     dz = (z2 - z1);
 
-        return z1 + a0ma1diva2ma1 * dz;
+        return z1 + yratio * dz;
       }
 
       /**
        * Z-Height at both ends. Then it does a linear interpolation of these heights based
        * on the Y position within the cell.
        */
-      float get_z_correction(float x0, float y0) {
-        int8_t cx = get_cell_index_x(x0),
-        cy = get_cell_index_y(y0);
+      float get_z_correction(const float &x0, const float &y0) const {
+        const int8_t cx = get_cell_index_x(RAW_X_POSITION(x0)),
+                     cy = get_cell_index_y(RAW_Y_POSITION(y0));
 
         if (cx < 0 || cy < 0 || cx >= UBL_MESH_NUM_X_POINTS || cy >= UBL_MESH_NUM_Y_POINTS) {
 
           return 0.0; // this used to return state.z_offset
         }
 
-        float z1 = calc_z0(x0,
-          map_x_index_to_bed_location(cx), z_values[cx][cy],
-          map_x_index_to_bed_location(cx + 1), z_values[cx + 1][cy]);
-        float z2 = calc_z0(x0,
-          map_x_index_to_bed_location(cx), z_values[cx][cy + 1],
-          map_x_index_to_bed_location(cx + 1), z_values[cx + 1][cy + 1]);
-        float z0 = calc_z0(y0,
-          map_y_index_to_bed_location(cy), z1,
-          map_y_index_to_bed_location(cy + 1), z2);
+        const float z1 = calc_z0(RAW_X_POSITION(x0),
+                      map_x_index_to_bed_location(cx), z_values[cx][cy],
+                      map_x_index_to_bed_location(cx + 1), z_values[cx + 1][cy]),
+                    z2 = calc_z0(RAW_X_POSITION(x0),
+                      map_x_index_to_bed_location(cx), z_values[cx][cy + 1],
+                      map_x_index_to_bed_location(cx + 1), z_values[cx + 1][cy + 1]);
+              float z0 = calc_z0(RAW_Y_POSITION(y0),
+                  map_y_index_to_bed_location(cy), z1,
+                  map_y_index_to_bed_location(cy + 1), z2);
 
         #if ENABLED(DEBUG_LEVELING_FEATURE)
           if (DEBUGGING(MESH_ADJUST)) {
        * factor is going to be the same as the last time the function calculated a value. If so, it just
        * returns it.
        *
-       * If it must do a calcuation, it will return a scaling factor of 0.0 if the UBL System is not active
-       * or if the current Z Height is past the specified 'Fade Height'
+       * It returns a scaling factor of 1.0 if UBL is inactive.
+       * It returns a scaling factor of 0.0 if Z is past the specified 'Fade Height'
        */
-      FORCE_INLINE float fade_scaling_factor_for_z(float current_z) {
-      #ifndef ENABLE_LEVELING_FADE_HEIGHT   // if turned off, just return 0.000    Note that we assume the
-        return 0.000;                       // compiler will do 'Dead Code' elimination so there is no need
-      #endif                                // for an #else clause here.
-        if (last_specified_z == current_z)
+      #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+
+        FORCE_INLINE float fade_scaling_factor_for_z(const float &lz) const {
+          const float rz = RAW_Z_POSITION(lz);
+          if (last_specified_z != rz) {
+            last_specified_z = rz;
+            fade_scaling_factor_for_current_height =
+              state.active && rz < state.g29_correction_fade_height
+                ? 1.0 - (rz * state.g29_fade_height_multiplier)
+                : 0.0;
+          }
           return fade_scaling_factor_for_current_height;
+        }
 
-        last_specified_z = current_z;
-        fade_scaling_factor_for_current_height =
-          state.active && current_z < state.g29_correction_fade_height
-          ? 1.0 - (current_z * state.g29_fade_height_multiplier)
-          : 0.0;
-        return fade_scaling_factor_for_current_height;
-      }
-    };
+      #else
+
+        static constexpr float fade_scaling_factor_for_z(const float &lz) { UNUSED(lz); return 1.0; }
+
+      #endif
+
+    }; // class unified_bed_leveling
 
     extern unified_bed_leveling ubl;
     extern int ubl_eeprom_start;
 
-#endif // AUTO_BED_LEVELING_UBL
+    #define UBL_LAST_EEPROM_INDEX (E2END - sizeof(unified_bed_leveling::state))
+
+  #endif // AUTO_BED_LEVELING_UBL
+
 #endif // UNIFIED_BED_LEVELING_H

Marlin/UBL_Bed_Leveling.cpp

 #include "math.h"
 
 #if ENABLED(AUTO_BED_LEVELING_UBL)
+
   #include "UBL.h"
   #include "hex_print_routines.h"
 
   /**
+   * These support functions allow the use of large bit arrays of flags that take very
+   * little RAM. Currently they are limited to being 16x16 in size. Changing the declaration
+   * to unsigned long will allow us to go to 32x32 if higher resolution Mesh's are needed
+   * in the future.
+   */
+  void bit_clear(uint16_t bits[16], uint8_t x, uint8_t y) { CBI(bits[y], x); }
+  void bit_set(uint16_t bits[16], uint8_t x, uint8_t y) { SBI(bits[y], x); }
+  bool is_bit_set(uint16_t bits[16], uint8_t x, uint8_t y) { return TEST(bits[y], x); }
+
+  /**
    * These variables used to be declared inside the unified_bed_leveling class. We are going to
    * still declare them within the .cpp file for bed leveling. But there is only one instance of
    * the bed leveling object and we can get rid of a level of inderection by not making them
   }
 
   void unified_bed_leveling::store_state() {
-    int k = E2END - sizeof(ubl.state);
-    eeprom_write_block((void *)&ubl.state, (void *)k, sizeof(ubl.state));
+    const uint16_t i = UBL_LAST_EEPROM_INDEX;
+    eeprom_write_block((void *)&ubl.state, (void *)i, sizeof(state));
   }
 
   void unified_bed_leveling::load_state() {
-    int k = E2END - sizeof(ubl.state);
-    eeprom_read_block((void *)&ubl.state, (void *)k, sizeof(ubl.state));
+    const uint16_t i = UBL_LAST_EEPROM_INDEX;
+    eeprom_read_block((void *)&ubl.state, (void *)i, sizeof(state));
 
     if (sanity_check())
       SERIAL_PROTOCOLLNPGM("?In load_state() sanity_check() failed.\n");
 
-    /**
-     * These lines can go away in a few weeks.  They are just
-     * to make sure people updating thier firmware won't be using
-     * an incomplete Bed_Leveling.state structure. For speed
-     * we now multiply by the inverse of the Fade Height instead of
-     * dividing by it. Soon... all of the old structures will be
-     * updated, but until then, we try to ease the transition
-     * for our Beta testers.
-     */
-    if (ubl.state.g29_fade_height_multiplier != 1.0 / ubl.state.g29_correction_fade_height) {
-      ubl.state.g29_fade_height_multiplier = 1.0 / ubl.state.g29_correction_fade_height;
-      store_state();
-    }
-
+    #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+      /**
+       * These lines can go away in a few weeks.  They are just
+       * to make sure people updating thier firmware won't be using
+       * an incomplete Bed_Leveling.state structure. For speed
+       * we now multiply by the inverse of the Fade Height instead of
+       * dividing by it. Soon... all of the old structures will be
+       * updated, but until then, we try to ease the transition
+       * for our Beta testers.
+       */
+      if (ubl.state.g29_fade_height_multiplier != 1.0 / ubl.state.g29_correction_fade_height) {
+        ubl.state.g29_fade_height_multiplier = 1.0 / ubl.state.g29_correction_fade_height;
+        store_state();
+      }
+    #endif
   }
 
-  void unified_bed_leveling::load_mesh(int m) {
-    int k = E2END - sizeof(ubl.state),
-        j = (k - ubl_eeprom_start) / sizeof(z_values);
+  void unified_bed_leveling::load_mesh(const int16_t m) {
+    int16_t j = (UBL_LAST_EEPROM_INDEX - ubl_eeprom_start) / sizeof(z_values);
 
     if (m == -1) {
       SERIAL_PROTOCOLLNPGM("?No mesh saved in EEPROM. Zeroing mesh in memory.\n");
       return;
     }
 
-    j = k - (m + 1) * sizeof(z_values);
+    j = UBL_LAST_EEPROM_INDEX - (m + 1) * sizeof(z_values);
     eeprom_read_block((void *)&z_values , (void *)j, sizeof(z_values));
 
     SERIAL_PROTOCOLPGM("Mesh loaded from slot ");
     SERIAL_EOL;
   }
 
-  void unified_bed_leveling:: store_mesh(int m) {
-    int k = E2END - sizeof(state),
-        j = (k - ubl_eeprom_start) / sizeof(z_values);
+  void unified_bed_leveling::store_mesh(const int16_t m) {
+    int16_t j = (UBL_LAST_EEPROM_INDEX - ubl_eeprom_start) / sizeof(z_values);
 
     if (m < 0 || m >= j || ubl_eeprom_start <= 0) {
       SERIAL_PROTOCOLLNPGM("?EEPROM storage not available to load mesh.\n");
       SERIAL_PROTOCOL(m);
       SERIAL_PROTOCOLLNPGM(" mesh slots available.\n");
       SERIAL_PROTOCOLLNPAIR("E2END     : ", E2END);
-      SERIAL_PROTOCOLLNPAIR("k         : ", k);
+      SERIAL_PROTOCOLLNPAIR("k         : ", (int)UBL_LAST_EEPROM_INDEX);
       SERIAL_PROTOCOLLNPAIR("j         : ", j);
       SERIAL_PROTOCOLLNPAIR("m         : ", m);
       SERIAL_EOL;
       return;
     }
 
-    j = k - (m + 1) * sizeof(z_values);
+    j = UBL_LAST_EEPROM_INDEX - (m + 1) * sizeof(z_values);
     eeprom_write_block((const void *)&z_values, (void *)j, sizeof(z_values));
 
     SERIAL_PROTOCOLPGM("Mesh saved in slot ");
         z_values[x][y] = NAN;
   }
 
-  void unified_bed_leveling::display_map(int map_type) {
+  void unified_bed_leveling::display_map(const int map_type) {
     float f, current_xi, current_yi;
     int8_t i, j;
     UNUSED(map_type);
       error_flag++;
     }
 
-    const int k = E2END - sizeof(ubl.state),
-              j = (k - ubl_eeprom_start) / sizeof(z_values);
-
+    const int j = (UBL_LAST_EEPROM_INDEX - ubl_eeprom_start) / sizeof(z_values);
     if (j < 1) {
       SERIAL_PROTOCOLLNPGM("?No EEPROM storage available for a mesh of this size.\n");
       error_flag++;

Marlin/UBL_G29.cpp

  *
  */
 
-#include "Marlin.h"
+#include "MarlinConfig.h"
+
 #if ENABLED(AUTO_BED_LEVELING_UBL)
   //#include "vector_3.h"
   //#include "qr_solve.h"
 
   #include "UBL.h"
+  #include "Marlin.h"
   #include "hex_print_routines.h"
   #include "configuration_store.h"
   #include "planner.h"
   #define DEPLOY_PROBE() set_probe_deployed(true)
   #define STOW_PROBE() set_probe_deployed(false)
   bool ProbeStay = true;
-  float ubl_3_point_1_X = UBL_PROBE_PT_1_X;
-  float ubl_3_point_1_Y = UBL_PROBE_PT_1_Y;
-  float ubl_3_point_2_X = UBL_PROBE_PT_2_X;
-  float ubl_3_point_2_Y = UBL_PROBE_PT_2_Y;
-  float ubl_3_point_3_X = UBL_PROBE_PT_3_X;
-  float ubl_3_point_3_Y = UBL_PROBE_PT_3_Y;
+
+  constexpr float ubl_3_point_1_X = UBL_PROBE_PT_1_X,
+                  ubl_3_point_1_Y = UBL_PROBE_PT_1_Y,
+                  ubl_3_point_2_X = UBL_PROBE_PT_2_X,
+                  ubl_3_point_2_Y = UBL_PROBE_PT_2_Y,
+                  ubl_3_point_3_X = UBL_PROBE_PT_3_X,
+                  ubl_3_point_3_Y = UBL_PROBE_PT_3_Y;
 
   #define SIZE_OF_LITTLE_RAISE 0
   #define BIG_RAISE_NOT_NEEDED 0
   volatile uint8_t ubl_encoderDiff = 0; // Volatile because it's changed by Temperature ISR button update
 
   // The simple parameter flags and values are 'static' so parameter parsing can be in a support routine.
-  static int g29_verbose_level = 0, test_value = 0,
-             phase_value = -1, repetition_cnt = 1;
+  static int g29_verbose_level = 0, phase_value = -1, repetition_cnt = 1,
+             storage_slot = 0, test_pattern = 0;
   static bool repeat_flag = UBL_OK, c_flag = false, x_flag = UBL_OK, y_flag = UBL_OK, statistics_flag = UBL_OK, business_card_mode = false;
   static float x_pos = 0.0, y_pos = 0.0, height_value = 5.0, measured_z, card_thickness = 0.0, constant = 0.0;
-  static int storage_slot = 0, test_pattern = 0;
 
   #if ENABLED(ULTRA_LCD)
     void lcd_setstatus(const char* message, bool persist);
   #endif
 
   void gcode_G29() {
-    mesh_index_pair location;
-    int j, k;
     float Z1, Z2, Z3;
 
     g29_verbose_level = 0;  // These may change, but let's get some reasonable values into them.
     if (code_seen('I')) {
       repetition_cnt = code_has_value() ? code_value_int() : 1;
       while (repetition_cnt--) {
-        location = find_closest_mesh_point_of_type(REAL, x_pos, y_pos, 0, NULL);  // The '0' says we want to use the nozzle's position
+        const mesh_index_pair location = find_closest_mesh_point_of_type(REAL, x_pos, y_pos, 0, NULL);  // The '0' says we want to use the nozzle's position
         if (location.x_index < 0) {
           SERIAL_PROTOCOLLNPGM("Entire Mesh invalidated.\n");
           break;            // No more invalid Mesh Points to populate
             SERIAL_ECHOPAIR(",", y_pos);
             SERIAL_PROTOCOLLNPGM(")\n");
           }
-          probe_entire_mesh( x_pos+X_PROBE_OFFSET_FROM_EXTRUDER, y_pos+Y_PROBE_OFFSET_FROM_EXTRUDER,
-                             code_seen('O') || code_seen('M'), code_seen('E'));
+          probe_entire_mesh(x_pos + X_PROBE_OFFSET_FROM_EXTRUDER, y_pos + Y_PROBE_OFFSET_FROM_EXTRUDER,
+                            code_seen('O') || code_seen('M'), code_seen('E'));
           break;
         //
         // Manually Probe Mesh in areas that can not be reached by the probe
           // If no repetition is specified, do the whole Mesh
           if (!repeat_flag) repetition_cnt = 9999;
           while (repetition_cnt--) {
-            location = find_closest_mesh_point_of_type(INVALID, x_pos, y_pos, 0, NULL); // The '0' says we want to use the nozzle's position
+            const mesh_index_pair location = find_closest_mesh_point_of_type(INVALID, x_pos, y_pos, 0, NULL); // The '0' says we want to use the nozzle's position
             if (location.x_index < 0) break; // No more invalid Mesh Points to populate
             z_values[location.x_index][location.y_index] = height_value;
           }
     if (code_seen('L')) {     // Load Current Mesh Data
       storage_slot = code_has_value() ? code_value_int() : ubl.state.eeprom_storage_slot;
 
-      k = E2END - sizeof(ubl.state);
-      j = (k - ubl_eeprom_start) / sizeof(z_values);
+      const int16_t j = (UBL_LAST_EEPROM_INDEX - ubl_eeprom_start) / sizeof(z_values);
 
       if (storage_slot < 0 || storage_slot >= j || ubl_eeprom_start <= 0) {
         SERIAL_PROTOCOLLNPGM("?EEPROM storage not available for use.\n");
         return;
       }
 
-      int k = E2END - sizeof(ubl.state),
-          j = (k - ubl_eeprom_start) / sizeof(z_values);
+      const int16_t j = (UBL_LAST_EEPROM_INDEX - ubl_eeprom_start) / sizeof(z_values);
 
       if (storage_slot < 0 || storage_slot >= j || ubl_eeprom_start <= 0) {
         SERIAL_PROTOCOLLNPGM("?EEPROM storage not available for use.\n");
             z_values[x][y] -= mean + constant;
   }
 
-  void shift_mesh_height( ) {
+  void shift_mesh_height() {
     for (uint8_t x = 0; x < UBL_MESH_NUM_X_POINTS; x++)
       for (uint8_t y = 0; y < UBL_MESH_NUM_Y_POINTS; y++)
         if (!isnan(z_values[x][y]))
    * Probe all invalidated locations of the mesh that can be reached by the probe.
    * This attempts to fill in locations closest to the nozzle's start location first.
    */
-  void probe_entire_mesh(float x_pos, float y_pos, bool do_ubl_mesh_map, bool stow_probe) {
+  void probe_entire_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map, const bool stow_probe) {
     mesh_index_pair location;
-    float xProbe, yProbe, measured_z;
 
     ubl_has_control_of_lcd_panel++;
     save_ubl_active_state_and_disable();   // we don't do bed level correction because we want the raw data when we probe
         restore_ubl_active_state_and_leave();
         return;
       }
-      location = find_closest_mesh_point_of_type(INVALID, x_pos, y_pos, 1, NULL);  // the '1' says we want the location to be relative to the probe
+
+      location = find_closest_mesh_point_of_type(INVALID, lx, ly, 1, NULL);  // the '1' says we want the location to be relative to the probe
       if (location.x_index >= 0 && location.y_index >= 0) {
-        xProbe = ubl.map_x_index_to_bed_location(location.x_index);
-        yProbe = ubl.map_y_index_to_bed_location(location.y_index);
+        const float xProbe = ubl.map_x_index_to_bed_location(location.x_index),
+                    yProbe = ubl.map_y_index_to_bed_location(location.y_index);
         if (xProbe < MIN_PROBE_X || xProbe > MAX_PROBE_X || yProbe < MIN_PROBE_Y || yProbe > MAX_PROBE_Y) {
           SERIAL_PROTOCOLLNPGM("?Error: Attempt to probe off the bed.");
           ubl_has_control_of_lcd_panel = false;
           goto LEAVE;
         }
-        measured_z = probe_pt(xProbe, yProbe, stow_probe, g29_verbose_level);
+        const float measured_z = probe_pt(xProbe, yProbe, stow_probe, g29_verbose_level);
         z_values[location.x_index][location.y_index] = measured_z + Z_PROBE_OFFSET_FROM_EXTRUDER;
       }
 
       if (do_ubl_mesh_map) ubl.display_map(1);
+
     } while (location.x_index >= 0 && location.y_index >= 0);
 
     LEAVE:
     STOW_PROBE();
     restore_ubl_active_state_and_leave();
 
-    x_pos = constrain(x_pos - (X_PROBE_OFFSET_FROM_EXTRUDER), X_MIN_POS, X_MAX_POS);
-    y_pos = constrain(y_pos - (Y_PROBE_OFFSET_FROM_EXTRUDER), Y_MIN_POS, Y_MAX_POS);
-
-    do_blocking_move_to_xy(x_pos, y_pos);
+    do_blocking_move_to_xy(
+      constrain(lx - (X_PROBE_OFFSET_FROM_EXTRUDER), X_MIN_POS, X_MAX_POS),
+      constrain(ly - (Y_PROBE_OFFSET_FROM_EXTRUDER), Y_MIN_POS, Y_MAX_POS)
+    );
   }
 
-  vector tilt_mesh_based_on_3pts(float pt1, float pt2, float pt3) {
-    vector v1, v2, normal;
+  vector_3 tilt_mesh_based_on_3pts(const float &pt1, const float &pt2, const float &pt3) {
     float c, d, t;
     int i, j;
 
-    v1.dx = (ubl_3_point_1_X - ubl_3_point_2_X);
-    v1.dy = (ubl_3_point_1_Y - ubl_3_point_2_Y);
-    v1.dz = (pt1 - pt2);
-
-    v2.dx = (ubl_3_point_3_X - ubl_3_point_2_X);
-    v2.dy = (ubl_3_point_3_Y - ubl_3_point_2_Y);
-    v2.dz = (pt3 - pt2);
+    vector_3 v1 = vector_3( (ubl_3_point_1_X - ubl_3_point_2_X),
+                            (ubl_3_point_1_Y - ubl_3_point_2_Y),
+                            (pt1 - pt2) ),
 
-    // do cross product
+             v2 = vector_3( (ubl_3_point_3_X - ubl_3_point_2_X),
+                            (ubl_3_point_3_Y - ubl_3_point_2_Y),
+                            (pt3 - pt2) ),
 
-    normal.dx = v1.dy * v2.dz - v1.dz * v2.dy;
-    normal.dy = v1.dz * v2.dx - v1.dx * v2.dz;
-    normal.dz = v1.dx * v2.dy - v1.dy * v2.dx;
+             normal = vector_3::cross(v1, v2);
 
-    // printf("[%f,%f,%f]    ", normal.dx, normal.dy, normal.dz);
+    // printf("[%f,%f,%f]    ", normal.x, normal.y, normal.z);
 
     /**
      * This code does two things. This vector is normal to the tilted plane.
      * We also need Z to be unity because we are going to be treating this triangle
      * as the sin() and cos() of the bed's tilt
      */
-    normal.dx /= normal.dz;
-    normal.dy /= normal.dz;
-    normal.dz /= normal.dz;
+    const float inv_z = 1.0 / normal.z;
+    normal.x *= inv_z;
+    normal.y *= inv_z;
+    normal.z = 1.0;
 
     //
     // All of 3 of these points should give us the same d constant
     //
-    t = normal.dx * ubl_3_point_1_X + normal.dy * ubl_3_point_1_Y;
-    d = t + normal.dz * pt1;
+    t = normal.x * ubl_3_point_1_X + normal.y * ubl_3_point_1_Y;
+    d = t + normal.z * pt1;
     c = d - t;
     SERIAL_ECHOPGM("d from 1st point: ");
     SERIAL_ECHO_F(d, 6);
     SERIAL_ECHOPGM("  c: ");
     SERIAL_ECHO_F(c, 6);
     SERIAL_EOL;
-    t = normal.dx * ubl_3_point_2_X + normal.dy * ubl_3_point_2_Y;
-    d = t + normal.dz * pt2;
+    t = normal.x * ubl_3_point_2_X + normal.y * ubl_3_point_2_Y;
+    d = t + normal.z * pt2;
     c = d - t;
     SERIAL_ECHOPGM("d from 2nd point: ");
     SERIAL_ECHO_F(d, 6);
     SERIAL_ECHOPGM("  c: ");
     SERIAL_ECHO_F(c, 6);
     SERIAL_EOL;
-    t = normal.dx * ubl_3_point_3_X + normal.dy * ubl_3_point_3_Y;
-    d = t + normal.dz * pt3;
+    t = normal.x * ubl_3_point_3_X + normal.y * ubl_3_point_3_Y;
+    d = t + normal.z * pt3;
     c = d - t;
     SERIAL_ECHOPGM("d from 3rd point: ");
     SERIAL_ECHO_F(d, 6);
 
     for (i = 0; i < UBL_MESH_NUM_X_POINTS; i++) {
       for (j = 0; j < UBL_MESH_NUM_Y_POINTS; j++) {
-        c = -((normal.dx * (UBL_MESH_MIN_X + i * (MESH_X_DIST)) + normal.dy * (UBL_MESH_MIN_Y + j * (MESH_Y_DIST))) - d);
+        c = -((normal.x * (UBL_MESH_MIN_X + i * (MESH_X_DIST)) + normal.y * (UBL_MESH_MIN_Y + j * (MESH_Y_DIST))) - d);
         z_values[i][j] += c;
       }
     }
     return current_position[Z_AXIS];
   }
 
-  float measure_business_card_thickness(float height_value) {
+  float measure_business_card_thickness(const float &height_value) {
 
     ubl_has_control_of_lcd_panel++;
     save_ubl_active_state_and_disable();   // we don't do bed level correction because we want the raw data when we probe
     return abs(Z1 - Z2);
   }
 
-  void manually_probe_remaining_mesh(float x_pos, float y_pos, float z_clearance, float card_thickness, bool do_ubl_mesh_map) {
-    mesh_index_pair location;
-    float last_x, last_y, dx, dy,
-          xProbe, yProbe;
+  void manually_probe_remaining_mesh(const float &lx, const float &ly, const float &z_clearance, const float &card_thickness, const bool do_ubl_mesh_map) {
 
     ubl_has_control_of_lcd_panel++;
-    last_x = last_y = -9999.99;
     save_ubl_active_state_and_disable();   // we don't do bed level correction because we want the raw data when we probe
     do_blocking_move_to_z(z_clearance);
-    do_blocking_move_to_xy(x_pos, y_pos);
+    do_blocking_move_to_xy(lx, ly);
 
+    float last_x = -9999.99, last_y = -9999.99;
+    mesh_index_pair location;
     do {
       if (do_ubl_mesh_map) ubl.display_map(1);
 
-      location = find_closest_mesh_point_of_type(INVALID, x_pos, y_pos, 0, NULL); // The '0' says we want to use the nozzle's position
+      location = find_closest_mesh_point_of_type(INVALID, lx, ly, 0, NULL); // The '0' says we want to use the nozzle's position
       // It doesn't matter if the probe can not reach the
       // NAN location. This is a manual probe.
       if (location.x_index < 0 && location.y_index < 0) continue;
 
-      xProbe = ubl.map_x_index_to_bed_location(location.x_index);
-      yProbe = ubl.map_y_index_to_bed_location(location.y_index);
+      const float xProbe = ubl.map_x_index_to_bed_location(location.x_index),
+                  yProbe = ubl.map_y_index_to_bed_location(location.y_index);
+
+      // Modify to use if (position_is_reachable(pos[XYZ]))
       if (xProbe < (X_MIN_POS) || xProbe > (X_MAX_POS) || yProbe < (Y_MIN_POS) || yProbe > (Y_MAX_POS)) {
         SERIAL_PROTOCOLLNPGM("?Error: Attempt to probe off the bed.");
         ubl_has_control_of_lcd_panel = false;
         goto LEAVE;
       }
 
-      dx = xProbe - last_x;
-      dy = yProbe - last_y;
+      const float dx = xProbe - last_x,
+                  dy = yProbe - last_y;
 
       if (HYPOT(dx, dy) < BIG_RAISE_NOT_NEEDED)
         do_blocking_move_to_z(current_position[Z_AXIS] + SIZE_OF_LITTLE_RAISE);
       else
         do_blocking_move_to_z(z_clearance);
 
+      do_blocking_move_to_xy(xProbe, yProbe);
+
       last_x = xProbe;
       last_y = yProbe;
-      do_blocking_move_to_xy(xProbe, yProbe);
 
       wait_for_user = true;
       while (wait_for_user) {     // we need the loop to move the nozzle based on the encoder wheel here!
     LEAVE:
     restore_ubl_active_state_and_leave();
     do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE);
-    do_blocking_move_to_xy(x_pos, y_pos);
+    do_blocking_move_to_xy(lx, ly);
   }
 
   bool g29_parameter_parsing() {
       ubl.store_state();
     }
 
-    if ((c_flag = code_seen('C')) && code_has_value())
+    if ((c_flag = code_seen('C') && code_has_value()))
       constant = code_value_float();
 
     if (code_seen('D')) {     // Disable the Unified Bed Leveling System
       ubl.store_state();
     }
 
-    if (code_seen('F')) {
-      ubl.state.g29_correction_fade_height = 10.00;
-      if (code_has_value()) {
-        ubl.state.g29_correction_fade_height = code_value_float();
-        ubl.state.g29_fade_height_multiplier = 1.0 / ubl.state.g29_correction_fade_height;
-      }
-      if (ubl.state.g29_correction_fade_height < 0.0 || ubl.state.g29_correction_fade_height > 100.0) {
-        SERIAL_PROTOCOLLNPGM("?Bed Level Correction Fade Height Not Plausible.\n");
-        ubl.state.g29_correction_fade_height = 10.00;
-        ubl.state.g29_fade_height_multiplier = 1.0 / ubl.state.g29_correction_fade_height;
-        return UBL_ERR;
+    #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+      if (code_seen('F') && code_has_value()) {
+        const float fh = code_value_float();
+        if (fh < 0.0 || fh > 100.0) {
+          SERIAL_PROTOCOLLNPGM("?Bed Level Correction Fade Height Not Plausible.\n");
+          return UBL_ERR;
+        }
+        ubl.state.g29_correction_fade_height = fh;
+        ubl.state.g29_fade_height_multiplier = 1.0 / fh;
       }
-    }
+    #endif
 
     if ((repeat_flag = code_seen('R'))) {
       repetition_cnt = code_has_value() ? code_value_int() : 9999;
    * This function goes away after G29 debug is complete. But for right now, it is a handy
    * routine to dump binary data structures.
    */
-  void dump(char *str, float f) {
+  void dump(char * const str, const float &f) {
     char *ptr;
 
     SERIAL_PROTOCOL(str);
     }
     ubl_state_at_invocation = ubl.state.active;
     ubl.state.active = 0;
-    return;
   }
 
   void restore_ubl_active_state_and_leave() {
    * good to have the extra information. Soon... we prune this to just a few items
    */
   void g29_what_command() {
-    int k = E2END - ubl_eeprom_start;
+    const uint16_t k = E2END - ubl_eeprom_start;
     statistics_flag++;
 
     SERIAL_PROTOCOLPGM("Unified Bed Leveling System Version 1.00 ");
-    if (ubl.state.active)
-      SERIAL_PROTOCOLPGM("Active.\n");
-    else
-      SERIAL_PROTOCOLPGM("Inactive.\n");
-    SERIAL_EOL;
+    ubl.state.active ? SERIAL_PROTOCOLCHAR('A') : SERIAL_PROTOCOLPGM("In");
+    SERIAL_PROTOCOLLNPGM("ctive.\n");
     delay(50);
 
-    if (ubl.state.eeprom_storage_slot == 0xFFFF) {
+    if (ubl.state.eeprom_storage_slot == -1)
       SERIAL_PROTOCOLPGM("No Mesh Loaded.");
-    }
     else {
       SERIAL_PROTOCOLPGM("Mesh: ");
       prt_hex_word(ubl.state.eeprom_storage_slot);
-      SERIAL_PROTOCOLPGM(" Loaded. ");
+      SERIAL_PROTOCOLPGM(" Loaded.");
     }
-
     SERIAL_EOL;
-    delay(50);
 
-    SERIAL_PROTOCOLPAIR("g29_correction_fade_height : ", ubl.state.g29_correction_fade_height );
-    SERIAL_EOL;
-
-    idle();
+    #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+      SERIAL_PROTOCOLPAIR("g29_correction_fade_height : ", ubl.state.g29_correction_fade_height);
+      SERIAL_EOL;
+    #endif
 
     SERIAL_PROTOCOLPGM("z_offset: ");
     SERIAL_PROTOCOL_F(ubl.state.z_offset, 6);
     for (uint8_t i = 0; i < UBL_MESH_NUM_X_POINTS; i++) {
       SERIAL_PROTOCOL_F( ubl.map_x_index_to_bed_location(i), 1);
       SERIAL_PROTOCOLPGM("  ");
-      delay(10);
     }
     SERIAL_EOL;
-    delay(50);
-    idle();
 
     SERIAL_PROTOCOLPGM("Y-Axis Mesh Points at: ");
     for (uint8_t i = 0; i < UBL_MESH_NUM_Y_POINTS; i++) {
       SERIAL_PROTOCOL_F( ubl.map_y_index_to_bed_location(i), 1);
       SERIAL_PROTOCOLPGM("  ");
-      delay(10);
     }
     SERIAL_EOL;
-    delay(50);
-    idle();
 
     #if HAS_KILL
       SERIAL_PROTOCOLPAIR("Kill pin on :", KILL_PIN);
       SERIAL_PROTOCOLLNPAIR("  state:", READ(KILL_PIN));
     #endif
-    delay(50);
-    idle();
     SERIAL_EOL;
 
     SERIAL_PROTOCOLLNPAIR("ubl_state_at_invocation :", ubl_state_at_invocation);
     SERIAL_PROTOCOLPGM("Free EEPROM space starts at: 0x");
     prt_hex_word(ubl_eeprom_start);
     SERIAL_EOL;
-    delay(50);
-    idle();
 
     SERIAL_PROTOCOLPGM("end of EEPROM              : ");
     prt_hex_word(E2END);
     SERIAL_EOL;
-    delay(50);
-    idle();
 
     SERIAL_PROTOCOLLNPAIR("sizeof(ubl) :  ", (int)sizeof(ubl));
     SERIAL_EOL;
     SERIAL_PROTOCOLLNPAIR("z_value[][] size: ", (int)sizeof(z_values));
     SERIAL_EOL;
-    delay(50);
-    idle();
 
     SERIAL_PROTOCOLPGM("EEPROM free for UBL: 0x");
     prt_hex_word(k);
     SERIAL_EOL;
-    idle();
 
     SERIAL_PROTOCOLPGM("EEPROM can hold 0x");
     prt_hex_word(k / sizeof(z_values));
     SERIAL_PROTOCOLLNPGM(" meshes.\n");
-    delay(50);
 
     SERIAL_PROTOCOLPGM("sizeof(ubl.state) :");
     prt_hex_word(sizeof(ubl.state));
-    idle();
 
     SERIAL_PROTOCOLPAIR("\nUBL_MESH_NUM_X_POINTS  ", UBL_MESH_NUM_X_POINTS);
     SERIAL_PROTOCOLPAIR("\nUBL_MESH_NUM_Y_POINTS  ", UBL_MESH_NUM_Y_POINTS);
     SERIAL_PROTOCOLPAIR("\nUBL_MESH_MIN_X         ", UBL_MESH_MIN_X);
-    delay(50);
-    idle();
     SERIAL_PROTOCOLPAIR("\nUBL_MESH_MIN_Y         ", UBL_MESH_MIN_Y);
     SERIAL_PROTOCOLPAIR("\nUBL_MESH_MAX_X         ", UBL_MESH_MAX_X);
     SERIAL_PROTOCOLPAIR("\nUBL_MESH_MAX_Y         ", UBL_MESH_MAX_Y);
-    delay(50);
-    idle();
     SERIAL_PROTOCOLPGM("\nMESH_X_DIST        ");
     SERIAL_PROTOCOL_F(MESH_X_DIST, 6);
     SERIAL_PROTOCOLPGM("\nMESH_Y_DIST        ");
     SERIAL_PROTOCOL_F(MESH_Y_DIST, 6);
     SERIAL_EOL;
 
-    idle();
-
     if (!ubl.sanity_check())
       SERIAL_PROTOCOLLNPGM("Unified Bed Leveling sanity checks passed.");
   }
     SERIAL_ECHO_START;
     SERIAL_ECHOLNPGM("EEPROM Dump:");
     for (uint16_t i = 0; i < E2END + 1; i += 16) {
-      if (i & 0x3 == 0) idle();
+      if (!(i & 0x3)) idle();
       prt_hex_word(i);
       SERIAL_ECHOPGM(": ");
       for (uint16_t j = 0; j < 16; j++) {
       SERIAL_EOL;
     }
     SERIAL_EOL;
-    return;
   }
 
   /**
     }
     storage_slot = code_value_int();
 
-    uint16_t k = E2END - sizeof(ubl.state),
-             j = (k - ubl_eeprom_start) / sizeof(tmp_z_values);
+    int16_t j = (UBL_LAST_EEPROM_INDEX - ubl_eeprom_start) / sizeof(tmp_z_values);
 
     if (storage_slot < 0 || storage_slot > j || ubl_eeprom_start <= 0) {
       SERIAL_PROTOCOLLNPGM("?EEPROM storage not available for use.\n");
       return;
     }
 
-    j = k - (storage_slot + 1) * sizeof(tmp_z_values);
+    j = UBL_LAST_EEPROM_INDEX - (storage_slot + 1) * sizeof(tmp_z_values);
     eeprom_read_block((void *)&tmp_z_values, (void *)j, sizeof(tmp_z_values));
 
     SERIAL_ECHOPAIR("Subtracting Mesh ", storage_slot);
         z_values[x][y] = z_values[x][y] - tmp_z_values[x][y];
   }
 
-  mesh_index_pair find_closest_mesh_point_of_type(MeshPointType type, float X, float Y, bool probe_as_reference, unsigned int bits[16]) {
+  mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType type, const float &lx, const float &ly, const bool probe_as_reference, unsigned int bits[16]) {
     int i, j;
-    float f, px, py, mx, my, dx, dy, closest = 99999.99,
-          current_x, current_y, distance;
+    float closest = 99999.99;
     mesh_index_pair return_val;
 
     return_val.x_index = return_val.y_index = -1;
 
-    current_x = current_position[X_AXIS];
-    current_y = current_position[Y_AXIS];
+    const float current_x = current_position[X_AXIS],
+                current_y = current_position[Y_AXIS];
 
-    px = X;       // Get our reference position. Either the nozzle or
-    py = Y;       // the probe location.
-    if (probe_as_reference) {
-      px -= X_PROBE_OFFSET_FROM_EXTRUDER;
-      py -= Y_PROBE_OFFSET_FROM_EXTRUDER;
-    }
+    // Get our reference position. Either the nozzle or probe location.
+    const float px = lx - (probe_as_reference ? X_PROBE_OFFSET_FROM_EXTRUDER : 0),
+                py = ly - (probe_as_reference ? Y_PROBE_OFFSET_FROM_EXTRUDER : 0);
 
     for (i = 0; i < UBL_MESH_NUM_X_POINTS; i++) {
       for (j = 0; j < UBL_MESH_NUM_Y_POINTS; j++) {
 
           // We only get here if we found a Mesh Point of the specified type
 
-          mx = ubl.map_x_index_to_bed_location(i); // Check if we can probe this mesh location
-          my = ubl.map_y_index_to_bed_location(j);
+          const float mx = LOGICAL_X_POSITION(ubl.map_x_index_to_bed_location(i)), // Check if we can probe this mesh location
+                      my = LOGICAL_Y_POSITION(ubl.map_y_index_to_bed_location(j));
 
           // If we are using the probe as the reference there are some locations we can't get to.
           // We prune these out of the list and ignore them until the next Phase where we do the
           // manual nozzle probing.
-	  
+
           if (probe_as_reference &&
-            ( mx < (MIN_PROBE_X) || mx > (MAX_PROBE_X) || my < (MIN_PROBE_Y) || my > (MAX_PROBE_Y) )
+            (mx < (MIN_PROBE_X) || mx > (MAX_PROBE_X) || my < (MIN_PROBE_Y) || my > (MAX_PROBE_Y))
           ) continue;
 
-          dx = px - mx;         // We can get to it. Let's see if it is the
-          dy = py - my;         // closest location to the nozzle.
-          distance = HYPOT(dx, dy);
-
-          dx = current_x - mx;                    // We are going to add in a weighting factor that considers
-          dy = current_y - my;                    // the current location of the nozzle. If two locations are equal
-          distance += HYPOT(dx, dy) * 0.01;       // distance from the measurement location, we are going to give
+          // We can get to it. Let's see if it is the closest location to the nozzle.
+          // Add in a weighting factor that considers the current location of the nozzle.
+          const float distance = HYPOT(px - mx, py - my) + HYPOT(current_x - mx, current_y - my) * 0.01;
 
           if (distance < closest) {
             closest = distance;       // We found a closer location with
     return return_val;
   }
 
-  void fine_tune_mesh(float x_pos, float y_pos, bool do_ubl_mesh_map) {
+  void fine_tune_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map) {
     mesh_index_pair location;
-    float xProbe, yProbe;
-    uint16_t i, not_done[16];
+    uint16_t not_done[16];
     int32_t round_off;
 
     save_ubl_active_state_and_disable();
     #endif
 
     do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE);
-    do_blocking_move_to_xy(x_pos, y_pos);
+    do_blocking_move_to_xy(lx, ly);
     do {
       if (do_ubl_mesh_map) ubl.display_map(1);
 
-      location = find_closest_mesh_point_of_type( SET_IN_BITMAP, x_pos,  y_pos, 0, not_done); // The '0' says we want to use the nozzle's position
+      location = find_closest_mesh_point_of_type( SET_IN_BITMAP, lx,  ly, 0, not_done); // The '0' says we want to use the nozzle's position
                                                                                               // It doesn't matter if the probe can not reach this
                                                                                               // location. This is a manual edit of the Mesh Point.
       if (location.x_index < 0 && location.y_index < 0) continue; // abort if we can't find any more points.
       bit_clear(not_done, location.x_index, location.y_index);  // Mark this location as 'adjusted' so we will find a
                                                                 // different location the next time through the loop
 
-      xProbe = ubl.map_x_index_to_bed_location(location.x_index);
-      yProbe = ubl.map_y_index_to_bed_location(location.y_index);
+      const float xProbe = ubl.map_x_index_to_bed_location(location.x_index),
+                  yProbe = ubl.map_y_index_to_bed_location(location.y_index);
       if (xProbe < X_MIN_POS || xProbe > X_MAX_POS || yProbe < Y_MIN_POS || yProbe > Y_MAX_POS) { // In theory, we don't need this check.
         SERIAL_PROTOCOLLNPGM("?Error: Attempt to edit off the bed.");                             // This really can't happen, but for now,
         ubl_has_control_of_lcd_panel = false;                                                         // Let's do the check.
     restore_ubl_active_state_and_leave();
     do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE);
 
-    do_blocking_move_to_xy(x_pos, y_pos);
+    do_blocking_move_to_xy(lx, ly);
 
     #if ENABLED(ULTRA_LCD)
       lcd_setstatus("Done Editing Mesh", true);

Marlin/UBL_line_to_destination.cpp

   #include <avr/io.h>
   #include <math.h>
 
+  extern float destination[XYZE];
   extern void set_current_to_destination();
-  extern void debug_current_and_destination(char *title);
 
-  void ubl_line_to_destination(const float &x_end, const float &y_end, const float &z_end, const float &e_end, const float &feed_rate, uint8_t extruder) {
+  void debug_current_and_destination(char *title) {
+
+    // if the title message starts with a '!' it is so important, we are going to
+    // ignore the status of the g26_debug_flag
+    if (*title != '!' && !g26_debug_flag) return;
+
+    const float de = destination[E_AXIS] - current_position[E_AXIS];
+
+    if (de == 0.0) return;
+
+    const float dx = current_position[X_AXIS] - destination[X_AXIS],
+                dy = current_position[Y_AXIS] - destination[Y_AXIS],
+                xy_dist = HYPOT(dx, dy);
+
+    if (xy_dist == 0.0) {
+      return;
+      //SERIAL_ECHOPGM("   FPMM=");
+      //const float fpmm = de / xy_dist;
+      //SERIAL_PROTOCOL_F(fpmm, 6);
+    }
+    else {
+      SERIAL_ECHOPGM("   fpmm=");
+      const float fpmm = de / xy_dist;
+      SERIAL_ECHO_F(fpmm, 6);
+    }
 
-    int cell_start_xi, cell_start_yi, cell_dest_xi, cell_dest_yi,
-        current_xi, current_yi,
-        dxi, dyi, xi_cnt, yi_cnt;
-    float x_start, y_start,
-          x, y, z1, z2, z0 /*, z_optimized */,
-          next_mesh_line_x, next_mesh_line_y, a0ma1diva2ma1,
-          on_axis_distance, e_normalized_dist, e_position, e_start, z_normalized_dist, z_position, z_start,
-          dx, dy, adx, ady, m, c;
+    SERIAL_ECHOPGM("    current=( ");
+    SERIAL_ECHO_F(current_position[X_AXIS], 6);
+    SERIAL_ECHOPGM(", ");
+    SERIAL_ECHO_F(current_position[Y_AXIS], 6);
+    SERIAL_ECHOPGM(", ");
+    SERIAL_ECHO_F(current_position[Z_AXIS], 6);
+    SERIAL_ECHOPGM(", ");
+    SERIAL_ECHO_F(current_position[E_AXIS], 6);
+    SERIAL_ECHOPGM(" )   destination=( ");
+    if (current_position[X_AXIS] == destination[X_AXIS])
+      SERIAL_ECHOPGM("-------------");
+    else
+      SERIAL_ECHO_F(destination[X_AXIS], 6);
+
+    SERIAL_ECHOPGM(", ");
+
+    if (current_position[Y_AXIS] == destination[Y_AXIS])
+      SERIAL_ECHOPGM("-------------");
+    else
+      SERIAL_ECHO_F(destination[Y_AXIS], 6);
+
+    SERIAL_ECHOPGM(", ");
+
+    if (current_position[Z_AXIS] == destination[Z_AXIS])
+      SERIAL_ECHOPGM("-------------");
+    else
+      SERIAL_ECHO_F(destination[Z_AXIS], 6);
+
+    SERIAL_ECHOPGM(", ");
+
+    if (current_position[E_AXIS] == destination[E_AXIS])
+      SERIAL_ECHOPGM("-------------");
+    else
+      SERIAL_ECHO_F(destination[E_AXIS], 6);
+
+    SERIAL_ECHOPGM(" )   ");
+    SERIAL_ECHO(title);
+    SERIAL_EOL;
+
+    SET_INPUT_PULLUP(66); // Roxy's Left Switch is on pin 66.  Right Switch is on pin 65
+
+    //if (been_to_2_6) {
+    //while ((digitalRead(66) & 0x01) != 0)
+    //  idle();
+    //}
+  }
 
+  void ubl_line_to_destination(const float &x_end, const float &y_end, const float &z_end, const float &e_end, const float &feed_rate, uint8_t extruder) {
     /**
      * Much of the nozzle movement will be within the same cell. So we will do as little computation
      * as possible to determine if this is the case. If this move is within the same cell, we will
      * just do the required Z-Height correction, call the Planner's buffer_line() routine, and leave
      */
+    const float x_start = current_position[X_AXIS],
+                y_start = current_position[Y_AXIS],
+                z_start = current_position[Z_AXIS],
+                e_start = current_position[E_AXIS];
 
-    x_start = current_position[X_AXIS];
-    y_start = current_position[Y_AXIS];
-    z_start = current_position[Z_AXIS];
-    e_start = current_position[E_AXIS];
-
-    cell_start_xi = ubl.get_cell_index_x(x_start);
-    cell_start_yi = ubl.get_cell_index_y(y_start);
-    cell_dest_xi  = ubl.get_cell_index_x(x_end);
-    cell_dest_yi  = ubl.get_cell_index_y(y_end);
+    const int cell_start_xi = ubl.get_cell_index_x(RAW_X_POSITION(x_start)),
+              cell_start_yi = ubl.get_cell_index_y(RAW_Y_POSITION(y_start)),
+              cell_dest_xi  = ubl.get_cell_index_x(RAW_X_POSITION(x_end)),
+              cell_dest_yi  = ubl.get_cell_index_y(RAW_Y_POSITION(y_end));
 
     if (g26_debug_flag) {
       SERIAL_ECHOPGM(" ubl_line_to_destination(xe=");
       SERIAL_ECHO(z_end);
       SERIAL_ECHOPGM(", ee=");
       SERIAL_ECHO(e_end);
-      SERIAL_ECHOPGM(")\n");
+      SERIAL_ECHOLNPGM(")");
       debug_current_and_destination((char*)"Start of ubl_line_to_destination()");
     }
 
 
       if (cell_dest_xi < 0 || cell_dest_yi < 0 || cell_dest_xi >= UBL_MESH_NUM_X_POINTS || cell_dest_yi >= UBL_MESH_NUM_Y_POINTS) {
 
-        // Note:  There is no Z Correction in this case. We are off the grid and don't know what
+        // Note: There is no Z Correction in this case. We are off the grid and don't know what
         // a reasonable correction would be.
 
         planner.buffer_line(x_end, y_end, z_end + ubl.state.z_offset, e_end, feed_rate, extruder);
        * to create a 1-over number for us. That will allow us to do a floating point multiply instead of a floating point divide.
        */
 
-      a0ma1diva2ma1 = (x_end - mesh_index_to_x_location[cell_dest_xi]) * 0.1 * (MESH_X_DIST);
-
-      z1 = z_values[cell_dest_xi    ][cell_dest_yi    ] + a0ma1diva2ma1 *
-          (z_values[cell_dest_xi + 1][cell_dest_yi    ] - z_values[cell_dest_xi][cell_dest_yi    ]);
-
-      z2 = z_values[cell_dest_xi    ][cell_dest_yi + 1] + a0ma1diva2ma1 *
-          (z_values[cell_dest_xi + 1][cell_dest_yi + 1] - z_values[cell_dest_xi][cell_dest_yi + 1]);
+      const float xratio = (RAW_X_POSITION(x_end) - mesh_index_to_x_location[cell_dest_xi]) * (1.0 / (MESH_X_DIST)),
+                  z1 = z_values[cell_dest_xi    ][cell_dest_yi    ] + xratio *
+                      (z_values[cell_dest_xi + 1][cell_dest_yi    ] - z_values[cell_dest_xi][cell_dest_yi    ]),
+                  z2 = z_values[cell_dest_xi    ][cell_dest_yi + 1] + xratio *
+                      (z_values[cell_dest_xi + 1][cell_dest_yi + 1] - z_values[cell_dest_xi][cell_dest_yi + 1]);
 
       // we are done with the fractional X distance into the cell. Now with the two Z-Heights we have calculated, we
       // are going to apply the Y-Distance into the cell to interpolate the final Z correction.
 
-      a0ma1diva2ma1 = (y_end - mesh_index_to_y_location[cell_dest_yi]) * 0.1 * (MESH_Y_DIST);
+      const float yratio = (RAW_Y_POSITION(y_end) - mesh_index_to_y_location[cell_dest_yi]) * (1.0 / (MESH_Y_DIST));
 
-      z0 = z1 + (z2 - z1) * a0ma1diva2ma1;
+      float z0 = z1 + (z2 - z1) * yratio;
 
       /**
        * Debug code to use non-optimized get_z_correction() and to do a sanity check
        */
       /*
         z_optimized = z0;
-        z0 = ubl.get_z_correction( x_end, y_end);
+        z0 = ubl.get_z_correction(x_end, y_end);
         if (fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized)) {
         debug_current_and_destination((char*)"FINAL_MOVE: z_correction()");
         if (isnan(z0)) SERIAL_ECHO(" z0==NAN  ");
         SERIAL_EOL;
         }
       //*/
-      z0 = z0 * ubl.fade_scaling_factor_for_z(z_end);
+      z0 *= ubl.fade_scaling_factor_for_z(z_end);
 
       /**
        * If part of the Mesh is undefined, it will show up as NAN
      * blocks of code:
      */
 
-    dx = x_end - x_start;
-    dy = y_end - y_start;
+    const float dx = x_end - x_start,
+                dy = y_end - y_start;
 
     const int left_flag = dx < 0.0 ? 1 : 0,
               down_flag = dy < 0.0 ? 1 : 0;
 
-    if (left_flag) { // figure out which way we need to move to get to the next cell
-      dxi = -1;
-      adx = -dx;  // absolute value of dx. We already need to check if dx and dy are negative.
-    }
-    else {      // We may as well generate the appropriate values for adx and ady right now
-      dxi = 1;  // to save setting up the abs() function call and actually doing the call.
-      adx = dx;
-    }
-    if (dy < 0.0) {
-      dyi = -1;
-      ady = -dy;  // absolute value of dy
-    }
-    else {
-      dyi = 1;
-      ady = dy;
-    }
+    const float adx = left_flag ? -dx : dx,
+                ady = down_flag ? -dy : dy;
 
-    if (cell_start_xi == cell_dest_xi) dxi = 0;
-    if (cell_start_yi == cell_dest_yi) dyi = 0;
+    const int dxi = cell_start_xi == cell_dest_xi ? 0 : left_flag ? -1 : 1,
+              dyi = cell_start_yi == cell_dest_yi ? 0 : down_flag ? -1 : 1;
 
     /**
      * Compute the scaling factor for the extruder for each partial move.
 
     const bool use_x_dist = adx > ady;
 
-    on_axis_distance = use_x_dist ? x_end - x_start : y_end - y_start;
-
-    e_position = e_end - e_start;
-    e_normalized_dist = e_position / on_axis_distance;
+    float on_axis_distance = use_x_dist ? dx : dy,
+          e_position = e_end - e_start,
+          z_position = z_end - z_start;
 
-    z_position = z_end - z_start;
-    z_normalized_dist = z_position / on_axis_distance;
+    const float e_normalized_dist = e_position / on_axis_distance,
+                z_normalized_dist = z_position / on_axis_distance;
 
-    const bool inf_normalized_flag = e_normalized_dist == INFINITY || e_normalized_dist == -INFINITY;
+    int current_xi = cell_start_xi, current_yi = cell_start_yi;
 
-    current_xi = cell_start_xi;
-    current_yi = cell_start_yi;
+    const float m = dy / dx,
+                c = y_start - m * x_start;
 
-    m = dy / dx;
-    c = y_start - m * x_start;
-    const bool inf_m_flag = (m == INFINITY || m == -INFINITY);
+    const bool inf_normalized_flag = NEAR_ZERO(on_axis_distance),
+               inf_m_flag = NEAR_ZERO(dx);
 
     /**
      * This block handles vertical lines. These are lines that stay within the same
       current_yi += down_flag;  // Line is heading down, we just want to go to the bottom
       while (current_yi != cell_dest_yi + down_flag) {
         current_yi += dyi;
-        next_mesh_line_y = mesh_index_to_y_location[current_yi];
+        const float next_mesh_line_y = LOGICAL_Y_POSITION(mesh_index_to_y_location[current_yi]);
 
         /**
          * inf_m_flag? the slope of the line is infinite, we won't do the calculations
          * else, we know the next X is the same so we can recover and continue!
          * Calculate X at the next Y mesh line
          */
-        x = inf_m_flag ? x_start : (next_mesh_line_y - c) / m;
+        const float x = inf_m_flag ? x_start : (next_mesh_line_y - c) / m;
 
-        z0 = ubl.get_z_correction_along_horizontal_mesh_line_at_specific_X(x, current_xi, current_yi);
+        float z0 = ubl.get_z_correction_along_horizontal_mesh_line_at_specific_X(x, current_xi, current_yi);
 
         /**
          * Debug code to use non-optimized get_z_correction() and to do a sanity check
          */
         /*
           z_optimized = z0;
-          z0 = ubl.get_z_correction( x, next_mesh_line_y);
+          z0 = ubl.get_z_correction(x, next_mesh_line_y);
           if (fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized)) {
             debug_current_and_destination((char*)"VERTICAL z_correction()");
           if (isnan(z0)) SERIAL_ECHO(" z0==NAN  ");
           }
         //*/
 
-        z0 = z0 * ubl.fade_scaling_factor_for_z(z_end);
+        z0 *= ubl.fade_scaling_factor_for_z(z_end);
 
         /**
          * If part of the Mesh is undefined, it will show up as NAN
          */
         if (isnan(z0)) z0 = 0.0;     
 
-        y = mesh_index_to_y_location[current_yi];
+        const float y = LOGICAL_Y_POSITION(mesh_index_to_y_location[current_yi]);
 
         /**
          * Without this check, it is possible for the algorithm to generate a zero length move in the case
                                 // edge of this cell for the first move.
       while (current_xi != cell_dest_xi + left_flag) {
         current_xi += dxi;
-        next_mesh_line_x = mesh_index_to_x_location[current_xi];
-        y = m * next_mesh_line_x + c;   // Calculate X at the next Y mesh line
+        const float next_mesh_line_x = LOGICAL_X_POSITION(mesh_index_to_x_location[current_xi]),
+                    y = m * next_mesh_line_x + c;   // Calculate X at the next Y mesh line
 
-        z0 = ubl.get_z_correction_along_vertical_mesh_line_at_specific_Y(y, current_xi, current_yi);
+        float z0 = ubl.get_z_correction_along_vertical_mesh_line_at_specific_Y(y, current_xi, current_yi);
 
         /**
          * Debug code to use non-optimized get_z_correction() and to do a sanity check
          */
         /*
           z_optimized = z0;
-          z0 = ubl.get_z_correction( next_mesh_line_x, y);
+          z0 = ubl.get_z_correction(next_mesh_line_x, y);
           if (fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized)) {
             debug_current_and_destination((char*)"HORIZONTAL z_correction()");
           if (isnan(z0)) SERIAL_ECHO(" z0==NAN  ");
          */
         if (isnan(z0)) z0 = 0.0;
 
-        x = mesh_index_to_x_location[current_xi];
+        const float x = LOGICAL_X_POSITION(mesh_index_to_x_location[current_xi]);
 
         /**
          * Without this check, it is possible for the algorithm to generate a zero length move in the case
      *
      */
 
-    xi_cnt = cell_start_xi - cell_dest_xi;
-    if (xi_cnt < 0) xi_cnt = -xi_cnt;
+    int xi_cnt = cell_start_xi - cell_dest_xi,
+        yi_cnt = cell_start_yi - cell_dest_yi;
 
-    yi_cnt = cell_start_yi - cell_dest_yi;
+    if (xi_cnt < 0) xi_cnt = -xi_cnt;
     if (yi_cnt < 0) yi_cnt = -yi_cnt;
 
     current_xi += left_flag;
 
     while (xi_cnt > 0 || yi_cnt > 0) {
 
-      next_mesh_line_x = mesh_index_to_x_location[current_xi + dxi];
-      next_mesh_line_y = mesh_index_to_y_location[current_yi + dyi];
-
-      y = m * next_mesh_line_x + c; // Calculate Y at the next X mesh line
-      x = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line    (we don't have to worry
-      // about m being equal to 0.0  If this was the case, we would have
-      // detected this as a vertical line move up above and we wouldn't
-      // be down here doing a generic type of move.
+      const float next_mesh_line_x = LOGICAL_X_POSITION(mesh_index_to_x_location[current_xi + dxi]),
+                  next_mesh_line_y = LOGICAL_Y_POSITION(mesh_index_to_y_location[current_yi + dyi]),
+                  y = m * next_mesh_line_x + c,   // Calculate Y at the next X mesh line
+                  x = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line    (we don't have to worry
+                                                  // about m being equal to 0.0  If this was the case, we would have
+                                                  // detected this as a vertical line move up above and we wouldn't
+                                                  // be down here doing a generic type of move.
 
       if (left_flag == (x > next_mesh_line_x)) { // Check if we hit the Y line first
         //
         // Yes!  Crossing a Y Mesh Line next
         //
-        z0 = ubl.get_z_correction_along_horizontal_mesh_line_at_specific_X(x, current_xi - left_flag, current_yi + dyi);
+        float z0 = ubl.get_z_correction_along_horizontal_mesh_line_at_specific_X(x, current_xi - left_flag, current_yi + dyi);
 
         /**
          * Debug code to use non-optimized get_z_correction() and to do a sanity check
          */
         /*
           z_optimized = z0;
-          z0 = ubl.get_z_correction( x, next_mesh_line_y);
+          z0 = ubl.get_z_correction(x, next_mesh_line_y);
           if (fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized)) {
             debug_current_and_destination((char*)"General_1: z_correction()");
             if (isnan(z0)) SERIAL_ECHO(" z0==NAN  ");
         //
         // Yes!  Crossing a X Mesh Line next
         //
-        z0 = ubl.get_z_correction_along_vertical_mesh_line_at_specific_Y(y, current_xi + dxi, current_yi - down_flag);
+        float z0 = ubl.get_z_correction_along_vertical_mesh_line_at_specific_Y(y, current_xi + dxi, current_yi - down_flag);
 
         /**
          * Debug code to use non-optimized get_z_correction() and to do a sanity check
          */
         /*
           z_optimized = z0;
-          z0 = ubl.get_z_correction( next_mesh_line_x, y);
+          z0 = ubl.get_z_correction(next_mesh_line_x, y);
           if (fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized)) {
           debug_current_and_destination((char*)"General_2: z_correction()");
           if (isnan(z0)) SERIAL_ECHO(" z0==NAN  ");
           }
         //*/
 
-        z0 = z0 * ubl.fade_scaling_factor_for_z(z_end);
+        z0 *= ubl.fade_scaling_factor_for_z(z_end);
 
         /**
          * If part of the Mesh is undefined, it will show up as NAN

Marlin/configuration_store.cpp

 
       #if ENABLED(AUTO_BED_LEVELING_UBL)
         ubl_eeprom_start = (eeprom_index + 32) & 0xFFF8; // Pad the end of configuration data so it
-                                                                          // can float up or down a little bit without
-                                                                          // disrupting the Unified Bed Leveling data
+                                                         // can float up or down a little bit without
+                                                         // disrupting the Unified Bed Leveling data
         ubl.load_state();
 
         SERIAL_ECHOPGM(" UBL ");
         }
         else {
           ubl.reset();
-          SERIAL_ECHOPGM("UBL System reset() \n");
+          SERIAL_ECHOLNPGM("UBL System reset()");
         }
       #endif
     }
       SERIAL_ECHOPAIR(" Z", home_offset[Z_AXIS]);
       SERIAL_EOL;
     #endif
-  #if ENABLED(AUTO_BED_LEVELING_UBL)
-    SERIAL_ECHOLNPGM("Unified Bed Leveling:");
-    CONFIG_ECHO_START;
-
-    SERIAL_ECHOPGM("System is: ");
-    if (ubl.state.active)
-       SERIAL_ECHOLNPGM("Active\n");
-    else
-       SERIAL_ECHOLNPGM("Deactive\n");
-
-    SERIAL_ECHOPAIR("Active Mesh Slot: ", ubl.state.eeprom_storage_slot);
-    SERIAL_EOL;
-
-    SERIAL_ECHOPGM("z_offset: ");
-    SERIAL_ECHO_F(ubl.state.z_offset, 6);
-    SERIAL_EOL;
-
-    SERIAL_ECHOPAIR("EEPROM can hold ", (int)((E2END - sizeof(ubl.state) - ubl_eeprom_start) / sizeof(z_values)));
-    SERIAL_ECHOLNPGM(" meshes. \n");
-
-    SERIAL_ECHOPAIR("\nUBL_MESH_NUM_X_POINTS  ", UBL_MESH_NUM_X_POINTS);
-    SERIAL_ECHOPAIR("\nUBL_MESH_NUM_Y_POINTS  ", UBL_MESH_NUM_Y_POINTS);
-
-    SERIAL_ECHOPAIR("\nUBL_MESH_MIN_X         ", UBL_MESH_MIN_X);
-    SERIAL_ECHOPAIR("\nUBL_MESH_MIN_Y         ", UBL_MESH_MIN_Y);
-
-    SERIAL_ECHOPAIR("\nUBL_MESH_MAX_X         ", UBL_MESH_MAX_X);
-    SERIAL_ECHOPAIR("\nUBL_MESH_MAX_Y         ", UBL_MESH_MAX_Y);
-
-    SERIAL_ECHOPGM("\nMESH_X_DIST        ");
-    SERIAL_ECHO_F(MESH_X_DIST, 6);
-    SERIAL_ECHOPGM("\nMESH_Y_DIST        ");
-    SERIAL_ECHO_F(MESH_Y_DIST, 6);
-    SERIAL_EOL;
-    SERIAL_EOL;
-  #endif
 
     #if HOTENDS > 1
       CONFIG_ECHO_START;
     #endif
 
     #if ENABLED(MESH_BED_LEVELING)
+
       if (!forReplay) {
         SERIAL_ECHOLNPGM("Mesh Bed Leveling:");
         CONFIG_ECHO_START;
           SERIAL_EOL;
         }
       }
+
+    #elif ENABLED(AUTO_BED_LEVELING_UBL)
+
+      if (!forReplay) {
+        SERIAL_ECHOLNPGM("Unified Bed Leveling:");
+        CONFIG_ECHO_START;
+      }
+
+      SERIAL_ECHOLNPAIR("  M420 S", ubl.state.active ? 1 : 0);
+
+      if (!forReplay) {
+        SERIAL_ECHOPGM("\nUBL is ");
+        ubl.state.active ? SERIAL_CHAR('A') : SERIAL_ECHOPGM("Ina");
+        SERIAL_ECHOLNPAIR("ctive\n\nActive Mesh Slot: ", ubl.state.eeprom_storage_slot);
+
+        SERIAL_ECHOPGM("z_offset: ");
+        SERIAL_ECHO_F(ubl.state.z_offset, 6);
+        SERIAL_EOL;
+
+        SERIAL_ECHOPAIR("EEPROM can hold ", (int)((UBL_LAST_EEPROM_INDEX - ubl_eeprom_start) / sizeof(z_values)));
+        SERIAL_ECHOLNPGM(" meshes.\n");
+
+        SERIAL_ECHOPAIR("\nUBL_MESH_NUM_X_POINTS  ", UBL_MESH_NUM_X_POINTS);
+        SERIAL_ECHOPAIR("\nUBL_MESH_NUM_Y_POINTS  ", UBL_MESH_NUM_Y_POINTS);
+
+        SERIAL_ECHOPAIR("\nUBL_MESH_MIN_X         ", UBL_MESH_MIN_X);
+        SERIAL_ECHOPAIR("\nUBL_MESH_MIN_Y         ", UBL_MESH_MIN_Y);
+
+        SERIAL_ECHOPAIR("\nUBL_MESH_MAX_X         ", UBL_MESH_MAX_X);
+        SERIAL_ECHOPAIR("\nUBL_MESH_MAX_Y         ", UBL_MESH_MAX_Y);
+
+        SERIAL_ECHOPGM("\nMESH_X_DIST        ");
+        SERIAL_ECHO_F(MESH_X_DIST, 6);
+        SERIAL_ECHOPGM("\nMESH_Y_DIST        ");
+        SERIAL_ECHO_F(MESH_Y_DIST, 6);
+        SERIAL_EOL;
+        SERIAL_EOL;
+      }
+
     #elif HAS_ABL
+
       if (!forReplay) {
         SERIAL_ECHOLNPGM("Auto Bed Leveling:");
         CONFIG_ECHO_START;
       }
       SERIAL_ECHOLNPAIR("  M420 S", planner.abl_enabled ? 1 : 0);
+
     #endif
 
     #if ENABLED(DELTA)

Marlin/example_configurations/Cartesio/Configuration.h

   #define UBL_PROBE_PT_2_Y 20
   #define UBL_PROBE_PT_3_X 180
   #define UBL_PROBE_PT_3_Y 20
+  #define UBL_MESH_EDIT_ENABLED     // Enable G26 mesh editing
 
 #elif ENABLED(MESH_BED_LEVELING)
 

Marlin/example_configurations/Felix/Configuration.h

   #define UBL_PROBE_PT_2_Y 20
   #define UBL_PROBE_PT_3_X 180
   #define UBL_PROBE_PT_3_Y 20
+  #define UBL_MESH_EDIT_ENABLED     // Enable G26 mesh editing
 
 #elif ENABLED(MESH_BED_LEVELING)
 

Marlin/example_configurations/Felix/DUAL/Configuration.h

   #define UBL_PROBE_PT_2_Y 20
   #define UBL_PROBE_PT_3_X 180
   #define UBL_PROBE_PT_3_Y 20
+  #define UBL_MESH_EDIT_ENABLED     // Enable G26 mesh editing
 
 #elif ENABLED(MESH_BED_LEVELING)
 

Marlin/example_configurations/Hephestos/Configuration.h

   #define UBL_PROBE_PT_2_Y 20
   #define UBL_PROBE_PT_3_X 180
   #define UBL_PROBE_PT_3_Y 20
+  #define UBL_MESH_EDIT_ENABLED     // Enable G26 mesh editing
 
 #elif ENABLED(MESH_BED_LEVELING)
 

Marlin/example_configurations/Hephestos_2/Configuration.h

   #define UBL_PROBE_PT_2_Y 20
   #define UBL_PROBE_PT_3_X 180
   #define UBL_PROBE_PT_3_Y 20
+  #define UBL_MESH_EDIT_ENABLED     // Enable G26 mesh editing
 
 #elif ENABLED(MESH_BED_LEVELING)
 

Marlin/example_configurations/K8200/Configuration.h

   #define UBL_PROBE_PT_2_Y 20
   #define UBL_PROBE_PT_3_X 180
   #define UBL_PROBE_PT_3_Y 20
+  #define UBL_MESH_EDIT_ENABLED     // Enable G26 mesh editing
 
 #elif ENABLED(MESH_BED_LEVELING)
 

Marlin/example_configurations/K8400/Configuration.h

   #define UBL_PROBE_PT_2_Y 20
   #define UBL_PROBE_PT_3_X 180
   #define UBL_PROBE_PT_3_Y 20
+  #define UBL_MESH_EDIT_ENABLED     // Enable G26 mesh editing
 
 #elif ENABLED(MESH_BED_LEVELING)
 

Marlin/example_configurations/K8400/Dual-head/Configuration.h

   #define UBL_PROBE_PT_2_Y 20
   #define UBL_PROBE_PT_3_X 180
   #define UBL_PROBE_PT_3_Y 20
+  #define UBL_MESH_EDIT_ENABLED     // Enable G26 mesh editing
 
 #elif ENABLED(MESH_BED_LEVELING)
 

Marlin/example_configurations/RepRapWorld/Megatronics/Configuration.h

   #define UBL_PROBE_PT_2_Y 20
   #define UBL_PROBE_PT_3_X 180
   #define UBL_PROBE_PT_3_Y 20
+  #define UBL_MESH_EDIT_ENABLED     // Enable G26 mesh editing
 
 #elif ENABLED(MESH_BED_LEVELING)
 

Marlin/example_configurations/RigidBot/Configuration.h

   #define UBL_PROBE_PT_2_Y 20
   #define UBL_PROBE_PT_3_X 180
   #define UBL_PROBE_PT_3_Y 20
+  #define UBL_MESH_EDIT_ENABLED     // Enable G26 mesh editing
 
 #elif ENABLED(MESH_BED_LEVELING)
 

Marlin/example_configurations/SCARA/Configuration.h

   #define UBL_PROBE_PT_2_Y 20
   #define UBL_PROBE_PT_3_X 180
   #define UBL_PROBE_PT_3_Y 20
+  #define UBL_MESH_EDIT_ENABLED     // Enable G26 mesh editing
 
 #elif ENABLED(MESH_BED_LEVELING)
 

Marlin/example_configurations/TAZ4/Configuration.h

   #define UBL_PROBE_PT_2_Y 20
   #define UBL_PROBE_PT_3_X 180
   #define UBL_PROBE_PT_3_Y 20
+  #define UBL_MESH_EDIT_ENABLED     // Enable G26 mesh editing
 
 #elif ENABLED(MESH_BED_LEVELING)
 

Marlin/example_configurations/WITBOX/Configuration.h

   #define UBL_PROBE_PT_2_Y 20
   #define UBL_PROBE_PT_3_X 180
   #define UBL_PROBE_PT_3_Y 20
+  #define UBL_MESH_EDIT_ENABLED     // Enable G26 mesh editing
 
 #elif ENABLED(MESH_BED_LEVELING)
 

Marlin/example_configurations/adafruit/ST7565/Configuration.h

   #define UBL_PROBE_PT_2_Y 20
   #define UBL_PROBE_PT_3_X 180
   #define UBL_PROBE_PT_3_Y 20
+  #define UBL_MESH_EDIT_ENABLED     // Enable G26 mesh editing
 
 #elif ENABLED(MESH_BED_LEVELING)
 

Marlin/example_configurations/delta/flsun_kossel_mini/Configuration.h

   #define UBL_PROBE_PT_2_Y 20
   #define UBL_PROBE_PT_3_X 180
   #define UBL_PROBE_PT_3_Y 20
+  #define UBL_MESH_EDIT_ENABLED     // Enable G26 mesh editing
 
 #elif ENABLED(MESH_BED_LEVELING)
 

Marlin/example_configurations/delta/generic/Configuration.h

   #define UBL_PROBE_PT_2_Y 20
   #define UBL_PROBE_PT_3_X 180
   #define UBL_PROBE_PT_3_Y 20
+  #define UBL_MESH_EDIT_ENABLED     // Enable G26 mesh editing
 
 #elif ENABLED(MESH_BED_LEVELING)
 

Marlin/example_configurations/delta/kossel_mini/Configuration.h

   #define UBL_PROBE_PT_2_Y 20
   #define UBL_PROBE_PT_3_X 180
   #define UBL_PROBE_PT_3_Y 20
+  #define UBL_MESH_EDIT_ENABLED     // Enable G26 mesh editing
 
 #elif ENABLED(MESH_BED_LEVELING)
 

Marlin/example_configurations/delta/kossel_pro/Configuration.h

   #define UBL_PROBE_PT_2_Y 20
   #define UBL_PROBE_PT_3_X 180
   #define UBL_PROBE_PT_3_Y 20
+  #define UBL_MESH_EDIT_ENABLED     // Enable G26 mesh editing
 
 #elif ENABLED(MESH_BED_LEVELING)
 

Marlin/example_configurations/delta/kossel_xl/Configuration.h

   #define UBL_PROBE_PT_2_Y 20
   #define UBL_PROBE_PT_3_X 180
   #define UBL_PROBE_PT_3_Y 20
+  #define UBL_MESH_EDIT_ENABLED     // Enable G26 mesh editing
 
 #elif ENABLED(MESH_BED_LEVELING)
 

Marlin/example_configurations/makibox/Configuration.h

   #define UBL_PROBE_PT_2_Y 20
   #define UBL_PROBE_PT_3_X 180
   #define UBL_PROBE_PT_3_Y 20
+  #define UBL_MESH_EDIT_ENABLED     // Enable G26 mesh editing
 
 #elif ENABLED(MESH_BED_LEVELING)
 

Marlin/example_configurations/tvrrug/Round2/Configuration.h

   #define UBL_PROBE_PT_2_Y 20
   #define UBL_PROBE_PT_3_X 180
   #define UBL_PROBE_PT_3_Y 20
+  #define UBL_MESH_EDIT_ENABLED     // Enable G26 mesh editing
 
 #elif ENABLED(MESH_BED_LEVELING)
 

Marlin/mesh_bed_leveling.h

     }
 
     int8_t probe_index_x(const float &x) const {
-      int8_t px = (x - (MESH_MIN_X) + (MESH_X_DIST) * 0.5) * (1.0 / (MESH_X_DIST));
+      int8_t px = (x - (MESH_MIN_X) + 0.5 * (MESH_X_DIST)) * (1.0 / (MESH_X_DIST));
       return (px >= 0 && px < (MESH_NUM_X_POINTS)) ? px : -1;
     }
 
     int8_t probe_index_y(const float &y) const {
-      int8_t py = (y - (MESH_MIN_Y) + (MESH_Y_DIST) * 0.5) * (1.0 / (MESH_Y_DIST));
+      int8_t py = (y - (MESH_MIN_Y) + 0.5 * (MESH_Y_DIST)) * (1.0 / (MESH_Y_DIST));
       return (py >= 0 && py < (MESH_NUM_Y_POINTS)) ? py : -1;
     }
 

Marlin/planner.cpp

   #endif
 }
 
-#if PLANNER_LEVELING
+#if PLANNER_LEVELING && DISABLED(AUTO_BED_LEVELING_UBL)
   /**
    * lx, ly, lz - logical (cartesian, not delta) positions in mm
    */
     #endif
   }
 
-#endif // PLANNER_LEVELING
+#endif // PLANNER_LEVELING && !AUTO_BED_LEVELING_UBL
 
 /**
  * Planner::_buffer_line
 }
 
 void Planner::set_position_mm_kinematic(const float position[NUM_AXIS]) {
-  #if PLANNER_LEVELING
+  #if PLANNER_LEVELING && DISABLED(AUTO_BED_LEVELING_UBL)
     float lpos[XYZ] = { position[X_AXIS], position[Y_AXIS], position[Z_AXIS] };
     apply_leveling(lpos);
   #else

Marlin/planner.h

 
     static bool is_full() { return (block_buffer_tail == BLOCK_MOD(block_buffer_head + 1)); }
 
-    #if PLANNER_LEVELING
+    #if PLANNER_LEVELING && DISABLED(AUTO_BED_LEVELING_UBL)
 
       #define ARG_X float lx
       #define ARG_Y float ly
      *  extruder     - target extruder
      */
     static FORCE_INLINE void buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, const float &fr_mm_s, const uint8_t extruder) {
-      #if PLANNER_LEVELING && IS_CARTESIAN
+      #if PLANNER_LEVELING && DISABLED(AUTO_BED_LEVELING_UBL) && IS_CARTESIAN
         apply_leveling(lx, ly, lz);
       #endif
       _buffer_line(lx, ly, lz, e, fr_mm_s, extruder);
      *  extruder - target extruder
      */
     static FORCE_INLINE void buffer_line_kinematic(const float ltarget[XYZE], const float &fr_mm_s, const uint8_t extruder) {
-      #if PLANNER_LEVELING
+      #if PLANNER_LEVELING && DISABLED(AUTO_BED_LEVELING_UBL)
         float lpos[XYZ] = { ltarget[X_AXIS], ltarget[Y_AXIS], ltarget[Z_AXIS] };
         apply_leveling(lpos);
       #else
      * Clears previous speed values.
      */
     static FORCE_INLINE void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) {
-      #if PLANNER_LEVELING && IS_CARTESIAN
+      #if PLANNER_LEVELING && DISABLED(AUTO_BED_LEVELING_UBL) && IS_CARTESIAN
         apply_leveling(lx, ly, lz);
       #endif
       _set_position_mm(lx, ly, lz, e);

Marlin/stepper.cpp

 
     // If a minimum pulse time was specified get the CPU clock
     #if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_CODE
-      static uint32_t pulse_start;
-      pulse_start = TCNT0;
+      uint32_t pulse_start = TCNT0;
     #endif
 
     #if HAS_X_STEP
     for (uint8_t i = 0; i < step_loops; i++) {
 
       #if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_E
-        static uint32_t pulse_start;
-        pulse_start = TCNT0;
+        uint32_t pulse_start = TCNT0;
       #endif
 
       START_E_PULSE(0);
 #if ENABLED(BABYSTEPPING)
 
   #define CYCLES_EATEN_BY_BABYSTEP 60
+
   #define _ENABLE(axis) enable_## axis()
   #define _READ_DIR(AXIS) AXIS ##_DIR_READ
   #define _INVERT_DIR(AXIS) INVERT_## AXIS ##_DIR
   #define _APPLY_DIR(AXIS, INVERT) AXIS ##_APPLY_DIR(INVERT, true)
 
+  #if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_BABYSTEP
+    #define _SAVE_START (pulse_start = TCNT0)
+    #define _PULSE_WAIT while ((uint32_t)(TCNT0 - pulse_start) < STEP_PULSE_CYCLES - CYCLES_EATEN_BY_BABYSTEP) { /* nada */ }
+  #else
+    #define _SAVE_START NOOP
+    #define _PULSE_WAIT NOOP
+  #endif
+
   #define START_BABYSTEP_AXIS(AXIS, INVERT) { \
+      old_dir = _READ_DIR(AXIS); \
+      _SAVE_START; \
       _APPLY_DIR(AXIS, _INVERT_DIR(AXIS)^direction^INVERT); \
       _APPLY_STEP(AXIS)(!_INVERT_STEP_PIN(AXIS), true); \
     }
 
   #define STOP_BABYSTEP_AXIS(AXIS) { \
+      _PULSE_WAIT; \
       _APPLY_STEP(AXIS)(_INVERT_STEP_PIN(AXIS), true); \
-      _APPLY_DIR(AXIS, old_pin); \
+      _APPLY_DIR(AXIS, old_dir); \
     }
 
   // MUST ONLY BE CALLED BY AN ISR,
   // No other ISR should ever interrupt this!
   void Stepper::babystep(const AxisEnum axis, const bool direction) {
     cli();
-    static uint8_t old_pin;
+    uint8_t old_dir;
     #if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_BABYSTEP
-      static uint32_t pulse_start;
+      uint32_t pulse_start;
     #endif
     
     switch (axis) {
 
       case X_AXIS:
         _ENABLE(x);
-        old_pin = _READ_DIR(X);
-        #if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_BABYSTEP
-          pulse_start = TCNT0;
-        #endif 
         START_BABYSTEP_AXIS(X, false);
-        #if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_BABYSTEP
-          while ((uint32_t)(TCNT0 - pulse_start) < STEP_PULSE_CYCLES - CYCLES_EATEN_BY_BABYSTEP) { /* nada */ }
-        #endif
         STOP_BABYSTEP_AXIS(X);
         break;
 
       case Y_AXIS:
         _ENABLE(y);
-        old_pin = _READ_DIR(Y);
-        #if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_BABYSTEP
-          pulse_start = TCNT0;
-        #endif
         START_BABYSTEP_AXIS(Y, false);
-        #if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_BABYSTEP
-          while ((uint32_t)(TCNT0 - pulse_start) < STEP_PULSE_CYCLES - CYCLES_EATEN_BY_BABYSTEP) { /* nada */ }
-        #endif
         STOP_BABYSTEP_AXIS(Y);
         break;
 
         #if DISABLED(DELTA)
 
           _ENABLE(z);
-          old_pin = _READ_DIR(Z);
-          #if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_BABYSTEP
-            pulse_start = TCNT0;
-          #endif
           START_BABYSTEP_AXIS(Z, BABYSTEP_INVERT_Z);
-          #if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_BABYSTEP
-            while ((uint32_t)(TCNT0 - pulse_start) < STEP_PULSE_CYCLES - CYCLES_EATEN_BY_BABYSTEP) { /* nada */ }
-          #endif
           STOP_BABYSTEP_AXIS(Z);
 
         #else // DELTA
     sei();
   }
 
-#endif //BABYSTEPPING
+#endif // BABYSTEPPING
 
 /**
  * Software-controlled Stepper Motor Current

Marlin/ultralcd.cpp

 
     static void _lcd_mesh_fine_tune(const char* msg) {
       static millis_t next_click = 0;
-      int16_t last_digit, movement;
+      int16_t last_digit;
       int32_t rounded;
 
       defer_return_to_status = true;

Marlin/vector_3.cpp

 float vector_3::get_length() { return sqrt((x * x) + (y * y) + (z * z)); }
 
 void vector_3::normalize() {
-  float length = get_length();
-  x /= length;
-  y /= length;
-  z /= length;
+  const float inv_length = 1.0 / get_length();
+  x *= inv_length;
+  y *= inv_length;
+  z *= inv_length;
 }
 
 void vector_3::apply_rotation(matrix_3x3 matrix) {
-  float resultX = x * matrix.matrix[3 * 0 + 0] + y * matrix.matrix[3 * 1 + 0] + z * matrix.matrix[3 * 2 + 0];
-  float resultY = x * matrix.matrix[3 * 0 + 1] + y * matrix.matrix[3 * 1 + 1] + z * matrix.matrix[3 * 2 + 1];
-  float resultZ = x * matrix.matrix[3 * 0 + 2] + y * matrix.matrix[3 * 1 + 2] + z * matrix.matrix[3 * 2 + 2];
+  const float resultX = x * matrix.matrix[3 * 0 + 0] + y * matrix.matrix[3 * 1 + 0] + z * matrix.matrix[3 * 2 + 0],
+              resultY = x * matrix.matrix[3 * 0 + 1] + y * matrix.matrix[3 * 1 + 1] + z * matrix.matrix[3 * 2 + 1],
+              resultZ = x * matrix.matrix[3 * 0 + 2] + y * matrix.matrix[3 * 1 + 2] + z * matrix.matrix[3 * 2 + 2];
   x = resultX;
   y = resultY;
   z = resultZ;
   SERIAL_EOL;
 }
 
-void apply_rotation_xyz(matrix_3x3 matrix, float& x, float& y, float& z) {
+void apply_rotation_xyz(matrix_3x3 matrix, float &x, float &y, float &z) {
   vector_3 vector = vector_3(x, y, z);
   vector.apply_rotation(matrix);
   x = vector.x;
 
 void matrix_3x3::debug(const char title[]) {
   SERIAL_PROTOCOLLN(title);
-  int count = 0;
-  for (int i = 0; i < 3; i++) {
-    for (int j = 0; j < 3; j++) {
+  uint8_t count = 0;
+  for (uint8_t i = 0; i < 3; i++) {
+    for (uint8_t j = 0; j < 3; j++) {
       if (matrix[count] >= 0.0) SERIAL_PROTOCOLCHAR('+');
       SERIAL_PROTOCOL_F(matrix[count], 6);
       SERIAL_PROTOCOLCHAR(' ');