Additional UBL fixes, optimizations

.travis.yml

@@ -120,7 +120,7 @@ script:
   # Test a simple build of AUTO_BED_LEVELING_UBL
   #
   - restore_configs
-  - opt_enable AUTO_BED_LEVELING_UBL UBL_G26_MESH_EDITING FIX_MOUNTED_PROBE EEPROM_SETTINGS G3D_PANEL
+  - opt_enable AUTO_BED_LEVELING_UBL UBL_G26_MESH_EDITING ENABLE_LEVELING_FADE_HEIGHT FIX_MOUNTED_PROBE EEPROM_SETTINGS G3D_PANEL
   - build_marlin
   #
   # Test a Sled Z Probe

Marlin/G26_Mesh_Validation_Tool.cpp

@@ -47,8 +47,8 @@
   #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
-                                // less than SIZE_OR_INTERSECTION_CIRCLES
+  #define SIZE_OF_CROSSHAIRS 3 // crosshairs inside the circle.  This number should be
+                               // less than SIZE_OR_INTERSECTION_CIRCLES
 
   /**
    *   Roxy's G26 Mesh Validation Tool
@@ -132,12 +132,12 @@
   void line_to_destination(float );
   void gcode_G28();
   void sync_plan_position_e();
-  void un_retract_filament();
-  void retract_filament();
+  void un_retract_filament(float where[XYZE]);
+  void retract_filament(float where[XYZE]);
   void look_for_lines_to_connect();
   bool parse_G26_parameters();
   void move_to(const float&, const float&, const float&, const float&) ;
-  void print_line_from_here_to_there(float sx, float sy, float sz, float ex, float ey, float ez);
+  void print_line_from_here_to_there(const float&, const float&, const float&, const float&, const float&, const float&);
   bool turn_on_heaters();
   bool prime_nozzle();
   void chirp_at_user();
@@ -154,8 +154,6 @@
 
   float valid_trig_angle(float);
   mesh_index_pair find_closest_circle_to_print(float, float);
-  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 */
 
   static float extrusion_multiplier = EXTRUSION_MULTIPLIER,
                retraction_multiplier = RETRACTION_MULTIPLIER,
@@ -359,7 +357,7 @@
     lcd_reset_alert_level();
     lcd_setstatuspgm(PSTR("Leaving G26"));
 
-    retract_filament();
+    retract_filament(destination);
     destination[Z_AXIS] = Z_CLEARANCE_BETWEEN_PROBES;
 
     //debug_current_and_destination((char*)"ready to do Z-Raise.");
@@ -445,18 +443,12 @@
               // We found two circles that need a horizontal line to connect them
               // Print it!
               //
-              sx = ubl.mesh_index_to_xpos[i];
-              sx = sx + SIZE_OF_INTERSECTION_CIRCLES - SIZE_OF_CROSS_HAIRS; // get the right edge of the circle
-              sy = ubl.mesh_index_to_ypos[j];
+              sx = ubl.mesh_index_to_xpos[  i  ] + (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // right edge
+              ex = ubl.mesh_index_to_xpos[i + 1] - (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // left edge
 
-              ex = ubl.mesh_index_to_xpos[i + 1];
-              ex = ex - SIZE_OF_INTERSECTION_CIRCLES + SIZE_OF_CROSS_HAIRS; // get the left edge of the circle
-              ey = sy;
-
-              sx = constrain(sx, X_MIN_POS + 1, X_MAX_POS - 1);             // This keeps us from bumping the endstops
-              sy = constrain(sy, Y_MIN_POS + 1, Y_MAX_POS - 1);
+              sx = constrain(sx, X_MIN_POS + 1, X_MAX_POS - 1);
+              sy = ey = constrain(ubl.mesh_index_to_ypos[j], Y_MIN_POS + 1, Y_MAX_POS - 1);
               ex = constrain(ex, X_MIN_POS + 1, X_MAX_POS - 1);
-              ey = constrain(ey, Y_MIN_POS + 1, Y_MAX_POS - 1);
 
               if (ubl.g26_debug_flag) {
                 SERIAL_ECHOPAIR(" Connecting with horizontal line (sx=", sx);
@@ -468,7 +460,7 @@
                 //debug_current_and_destination((char*)"Connecting horizontal line.");
               }
 
-              print_line_from_here_to_there(sx, sy, layer_height, ex, ey, layer_height);
+              print_line_from_here_to_there(LOGICAL_X_POSITION(sx), LOGICAL_Y_POSITION(sy), layer_height, LOGICAL_X_POSITION(ex), LOGICAL_Y_POSITION(ey), layer_height);
               bit_set(horizontal_mesh_line_flags, i, j);   // Mark it as done so we don't do it again
             }
           }
@@ -482,17 +474,11 @@
                 // We found two circles that need a vertical line to connect them
                 // Print it!
                 //
