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]);
/**
--
GitLab