From 2cbdc0ebb6bbf64daa3f8b0ea9ab615778633e49 Mon Sep 17 00:00:00 2001
From: Roxy-3D <Roxy-3D@users.noreply.github.com>
Date: Wed, 2 Aug 2017 16:51:04 -0500
Subject: [PATCH] Fix large Z corrections when nozzle moves off of UBL mesh
 (#7415)

---
 Marlin/ubl.h          | 27 +++++++++++++++++++--------
 Marlin/ubl_motion.cpp | 13 +++++++++++--
 2 files changed, 30 insertions(+), 10 deletions(-)

diff --git a/Marlin/ubl.h b/Marlin/ubl.h
index 2c2ca1ec39..90de810a67 100644
--- a/Marlin/ubl.h
+++ b/Marlin/ubl.h
@@ -152,7 +152,7 @@
       static void save_ubl_active_state_and_disable();
       static void restore_ubl_active_state_and_leave();
       static void display_map(const int);
-      static mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const float&, const float&, const bool, unsigned int[16], bool);
+      static mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const float&, const float&, const bool, uint16_t[16], bool);
       static void reset();
       static void invalidate();
       static void set_all_mesh_points_to_value(float);
@@ -247,10 +247,10 @@
 
       /**
        * 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).
+       * the case where the printer is making a vertical line that only crosses horizontal mesh lines.
        */
       inline static float z_correction_for_x_on_horizontal_mesh_line(const float &lx0, const int x1_i, const int yi) {
-        if (!WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(yi, 0, GRID_MAX_POINTS_Y - 1)) {
+        if (!WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 2) || !WITHIN(yi, 0, GRID_MAX_POINTS_Y - 1)) {
           serialprintPGM( !WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 1) ? PSTR("x1l_i") : PSTR("yi") );
           SERIAL_ECHOPAIR(" out of bounds in z_correction_for_x_on_horizontal_mesh_line(lx0=", lx0);
           SERIAL_ECHOPAIR(",x1_i=", x1_i);
@@ -270,7 +270,7 @@
       // See comments above for z_correction_for_x_on_horizontal_mesh_line
       //
       inline static float z_correction_for_y_on_vertical_mesh_line(const float &ly0, const int xi, const int y1_i) {
-        if (!WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(y1_i, 0, GRID_MAX_POINTS_Y - 1)) {
+        if (!WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(y1_i, 0, GRID_MAX_POINTS_Y - 2)) {
           serialprintPGM( !WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) ? PSTR("xi") : PSTR("yl_i") );
           SERIAL_ECHOPAIR(" out of bounds in z_correction_for_y_on_vertical_mesh_line(ly0=", ly0);
           SERIAL_ECHOPAIR(", xi=", xi);
@@ -296,7 +296,7 @@
         const int8_t cx = get_cell_index_x(RAW_X_POSITION(lx0)),
                      cy = get_cell_index_y(RAW_Y_POSITION(ly0));
 
-        if (!WITHIN(cx, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(cy, 0, GRID_MAX_POINTS_Y - 1)) {
+        if (!WITHIN(cx, 0, GRID_MAX_POINTS_X - 2) || !WITHIN(cy, 0, GRID_MAX_POINTS_Y - 2)) {
 
           SERIAL_ECHOPAIR("? in get_z_correction(lx0=", lx0);
           SERIAL_ECHOPAIR(", ly0=", ly0);
@@ -307,7 +307,7 @@
             strcpy(lcd_status_message, "get_z_correction() indexes out of range.");
             lcd_quick_feedback();
           #endif
-          return 0.0; // this used to return state.z_offset
+          return NAN; // this used to return state.z_offset
         }
 
         const float z1 = calc_z0(RAW_X_POSITION(lx0),
@@ -384,8 +384,19 @@
         FORCE_INLINE static float fade_scaling_factor_for_z(const float &lz) { return 1.0; }
       #endif
 
-      FORCE_INLINE static float mesh_index_to_xpos(const uint8_t i) { return pgm_read_float(&_mesh_index_to_xpos[i]); }
-      FORCE_INLINE static float mesh_index_to_ypos(const uint8_t i) { return pgm_read_float(&_mesh_index_to_ypos[i]); }
+      FORCE_INLINE static float mesh_index_to_xpos(const uint8_t i) { 
+        if (i<GRID_MAX_POINTS_X) 
+          return pgm_read_float(&_mesh_index_to_xpos[i]); 
+        else  
+          return UBL_MESH_MIN_X + i * (MESH_X_DIST);
+      }
+
+      FORCE_INLINE static float mesh_index_to_ypos(const uint8_t i) { 
+        if (i<GRID_MAX_POINTS_Y) 
+          return pgm_read_float(&_mesh_index_to_ypos[i]); 
+        else  
+          return UBL_MESH_MIN_Y + i * (MESH_Y_DIST);
+      }
 
       static bool prepare_segmented_line_to(const float ltarget[XYZE], const float &feedrate);
       static void line_to_destination_cartesian(const float &fr, uint8_t e);
diff --git a/Marlin/ubl_motion.cpp b/Marlin/ubl_motion.cpp
index 45fe4caf91..827e4e2e4a 100644
--- a/Marlin/ubl_motion.cpp
+++ b/Marlin/ubl_motion.cpp
@@ -173,12 +173,18 @@
        * 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(end[X_AXIS]) - mesh_index_to_xpos(cell_dest_xi)) * (1.0 / (MESH_X_DIST)),
-                  z1 = z_values[cell_dest_xi    ][cell_dest_yi    ] + xratio *
+      const float xratio = (RAW_X_POSITION(end[X_AXIS]) - mesh_index_to_xpos(cell_dest_xi)) * (1.0 / (MESH_X_DIST));
+
+      float 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]);
 
+      if ( cell_dest_xi >= GRID_MAX_POINTS_X-1) {
+        z1 = 0.0;
+        z2 = 0.0;
+      }
+
       // 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.
 
@@ -186,6 +192,9 @@
 
       float z0 = z1 + (z2 - z1) * yratio;
 
+      if ( cell_dest_yi >= GRID_MAX_POINTS_Y-1) 
+        z0 = 0.0;
+
       z0 *= fade_scaling_factor_for_z(end[Z_AXIS]);
 
       /**