-                sx = ubl.mesh_index_to_xpos[i];
-                sy = ubl.mesh_index_to_ypos[j];
-                sy = sy + SIZE_OF_INTERSECTION_CIRCLES - SIZE_OF_CROSS_HAIRS; // get the top edge of the circle
-
-                ex = sx;
-                ey = ubl.mesh_index_to_ypos[j + 1];
-                ey = ey - SIZE_OF_INTERSECTION_CIRCLES + SIZE_OF_CROSS_HAIRS; // get the bottom edge of the circle
+                sy = ubl.mesh_index_to_ypos[  j  ] + (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // top edge
+                ey = ubl.mesh_index_to_ypos[j + 1] - (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // bottom edge
 
-                sx = constrain(sx, X_MIN_POS + 1, X_MAX_POS - 1);             // This keeps us from bumping the endstops
+                sx = ex = constrain(ubl.mesh_index_to_xpos[i], X_MIN_POS + 1, X_MAX_POS - 1);
                 sy = constrain(sy, Y_MIN_POS + 1, Y_MAX_POS - 1);
-                ex = constrain(ex, X_MIN_POS + 1, X_MAX_POS - 1);
                 ey = constrain(ey, Y_MIN_POS + 1, Y_MAX_POS - 1);
 
                 if (ubl.g26_debug_flag) {
@@ -504,8 +490,8 @@
                   SERIAL_EOL;
                   debug_current_and_destination((char*)"Connecting vertical line.");
                 }
-                print_line_from_here_to_there(sx, sy, layer_height, ex, ey, layer_height);
-                bit_set( vertical_mesh_line_flags, i, j);   // Mark it as done so we don't do it again
+                print_line_from_here_to_there(LOGICAL_X_POSITION(sx), LOGICAL_Y_POSITION(sy), layer_height, LOGICAL_X_POSITION(ex), LOGICAL_Y_POSITION(ey), layer_height);
+                bit_set(vertical_mesh_line_flags, i, j);   // Mark it as done so we don't do it again
               }
             }
           }
@@ -533,7 +519,7 @@
       destination[Z_AXIS] = z;                          // We know the last_z==z or we wouldn't be in this block of code.
       destination[E_AXIS] = current_position[E_AXIS];
 
-      ubl_line_to_destination(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feed_value, 0);
+      ubl_line_to_destination(feed_value, 0);
 
       stepper.synchronize();
       set_destination_to_current();
@@ -553,7 +539,7 @@
 
     //if (ubl.g26_debug_flag) debug_current_and_destination((char*)" in move_to() doing last move");
 
-    ubl_line_to_destination(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feed_value, 0);
+    ubl_line_to_destination(feed_value, 0);
 
     //if (ubl.g26_debug_flag) debug_current_and_destination((char*)" in move_to() after last move");
 
@@ -562,18 +548,18 @@
 
   }
 
-  void retract_filament() {
+  void retract_filament(float where[XYZE]) {
     if (!g26_retracted) { // Only retract if we are not already retracted!
       g26_retracted = true;
       //if (ubl.g26_debug_flag) SERIAL_ECHOLNPGM(" Decided to do retract.");
-      move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], -1.0 * retraction_multiplier);
+      move_to(where[X_AXIS], where[Y_AXIS], where[Z_AXIS], -1.0 * retraction_multiplier);
       //if (ubl.g26_debug_flag) SERIAL_ECHOLNPGM(" Retraction done.");
     }
   }
 
-  void un_retract_filament() {
+  void un_retract_filament(float where[XYZE]) {
     if (g26_retracted) { // Only un-retract if we are retracted.
-      move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 1.2 * retraction_multiplier);
+      move_to(where[X_AXIS], where[Y_AXIS], where[Z_AXIS], 1.2 * retraction_multiplier);
       g26_retracted = false;
       //if (ubl.g26_debug_flag) SERIAL_ECHOLNPGM(" unretract done.");
     }
@@ -594,7 +580,7 @@
    * segment of a 'circle'.   The time this requires is very short and is easily saved by the other
    * cases where the optimization comes into play.
    */
-  void print_line_from_here_to_there( float sx, float sy, float sz, float ex, float ey, float ez) {
+  void print_line_from_here_to_there(const float &sx, const float &sy, const float &sz, const float &ex, const float &ey, const float &ez) {
     const float dx_s = current_position[X_AXIS] - sx,   // find our distance from the start of the actual line segment
                 dy_s = current_position[Y_AXIS] - sy,
                 dist_start = HYPOT2(dx_s, dy_s),        // We don't need to do a sqrt(), we can compare the distance^2
@@ -603,31 +589,26 @@
                 dy_e = current_position[Y_AXIS] - ey,
                 dist_end = HYPOT2(dx_e, dy_e),
 
-                dx = ex - sx,
-                dy = ey - sy,
-                line_length = HYPOT(dx, dy);
+                line_length = HYPOT(ex - sx, ey - sy);
 
-    // If the end point of the line is closer to the nozzle, we are going to
-    // flip the direction of this line.   We will print it from the end to the start.
-    // On very small lines we don't do the optimization because it just isn't worth it.
-    //
+    // If the end point of the line is closer to the nozzle, flip the direction,
+    // moving from the end to the start. On very small lines the optimization isn't worth it.
     if (dist_end < dist_start && (SIZE_OF_INTERSECTION_CIRCLES) < abs(line_length)) {
       //if (ubl.g26_debug_flag) SERIAL_ECHOLNPGM("  Reversing start and end of print_line_from_here_to_there()");
-      print_line_from_here_to_there(ex, ey, ez, sx, sy, sz);
-      return;
+      return print_line_from_here_to_there(ex, ey, ez, sx, sy, sz);
     }
 
-    // Now decide if we should retract.
+    // Decide whether to retract.
 
     if (dist_start > 2.0) {
-      retract_filament();
+      retract_filament(destination);
       //if (ubl.g26_debug_flag) SERIAL_ECHOLNPGM("  filament retracted.");
     }
     move_to(sx, sy, sz, 0.0); // Get to the starting point with no extrusion
 
     const float e_pos_delta = line_length * g26_e_axis_feedrate * extrusion_multiplier;
 
-    un_retract_filament();
+    un_retract_filament(destination);
 
     //if (ubl.g26_debug_flag) {
     //  SERIAL_ECHOLNPGM("  doing printing move.");
@@ -814,6 +795,7 @@
       lcd_setstatuspgm(PSTR(""));
       lcd_quick_feedback();
     #endif
+
     return UBL_OK;
   }
 
@@ -832,9 +814,8 @@
 
       set_destination_to_current();
 
-      un_retract_filament();    // Lets make sure the G26 command doesn't think the filament is
-                                // retracted().  We are here because we want to prime the nozzle.
-                                // So let's just unretract just to be sure.
+      un_retract_filament(destination); // Make sure G26 doesn't think the filament is retracted().
+
       while (!ubl_lcd_clicked()) {
         chirp_at_user();
         destination[E_AXIS] += 0.25;
@@ -842,10 +823,7 @@
           Total_Prime += 0.25;
           if (Total_Prime >= EXTRUDE_MAXLENGTH) return UBL_ERR;
         #endif
-        ubl_line_to_destination(
-          destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS],
-          planner.max_feedrate_mm_s[E_AXIS] / 15.0, 0
-        );
+        ubl_line_to_destination(planner.max_feedrate_mm_s[E_AXIS] / 15.0, 0);
 
         stepper.synchronize();    // Without this synchronize, the purge is more consistent,
                                   // but because the planner has a buffer, we won't be able
@@ -874,13 +852,10 @@
       #endif
       set_destination_to_current();
       destination[E_AXIS] += prime_length;
-      ubl_line_to_destination(
-        destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS],
-        planner.max_feedrate_mm_s[E_AXIS] / 15.0, 0
-      );
+      ubl_line_to_destination(planner.max_feedrate_mm_s[E_AXIS] / 15.0, 0);
       stepper.synchronize();
       set_destination_to_current();
-      retract_filament();
+      retract_filament(destination);
     }
 
     return UBL_OK;

Marlin/Marlin_main.cpp

@@ -9901,11 +9901,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
       #elif ENABLED(AUTO_BED_LEVELING_UBL)
         if (ubl.state.active) {
 
-//        ubl_line_to_destination(MMS_SCALED(feedrate_mm_s));
-
-          ubl_line_to_destination(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS],
-//                      (feedrate*(1.0/60.0))*(feedrate_percentage*(1.0/100.0) ), active_extruder);
-                      MMS_SCALED(feedrate_mm_s), active_extruder);
+          ubl_line_to_destination(MMS_SCALED(feedrate_mm_s), active_extruder);
 
           return false;
         }

Marlin/UBL.h

@@ -43,7 +43,7 @@
     bool ubl_lcd_clicked();
     void probe_entire_mesh(const float&, const float&, const bool, 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 ubl_line_to_destination(const float&, uint8_t);
     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&);
@@ -193,22 +193,16 @@
          *  multiplications.
          */
         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;
+          return z1 + (z2 - z1) * (a0 - a1) / (a2 - a1);
         }
 
         /**
-         * get_z_correction_at_Y_intercept(float x0, int x1_i, int yi) only takes
-         * three parameters. It assumes the x0 point is on a Mesh line denoted by yi. In theory
-         * we could use get_cell_index_x(float x) to obtain the 2nd parameter x1_i but any code calling
-         * the get_z_correction_along_vertical_mesh_line_at_specific_X routine  will already have
-         * the X index of the x0 intersection available and we don't want to perform any extra floating
-         * point operations.
+         * z_correction_for_x_on_horizontal_mesh_line is an optimization for
+         * the rare occasion when a point lies exactly on a Mesh line (denoted by index yi).
          */
-        static 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);
+        static inline float z_correction_for_x_on_horizontal_mesh_line(const float &lx0, const int x1_i, const int yi) {
+          if (!WITHIN(x1_i, 0, UBL_MESH_NUM_X_POINTS - 1) || !WITHIN(yi, 0, UBL_MESH_NUM_Y_POINTS - 1)) {
+            SERIAL_ECHOPAIR("? in z_correction_for_x_on_horizontal_mesh_line(lx0=", lx0);
             SERIAL_ECHOPAIR(",x1_i=", x1_i);
             SERIAL_ECHOPAIR(",yi=", yi);
             SERIAL_CHAR(')');
@@ -216,20 +210,18 @@
             return NAN;
           }
 
-          const float xratio = (RAW_X_POSITION(x0) - mesh_index_to_xpos[x1_i]) * (1.0 / (MESH_X_DIST)),
-                      z1 = z_values[x1_i][yi],
-                      z2 = z_values[x1_i + 1][yi],
-                      dz = (z2 - z1);
+          const float xratio = (RAW_X_POSITION(lx0) - mesh_index_to_xpos[x1_i]) * (1.0 / (MESH_X_DIST)),
+                      z1 = z_values[x1_i][yi];
 
-          return z1 + xratio * dz;
+          return z1 + xratio * (z_values[x1_i + 1][yi] - z1);
         }
 
         //
-        // See comments above for get_z_correction_along_horizontal_mesh_line_at_specific_X
+        // See comments above for z_correction_for_x_on_horizontal_mesh_line
         //
-        static 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);
+        static inline float z_correction_for_y_on_vertical_mesh_line(const float &ly0, const int xi, const int y1_i) {
+          if (!WITHIN(xi, 0, UBL_MESH_NUM_X_POINTS - 1) || !WITHIN(y1_i, 0, UBL_MESH_NUM_Y_POINTS - 1)) {
+            SERIAL_ECHOPAIR("? in get_z_correction_along_vertical_mesh_line_at_specific_x(ly0=", ly0);
             SERIAL_ECHOPAIR(", x1_i=", xi);
             SERIAL_ECHOPAIR(", yi=", y1_i);
             SERIAL_CHAR(')');
@@ -237,12 +229,10 @@
             return NAN;
           }
 
-          const float yratio = (RAW_Y_POSITION(y0) - mesh_index_to_ypos[y1_i]) * (1.0 / (MESH_Y_DIST)),
-                      z1 = z_values[xi][y1_i],
-                      z2 = z_values[xi][y1_i + 1],
-                      dz = (z2 - z1);
+          const float yratio = (RAW_Y_POSITION(ly0) - mesh_index_to_ypos[y1_i]) * (1.0 / (MESH_Y_DIST)),
+                      z1 = z_values[xi][y1_i];
 
-          return z1 + yratio * dz;
+          return z1 + yratio * (z_values[xi][y1_i + 1] - z1);
         }
 
         /**
@@ -251,14 +241,14 @@
          * Z-Height at both ends. Then it does a linear interpolation of these heights based
          * on the Y position within the cell.
          */
-        static float get_z_correction(const float &x0, const float &y0) {
-          const int8_t cx = get_cell_index_x(RAW_X_POSITION(x0)),
-                       cy = get_cell_index_y(RAW_Y_POSITION(y0));
+        static float get_z_correction(const float &lx0, const float &ly0) {
+          const int8_t cx = get_cell_index_x(RAW_X_POSITION(lx0)),
+                       cy = get_cell_index_y(RAW_Y_POSITION(ly0));
 
-          if (cx < 0 || cy < 0 || cx >= UBL_MESH_NUM_X_POINTS || cy >= UBL_MESH_NUM_Y_POINTS) {
+          if (!WITHIN(cx, 0, UBL_MESH_NUM_X_POINTS - 1) || !WITHIN(cy, 0, UBL_MESH_NUM_Y_POINTS - 1)) {
 
-            SERIAL_ECHOPAIR("? in get_z_correction(x0=", x0);
-            SERIAL_ECHOPAIR(", y0=", y0);
+            SERIAL_ECHOPAIR("? in get_z_correction(lx0=", lx0);
+            SERIAL_ECHOPAIR(", ly0=", ly0);
             SERIAL_CHAR(')');
             SERIAL_EOL;
 
@@ -269,21 +259,21 @@
             return 0.0; // this used to return state.z_offset
           }
 
-          const float z1 = calc_z0(RAW_X_POSITION(x0),
+          const float z1 = calc_z0(RAW_X_POSITION(lx0),
                         mesh_index_to_xpos[cx], z_values[cx][cy],
                         mesh_index_to_xpos[cx + 1], z_values[cx + 1][cy]),
-                      z2 = calc_z0(RAW_X_POSITION(x0),
+                      z2 = calc_z0(RAW_X_POSITION(lx0),
                         mesh_index_to_xpos[cx], z_values[cx][cy + 1],
                         mesh_index_to_xpos[cx + 1], z_values[cx + 1][cy + 1]);
-                float z0 = calc_z0(RAW_Y_POSITION(y0),
+                float z0 = calc_z0(RAW_Y_POSITION(ly0),
                     mesh_index_to_ypos[cy], z1,
                     mesh_index_to_ypos[cy + 1], z2);
 
           #if ENABLED(DEBUG_LEVELING_FEATURE)
             if (DEBUGGING(MESH_ADJUST)) {
-              SERIAL_ECHOPAIR(" raw get_z_correction(", x0);
+              SERIAL_ECHOPAIR(" raw get_z_correction(", lx0);
               SERIAL_CHAR(',')
-              SERIAL_ECHO(y0);
+              SERIAL_ECHO(ly0);
               SERIAL_ECHOPGM(") = ");
               SERIAL_ECHO_F(z0, 6);
             }
@@ -305,9 +295,9 @@
 
             #if ENABLED(DEBUG_LEVELING_FEATURE)
               if (DEBUGGING(MESH_ADJUST)) {
-                SERIAL_ECHOPAIR("??? Yikes!  NAN in get_z_correction(", x0);
+                SERIAL_ECHOPAIR("??? Yikes!  NAN in get_z_correction(", lx0);
                 SERIAL_CHAR(',');
-                SERIAL_ECHO(y0);
+                SERIAL_ECHO(ly0);
                 SERIAL_CHAR(')');
                 SERIAL_EOL;
               }
@@ -327,7 +317,7 @@
          */
         #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
 
-          FORCE_INLINE float fade_scaling_factor_for_z(const float &lz) {
+          static FORCE_INLINE float fade_scaling_factor_for_z(const float &lz) {
             const float rz = RAW_Z_POSITION(lz);
             if (last_specified_z != rz) {
               last_specified_z = rz;

Marlin/UBL_Bed_Leveling.cpp

@@ -203,7 +203,7 @@
 
         const float f = z_values[i][j];
         if (isnan(f)) {
-          serialprintPGM(map0 ? PSTR("    .    ") : PSTR("NAN"));
+          serialprintPGM(map0 ? PSTR("   .  ") : PSTR("NAN"));
         }
         else {
           // if we don't do this, the columns won't line up nicely

Marlin/UBL_G29.cpp

@@ -515,16 +515,23 @@
     }
 
     if (code_seen('T')) {
-      float z1 = probe_pt(ubl_3_point_1_X, ubl_3_point_1_Y, false /*Stow Flag*/, g29_verbose_level),
-            z2 = probe_pt(ubl_3_point_2_X, ubl_3_point_2_Y, false /*Stow Flag*/, g29_verbose_level),
-            z3 = probe_pt(ubl_3_point_3_X, ubl_3_point_3_Y, true  /*Stow Flag*/, g29_verbose_level);
+      const float lx1 = LOGICAL_X_POSITION(ubl_3_point_1_X),
+                  lx2 = LOGICAL_X_POSITION(ubl_3_point_2_X),
+                  lx3 = LOGICAL_X_POSITION(ubl_3_point_3_X),
+                  ly1 = LOGICAL_Y_POSITION(ubl_3_point_1_Y),
+                  ly2 = LOGICAL_Y_POSITION(ubl_3_point_2_Y),
+                  ly3 = LOGICAL_Y_POSITION(ubl_3_point_3_Y);
+
+      float z1 = probe_pt(lx1, ly1, false /*Stow Flag*/, g29_verbose_level),
+            z2 = probe_pt(lx2, ly2, false /*Stow Flag*/, g29_verbose_level),
+            z3 = probe_pt(lx3, ly3, true  /*Stow Flag*/, g29_verbose_level);
 
       //  We need to adjust z1, z2, z3 by the Mesh Height at these points. Just because they are non-zero doesn't mean
       //  the Mesh is tilted!  (We need to compensate each probe point by what the Mesh says that location's height is)
 
-      z1 -= ubl.get_z_correction(ubl_3_point_1_X, ubl_3_point_1_Y);
-      z2 -= ubl.get_z_correction(ubl_3_point_2_X, ubl_3_point_2_Y);
-      z3 -= ubl.get_z_correction(ubl_3_point_3_X, ubl_3_point_3_Y);
+      z1 -= ubl.get_z_correction(lx1, ly1);
+      z2 -= ubl.get_z_correction(lx2, ly2);
+      z3 -= ubl.get_z_correction(lx3, ly3);
 
       do_blocking_move_to_xy((X_MAX_POS - (X_MIN_POS)) / 2.0, (Y_MAX_POS - (Y_MIN_POS)) / 2.0);
       tilt_mesh_based_on_3pts(z1, z2, z3);
@@ -778,17 +785,17 @@
     );
   }
 
-  vector_3 tilt_mesh_based_on_3pts(const float &pt1, const float &pt2, const float &pt3) {
+  vector_3 tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3) {
     float c, d, t;
     int i, j;
 
     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) ),
+                            (z1 - z2) ),
 
              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) ),
+                            (z3 - z2) ),
 
              normal = vector_3::cross(v1, v2);
 
@@ -810,7 +817,7 @@
     // All of 3 of these points should give us the same d constant
     //
     t = normal.x * ubl_3_point_1_X + normal.y * ubl_3_point_1_Y;
-    d = t + normal.z * pt1;
+    d = t + normal.z * z1;
     c = d - t;
     SERIAL_ECHOPGM("d from 1st point: ");
     SERIAL_ECHO_F(d, 6);
@@ -818,7 +825,7 @@
     SERIAL_ECHO_F(c, 6);
     SERIAL_EOL;
     t = normal.x * ubl_3_point_2_X + normal.y * ubl_3_point_2_Y;
-    d = t + normal.z * pt2;
+    d = t + normal.z * z2;
     c = d - t;
     SERIAL_ECHOPGM("d from 2nd point: ");
     SERIAL_ECHO_F(d, 6);
@@ -826,7 +833,7 @@
     SERIAL_ECHO_F(c, 6);
     SERIAL_EOL;
     t = normal.x * ubl_3_point_3_X + normal.y * ubl_3_point_3_Y;
-    d = t + normal.z * pt3;
+    d = t + normal.z * z3;
     c = d - t;
     SERIAL_ECHOPGM("d from 3rd point: ");
     SERIAL_ECHO_F(d, 6);
@@ -1425,4 +1432,4 @@
     SERIAL_ECHOLNPGM("Done Editing Mesh.");
   }
 
-#endif // AUTO_BED_LEVELING_UBL
+#endif // AUTO_BED_LEVELING_UBL

Marlin/UBL_line_to_destination.cpp

@@ -31,7 +31,14 @@
 
   extern float destination[XYZE];
   extern void set_current_to_destination();
-  extern float destination[];
+
+  static void debug_echo_axis(const AxisEnum axis) {
+    if (current_position[axis] == destination[axis])
+      SERIAL_ECHOPGM("-------------");
+    else
+      SERIAL_ECHO_F(destination[X_AXIS], 6);
+  }
+
   void debug_current_and_destination(char *title) {
 
     // if the title message starts with a '!' it is so important, we are going to
@@ -67,32 +74,13 @@
     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);
-
+    debug_echo_axis(X_AXIS);
     SERIAL_ECHOPGM(", ");
-
-    if (current_position[Y_AXIS] == destination[Y_AXIS])
-      SERIAL_ECHOPGM("-------------");
-    else
-      SERIAL_ECHO_F(destination[Y_AXIS], 6);
-
+    debug_echo_axis(Y_AXIS);
     SERIAL_ECHOPGM(", ");
-
-    if (current_position[Z_AXIS] == destination[Z_AXIS])
-      SERIAL_ECHOPGM("-------------");
-    else
-      SERIAL_ECHO_F(destination[Z_AXIS], 6);
-
+    debug_echo_axis(Z_AXIS);
     SERIAL_ECHOPGM(", ");
-
-    if (current_position[E_AXIS] == destination[E_AXIS])
-      SERIAL_ECHOPGM("-------------");
-    else
-      SERIAL_ECHO_F(destination[E_AXIS], 6);
-
+    debug_echo_axis(E_AXIS);
     SERIAL_ECHOPGM(" )   ");
     SERIAL_ECHO(title);
     SERIAL_EOL;
@@ -105,32 +93,37 @@
     //}
   }
 
-  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 ubl_line_to_destination(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];
-
-    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));
+    const float start[XYZE] = {
+                  current_position[X_AXIS],
+                  current_position[Y_AXIS],
+                  current_position[Z_AXIS],
+                  current_position[E_AXIS]
+                },
+                end[XYZE] = {
+                  destination[X_AXIS],
+                  destination[Y_AXIS],
+                  destination[Z_AXIS],
+                  destination[E_AXIS]
+                };
+
+    const int cell_start_xi = ubl.get_cell_index_x(RAW_X_POSITION(start[X_AXIS])),
+              cell_start_yi = ubl.get_cell_index_y(RAW_Y_POSITION(start[Y_AXIS])),
+              cell_dest_xi  = ubl.get_cell_index_x(RAW_X_POSITION(end[X_AXIS])),
+              cell_dest_yi  = ubl.get_cell_index_y(RAW_Y_POSITION(end[Y_AXIS]));
 
     if (ubl.g26_debug_flag) {
-      SERIAL_ECHOPGM(" ubl_line_to_destination(xe=");
-      SERIAL_ECHO(x_end);
-      SERIAL_ECHOPGM(", ye=");
-      SERIAL_ECHO(y_end);
-      SERIAL_ECHOPGM(", ze=");
-      SERIAL_ECHO(z_end);
-      SERIAL_ECHOPGM(", ee=");
-      SERIAL_ECHO(e_end);
-      SERIAL_ECHOLNPGM(")");
+      SERIAL_ECHOPAIR(" ubl_line_to_destination(xe=", end[X_AXIS]);
+      SERIAL_ECHOPAIR(", ye=", end[Y_AXIS]);
+      SERIAL_ECHOPAIR(", ze=", end[Z_AXIS]);
+      SERIAL_ECHOPAIR(", ee=", end[E_AXIS]);
+      SERIAL_CHAR(')');
+      SERIAL_EOL;
       debug_current_and_destination((char*)"Start of ubl_line_to_destination()");
     }
 
@@ -142,12 +135,12 @@
        * But we detect it and isolate it. For now, we just pass along the request.
        */
 
-      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) {
+      if (!WITHIN(cell_dest_xi, 0, UBL_MESH_NUM_X_POINTS - 1) || !WITHIN(cell_dest_yi, 0, UBL_MESH_NUM_Y_POINTS - 1)) {
 
         // 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);
+        planner.buffer_line(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + ubl.state.z_offset, end[E_AXIS], feed_rate, extruder);
         set_current_to_destination();
 
         if (ubl.g26_debug_flag)
@@ -167,7 +160,7 @@
        * to create a 1-over number for us. That will allow us to do a floating point multiply instead of a floating point divide.
        */
 
-      const float xratio = (RAW_X_POSITION(x_end) - ubl.mesh_index_to_xpos[cell_dest_xi]) * (1.0 / (MESH_X_DIST)),
+      const float xratio = (RAW_X_POSITION(end[X_AXIS]) - ubl.mesh_index_to_xpos[cell_dest_xi]) * (1.0 / (MESH_X_DIST)),
                   z1 = ubl.z_values[cell_dest_xi    ][cell_dest_yi    ] + xratio *
                       (ubl.z_values[cell_dest_xi + 1][cell_dest_yi    ] - ubl.z_values[cell_dest_xi][cell_dest_yi    ]),
                   z2 = ubl.z_values[cell_dest_xi    ][cell_dest_yi + 1] + xratio *
@@ -176,7 +169,7 @@
       // 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.
 
-      const float yratio = (RAW_Y_POSITION(y_end) - ubl.mesh_index_to_ypos[cell_dest_yi]) * (1.0 / (MESH_Y_DIST));
+      const float yratio = (RAW_Y_POSITION(end[Y_AXIS]) - ubl.mesh_index_to_ypos[cell_dest_yi]) * (1.0 / (MESH_Y_DIST));
 
       float z0 = z1 + (z2 - z1) * yratio;
 
@@ -186,20 +179,20 @@
        */
       /*
         z_optimized = z0;
-        z0 = ubl.get_z_correction(x_end, y_end);
+        z0 = ubl.get_z_correction(end[X_AXIS], end[Y_AXIS]);
         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  ");
         if (isnan(z_optimized)) SERIAL_ECHO(" z_optimized==NAN  ");
-        SERIAL_ECHOPAIR("  x_end=", x_end);
-        SERIAL_ECHOPAIR("  y_end=", y_end);
+        SERIAL_ECHOPAIR("  end[X_AXIS]=", end[X_AXIS]);
+        SERIAL_ECHOPAIR("  end[Y_AXIS]=", end[Y_AXIS]);
         SERIAL_ECHOPAIR("  z0=", z0);
         SERIAL_ECHOPAIR("  z_optimized=", z_optimized);
         SERIAL_ECHOPAIR("  err=",fabs(z_optimized - z0));
         SERIAL_EOL;
         }
       //*/
-      z0 *= ubl.fade_scaling_factor_for_z(z_end);
+      z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]);
 
       /**
        * If part of the Mesh is undefined, it will show up as NAN
@@ -210,7 +203,7 @@
        */
       if (isnan(z0)) z0 = 0.0;
 
-      planner.buffer_line(x_end, y_end, z_end + z0 + ubl.state.z_offset, e_end, feed_rate, extruder);
+      planner.buffer_line(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0 + ubl.state.z_offset, end[E_AXIS], feed_rate, extruder);
 
       if (ubl.g26_debug_flag)
         debug_current_and_destination((char*)"FINAL_MOVE in ubl_line_to_destination()");
@@ -227,8 +220,8 @@
      * blocks of code:
      */
 
-    const float dx = x_end - x_start,
-                dy = y_end - y_start;
+    const float dx = end[X_AXIS] - start[X_AXIS],
+                dy = end[Y_AXIS] - start[Y_AXIS];
 
     const int left_flag = dx < 0.0 ? 1 : 0,
               down_flag = dy < 0.0 ? 1 : 0;
@@ -251,8 +244,8 @@
     const bool use_x_dist = adx > ady;
 
     float on_axis_distance = use_x_dist ? dx : dy,
-          e_position = e_end - e_start,
-          z_position = z_end - z_start;
+          e_position = end[E_AXIS] - start[E_AXIS],
+          z_position = end[Z_AXIS] - start[Z_AXIS];
 
     const float e_normalized_dist = e_position / on_axis_distance,
                 z_normalized_dist = z_position / on_axis_distance;
@@ -260,7 +253,7 @@
     int current_xi = cell_start_xi, current_yi = cell_start_yi;
 
     const float m = dy / dx,
-                c = y_start - m * x_start;
+                c = start[Y_AXIS] - m * start[X_AXIS];
 
     const bool inf_normalized_flag = NEAR_ZERO(on_axis_distance),
                inf_m_flag = NEAR_ZERO(dx);
@@ -281,9 +274,9 @@
          * else, we know the next X is the same so we can recover and continue!
          * Calculate X at the next Y mesh line
          */
-        const float x = inf_m_flag ? x_start : (next_mesh_line_y - c) / m;
+        const float x = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m;
 
-        float z0 = ubl.get_z_correction_along_horizontal_mesh_line_at_specific_X(x, current_xi, current_yi);
+        float z0 = ubl.z_correction_for_x_on_horizontal_mesh_line(x, current_xi, current_yi);
 
         /**
          * Debug code to use non-optimized get_z_correction() and to do a sanity check
@@ -305,7 +298,7 @@
           }
         //*/
 
-        z0 *= ubl.fade_scaling_factor_for_z(z_end);
+        z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]);
 
         /**
          * If part of the Mesh is undefined, it will show up as NAN
@@ -324,15 +317,15 @@
          * happens, it might be best to remove the check and always 'schedule' the move because
          * the planner.buffer_line() routine will filter it if that happens.
          */
-        if (y != y_start) {
+        if (y != start[Y_AXIS]) {
           if (!inf_normalized_flag) {
-            on_axis_distance = y - y_start;                               // we don't need to check if the extruder position
-            e_position = e_start + on_axis_distance * e_normalized_dist;  // is based on X or Y because this is a vertical move
-            z_position = z_start + on_axis_distance * z_normalized_dist;
+            on_axis_distance = y - start[Y_AXIS];                               // we don't need to check if the extruder position
+            e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;  // is based on X or Y because this is a vertical move
+            z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
           }
           else {
-            e_position = e_start;
-            z_position = z_start;
+            e_position = start[E_AXIS];
+            z_position = start[Z_AXIS];
           }
 
           planner.buffer_line(x, y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder);
@@ -345,7 +338,7 @@
       //
       // Check if we are at the final destination. Usually, we won't be, but if it is on a Y Mesh Line, we are done.
       //
-      if (current_position[X_AXIS] != x_end || current_position[Y_AXIS] != y_end)
+      if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
         goto FINAL_MOVE;
 
       set_current_to_destination();
@@ -368,7 +361,7 @@
         const float next_mesh_line_x = LOGICAL_X_POSITION(ubl.mesh_index_to_xpos[current_xi]),
                     y = m * next_mesh_line_x + c;   // Calculate X at the next Y mesh line
 
-        float z0 = ubl.get_z_correction_along_vertical_mesh_line_at_specific_Y(y, current_xi, current_yi);
+        float z0 = ubl.z_correction_for_y_on_vertical_mesh_line(y, current_xi, current_yi);
 
         /**
          * Debug code to use non-optimized get_z_correction() and to do a sanity check
@@ -390,7 +383,7 @@
           }
         //*/
 
-        z0 = z0 * ubl.fade_scaling_factor_for_z(z_end);
+        z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]);
 
         /**
          * If part of the Mesh is undefined, it will show up as NAN
@@ -409,15 +402,15 @@
          * that happens, it might be best to remove the check and always 'schedule' the move because
          * the planner.buffer_line() routine will filter it if that happens.
          */
-        if (x != x_start) {
+        if (x != start[X_AXIS]) {
           if (!inf_normalized_flag) {
-            on_axis_distance = x - x_start;                               // we don't need to check if the extruder position
-            e_position = e_start + on_axis_distance * e_normalized_dist;  // is based on X or Y because this is a horizontal move
-            z_position = z_start + on_axis_distance * z_normalized_dist;
+            on_axis_distance = x - start[X_AXIS];                               // we don't need to check if the extruder position
+            e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;  // is based on X or Y because this is a horizontal move
+            z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
           }
           else {
-            e_position = e_start;
-            z_position = z_start;
+            e_position = start[E_AXIS];
+            z_position = start[Z_AXIS];
           }
 
           planner.buffer_line(x, y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder);
@@ -427,7 +420,7 @@
       if (ubl.g26_debug_flag)
         debug_current_and_destination((char*)"horizontal move done in ubl_line_to_destination()");
 
-      if (current_position[X_AXIS] != x_end || current_position[Y_AXIS] != y_end)
+      if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
         goto FINAL_MOVE;
 
       set_current_to_destination();
@@ -454,16 +447,16 @@
       const float next_mesh_line_x = LOGICAL_X_POSITION(ubl.mesh_index_to_xpos[current_xi + dxi]),
                   next_mesh_line_y = LOGICAL_Y_POSITION(ubl.mesh_index_to_ypos[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.
+                  x = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line
+                                                  // (No need to worry about m being zero.
+                                                  //  If that was the case, it was already detected
+                                                  //  as a vertical line move above.)
 
       if (left_flag == (x > next_mesh_line_x)) { // Check if we hit the Y line first
         //
         // Yes!  Crossing a Y Mesh Line next
         //
-        float z0 = ubl.get_z_correction_along_horizontal_mesh_line_at_specific_X(x, current_xi - left_flag, current_yi + dyi);
+        float z0 = ubl.z_correction_for_x_on_horizontal_mesh_line(x, current_xi - left_flag, current_yi + dyi);
 
         /**
          * Debug code to use non-optimized get_z_correction() and to do a sanity check
@@ -486,7 +479,7 @@
           }
         //*/
 
-        z0 *= ubl.fade_scaling_factor_for_z(z_end);
+        z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]);
 
         /**
          * If part of the Mesh is undefined, it will show up as NAN
@@ -498,13 +491,13 @@
         if (isnan(z0)) z0 = 0.0;
 
         if (!inf_normalized_flag) {
-          on_axis_distance = use_x_dist ? x - x_start : next_mesh_line_y - y_start;
-          e_position = e_start + on_axis_distance * e_normalized_dist;
-          z_position = z_start + on_axis_distance * z_normalized_dist;
+          on_axis_distance = use_x_dist ? x - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS];
+          e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
+          z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
         }
         else {
-          e_position = e_start;
-          z_position = z_start;
+          e_position = start[E_AXIS];
+          z_position = start[Z_AXIS];
         }
         planner.buffer_line(x, next_mesh_line_y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder);
         current_yi += dyi;
@@ -514,7 +507,7 @@
         //
         // Yes!  Crossing a X Mesh Line next
         //
-        float z0 = ubl.get_z_correction_along_vertical_mesh_line_at_specific_Y(y, current_xi + dxi, current_yi - down_flag);
+        float z0 = ubl.z_correction_for_y_on_vertical_mesh_line(y, current_xi + dxi, current_yi - down_flag);
 
         /**
          * Debug code to use non-optimized get_z_correction() and to do a sanity check
@@ -536,7 +529,7 @@
           }
         //*/
 
-        z0 *= ubl.fade_scaling_factor_for_z(z_end);
+        z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]);
 
         /**
          * If part of the Mesh is undefined, it will show up as NAN
@@ -548,13 +541,13 @@
         if (isnan(z0)) z0 = 0.0;
 
         if (!inf_normalized_flag) {
-          on_axis_distance = use_x_dist ? next_mesh_line_x - x_start : y - y_start;
-          e_position = e_start + on_axis_distance * e_normalized_dist;
-          z_position = z_start + on_axis_distance * z_normalized_dist;
+          on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : y - start[Y_AXIS];
+          e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
+          z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
         }
         else {
-          e_position = e_start;
-          z_position = z_start;
+          e_position = start[E_AXIS];
+          z_position = start[Z_AXIS];
         }
 
         planner.buffer_line(next_mesh_line_x, y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder);
@@ -566,7 +559,7 @@
     if (ubl.g26_debug_flag)
       debug_current_and_destination((char*)"generic move done in ubl_line_to_destination()");
 
-    if (current_position[0] != x_end || current_position[1] != y_end)
+    if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
       goto FINAL_MOVE;
 
     set_current_to_destination();