From 4902fd4e956ac86aad331cff5571952e4dd76745 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Fri, 31 Mar 2017 00:15:32 -0500 Subject: [PATCH] More data in UBL class, make it a static class - Make all `unified_bed_leveling` data/methods static - Move some UBL-related variables into the class - Replace `map_[xy]_index_to_bed_location` with `mesh_index_to_[xy]pos` --- Marlin/G26_Mesh_Validation_Tool.cpp | 20 +- Marlin/Marlin.h | 4 + Marlin/Marlin_main.cpp | 2 +- Marlin/UBL.h | 460 ++++++++++++++-------------- Marlin/UBL_Bed_Leveling.cpp | 34 +- Marlin/UBL_G29.cpp | 20 +- Marlin/UBL_line_to_destination.cpp | 24 +- 7 files changed, 282 insertions(+), 282 deletions(-) diff --git a/Marlin/G26_Mesh_Validation_Tool.cpp b/Marlin/G26_Mesh_Validation_Tool.cpp index f11116b63..1347b2b6d 100644 --- a/Marlin/G26_Mesh_Validation_Tool.cpp +++ b/Marlin/G26_Mesh_Validation_Tool.cpp @@ -265,8 +265,8 @@ location = find_closest_circle_to_print(x_pos, y_pos); // Find the closest Mesh Intersection to where we are now. if (location.x_index >= 0 && location.y_index >= 0) { - circle_x = ubl.map_x_index_to_bed_location(location.x_index); - circle_y = ubl.map_y_index_to_bed_location(location.y_index); + circle_x = ubl.mesh_index_to_xpos[location.x_index]; + circle_y = ubl.mesh_index_to_ypos[location.y_index]; // Let's do a couple of quick sanity checks. We can pull this code out later if we never see it catch a problem #ifdef DELTA @@ -415,8 +415,8 @@ for (uint8_t i = 0; i < UBL_MESH_NUM_X_POINTS; i++) { for (uint8_t j = 0; j < UBL_MESH_NUM_Y_POINTS; j++) { if (!is_bit_set(circle_flags, i, j)) { - mx = ubl.map_x_index_to_bed_location(i); // We found a circle that needs to be printed - my = ubl.map_y_index_to_bed_location(j); + mx = ubl.mesh_index_to_xpos[i]; // We found a circle that needs to be printed + my = ubl.mesh_index_to_ypos[j]; dx = X - mx; // Get the distance to this intersection dy = Y - my; @@ -461,11 +461,11 @@ // We found two circles that need a horizontal line to connect them // Print it! // - sx = ubl.map_x_index_to_bed_location(i); + 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.map_y_index_to_bed_location(j); + sy = ubl.mesh_index_to_ypos[j]; - ex = ubl.map_x_index_to_bed_location(i + 1); + 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; @@ -498,12 +498,12 @@ // We found two circles that need a vertical line to connect them // Print it! // - sx = ubl.map_x_index_to_bed_location(i); - sy = ubl.map_y_index_to_bed_location(j); + 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.map_y_index_to_bed_location(j + 1); + 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 sx = constrain(sx, X_MIN_POS + 1, X_MAX_POS - 1); // This keeps us from bumping the endstops diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h index bf951cec2..7254d345e 100644 --- a/Marlin/Marlin.h +++ b/Marlin/Marlin.h @@ -430,4 +430,8 @@ void do_blocking_move_to_x(const float &x, const float &fr_mm_s=0.0); void do_blocking_move_to_z(const float &z, const float &fr_mm_s=0.0); void do_blocking_move_to_xy(const float &x, const float &y, const float &fr_mm_s=0.0); +#if ENABLED(Z_PROBE_ALLEN_KEY) || ENABLED(Z_PROBE_SLED) || HAS_PROBING_PROCEDURE || HOTENDS > 1 || ENABLED(NOZZLE_CLEAN_FEATURE) || ENABLED(NOZZLE_PARK_FEATURE) + bool axis_unhomed_error(const bool x, const bool y, const bool z); +#endif + #endif //MARLIN_H diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 6dff52e85..f0ec84f20 100755 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -3221,7 +3221,7 @@ inline void gcode_G4() { */ inline void gcode_G12() { // Don't allow nozzle cleaning without homing first - if (axis_unhomed_error(true, true, true)) { return; } + if (axis_unhomed_error(true, true, true)) return; const uint8_t pattern = code_seen('P') ? code_value_ushort() : 0, strokes = code_seen('S') ? code_value_ushort() : NOZZLE_CLEAN_STROKES, diff --git a/Marlin/UBL.h b/Marlin/UBL.h index 933d36774..1d3a91b30 100644 --- a/Marlin/UBL.h +++ b/Marlin/UBL.h @@ -39,7 +39,6 @@ enum MeshPointType { INVALID, REAL, SET_IN_BITMAP }; - bool axis_unhomed_error(bool, bool, bool); void dump(char * const str, const float &f); bool ubl_lcd_clicked(); void probe_entire_mesh(const float&, const float&, const bool, const bool, const bool); @@ -78,275 +77,273 @@ enum MBLStatus { MBL_STATUS_NONE = 0, MBL_STATUS_HAS_MESH_BIT = 0, MBL_STATUS_ACTIVE_BIT = 1 }; - #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)) + #define MESH_X_DIST (float(UBL_MESH_MAX_X - (UBL_MESH_MIN_X)) / float(UBL_MESH_NUM_X_POINTS - 1)) + #define MESH_Y_DIST (float(UBL_MESH_MAX_Y - (UBL_MESH_MIN_Y)) / float(UBL_MESH_NUM_Y_POINTS - 1)) - extern float mesh_index_to_x_location[UBL_MESH_NUM_X_POINTS + 1]; // +1 just because of paranoia that we might end up on the - extern float mesh_index_to_y_location[UBL_MESH_NUM_Y_POINTS + 1]; // the last Mesh Line and that is the start of a whole new cell + typedef struct { + bool active = false; + float z_offset = 0.0; + int8_t eeprom_storage_slot = -1, + n_x = UBL_MESH_NUM_X_POINTS, + n_y = UBL_MESH_NUM_Y_POINTS; + + float mesh_x_min = UBL_MESH_MIN_X, + mesh_y_min = UBL_MESH_MIN_Y, + 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; + + #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) + float g29_correction_fade_height = 10.0, + g29_fade_height_multiplier = 1.0 / 10.0; // It's cheaper to do a floating point multiply than divide, + // so keep this value and its reciprocal. + #else + const float g29_correction_fade_height = 10.0, + g29_fade_height_multiplier = 1.0 / 10.0; + #endif + + // If you change this struct, adjust TOTAL_STRUCT_SIZE + + #define TOTAL_STRUCT_SIZE 40 // Total size of the above fields + + // padding provides space to add state variables without + // changing the location of data structures in the EEPROM. + // This is for compatibility with future versions to keep + // users from having to regenerate their mesh data. + unsigned char padding[64 - TOTAL_STRUCT_SIZE]; + + } ubl_state; class unified_bed_leveling { private: - float last_specified_z, - fade_scaling_factor_for_current_height; + static float last_specified_z, + fade_scaling_factor_for_current_height; public: - float z_values[UBL_MESH_NUM_X_POINTS][UBL_MESH_NUM_Y_POINTS]; + static ubl_state state, pre_initialized; - bool g26_debug_flag = false, - has_control_of_lcd_panel = false; + static float z_values[UBL_MESH_NUM_X_POINTS][UBL_MESH_NUM_Y_POINTS], + mesh_index_to_xpos[UBL_MESH_NUM_X_POINTS + 1], // +1 safety margin for now, until determinism prevails + mesh_index_to_ypos[UBL_MESH_NUM_Y_POINTS + 1]; - int8_t eeprom_start = -1; + static bool g26_debug_flag, + has_control_of_lcd_panel; - volatile int encoder_diff; // Volatile because it's changed at interrupt time. + static int8_t eeprom_start; - struct ubl_state { - bool active = false; - float z_offset = 0.0; - int8_t eeprom_storage_slot = -1, - n_x = UBL_MESH_NUM_X_POINTS, - n_y = UBL_MESH_NUM_Y_POINTS; + static volatile int encoder_diff; // Volatile because it's changed at interrupt time. - float mesh_x_min = UBL_MESH_MIN_X, - mesh_y_min = UBL_MESH_MIN_Y, - 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; + unified_bed_leveling(); - #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) - float g29_correction_fade_height = 10.0, - g29_fade_height_multiplier = 1.0 / 10.0; // It's cheaper to do a floating point multiply than divide, - // so keep this value and its reciprocal. - #else - const float g29_correction_fade_height = 10.0, - g29_fade_height_multiplier = 1.0 / 10.0; - #endif + static void display_map(const int); - // If you change this struct, adjust TOTAL_STRUCT_SIZE - - #define TOTAL_STRUCT_SIZE 43 // Total size of the above fields - - // padding provides space to add state variables without - // changing the location of data structures in the EEPROM. - // This is for compatibility with future versions to keep - // users from having to regenerate their mesh data. - unsigned char padding[64 - TOTAL_STRUCT_SIZE]; - - } state, pre_initialized; - - unified_bed_leveling(); - - void display_map(const int); - - void reset(); - void invalidate(); - - void store_state(); - void load_state(); - void store_mesh(const int16_t); - void load_mesh(const int16_t); - - bool sanity_check(); - - 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); }; - - FORCE_INLINE void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; } - - 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. - 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. - - 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; - } - - 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; - } - - /** - * z2 --| - * z0 | | - * | | + (z2-z1) - * z1 | | | - * ---+-------------+--------+-- --| - * a1 a0 a2 - * |<---delta_a---------->| - * - * calc_z0 is the basis for all the Mesh Based correction. It is used to - * find the expected Z Height at a position between two known Z-Height locations. - * - * It is fairly expensive with its 4 floating point additions and 2 floating point - * multiplications. - */ - 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; - } - - /** - * 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. - */ - 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); - SERIAL_ECHOPAIR(",yi=", yi); - SERIAL_CHAR(')'); - SERIAL_EOL; - return NAN; + static void reset(); + static void invalidate(); + + static void store_state(); + static void load_state(); + static void store_mesh(const int16_t); + static void load_mesh(const int16_t); + + static bool sanity_check(); + + static FORCE_INLINE void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; } + + 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. + 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. + + 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; } - 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 + 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(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); - SERIAL_ECHOPAIR(", yi=", y1_i); - SERIAL_CHAR(')'); - SERIAL_EOL; - return NAN; + 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; } - 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 + yratio * dz; - } - - /** - * This is the generic Z-Correction. It works anywhere within a Mesh Cell. It first - * does a linear interpolation along both of the bounding X-Mesh-Lines to find the - * Z-Height at both ends. Then it does a linear interpolation of these heights based - * on the Y position within the cell. - */ - 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) { - - SERIAL_ECHOPAIR("? in get_z_correction(x0=", x0); - SERIAL_ECHOPAIR(", y0=", y0); - SERIAL_CHAR(')'); - SERIAL_EOL; - - #if ENABLED(ULTRA_LCD) - 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 + /** + * z2 --| + * z0 | | + * | | + (z2-z1) + * z1 | | | + * ---+-------------+--------+-- --| + * a1 a0 a2 + * |<---delta_a---------->| + * + * calc_z0 is the basis for all the Mesh Based correction. It is used to + * find the expected Z Height at a position between two known Z-Height locations. + * + * It is fairly expensive with its 4 floating point additions and 2 floating point + * multiplications. + */ + 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; } - 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)) { - SERIAL_ECHOPAIR(" raw get_z_correction(", x0); - SERIAL_CHAR(',') - SERIAL_ECHO(y0); - SERIAL_ECHOPGM(") = "); - SERIAL_ECHO_F(z0, 6); + /** + * 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. + */ + 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); + SERIAL_ECHOPAIR(",x1_i=", x1_i); + SERIAL_ECHOPAIR(",yi=", yi); + SERIAL_CHAR(')'); + SERIAL_EOL; + return NAN; } - #endif - #if ENABLED(DEBUG_LEVELING_FEATURE) - if (DEBUGGING(MESH_ADJUST)) { - SERIAL_ECHOPGM(" >>>---> "); - SERIAL_ECHO_F(z0, 6); + 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); + + return z1 + xratio * dz; + } + + // + // See comments above for get_z_correction_along_horizontal_mesh_line_at_specific_X + // + 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); + SERIAL_ECHOPAIR(", x1_i=", xi); + SERIAL_ECHOPAIR(", yi=", y1_i); + SERIAL_CHAR(')'); SERIAL_EOL; + return NAN; } - #endif - if (isnan(z0)) { // if part of the Mesh is undefined, it will show up as NAN - z0 = 0.0; // in ubl.z_values[][] and propagate through the - // calculations. If our correction is NAN, we throw it out - // because part of the Mesh is undefined and we don't have the - // information we need to complete the height correction. + 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); + + return z1 + yratio * dz; + } + + /** + * This is the generic Z-Correction. It works anywhere within a Mesh Cell. It first + * does a linear interpolation along both of the bounding X-Mesh-Lines to find the + * Z-Height at both ends. Then it does a linear interpolation of these heights based + * on the Y position within the cell. + */ + static float get_z_correction(const float &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)); + + if (cx < 0 || cy < 0 || cx >= UBL_MESH_NUM_X_POINTS || cy >= UBL_MESH_NUM_Y_POINTS) { + + SERIAL_ECHOPAIR("? in get_z_correction(x0=", x0); + SERIAL_ECHOPAIR(", y0=", y0); + SERIAL_CHAR(')'); + SERIAL_EOL; + + #if ENABLED(ULTRA_LCD) + 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 + } + + const float z1 = calc_z0(RAW_X_POSITION(x0), + 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), + 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), + mesh_index_to_ypos[cy], z1, + mesh_index_to_ypos[cy + 1], z2); #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(MESH_ADJUST)) { - SERIAL_ECHOPAIR("??? Yikes! NAN in get_z_correction(", x0); - SERIAL_CHAR(','); + SERIAL_ECHOPAIR(" raw get_z_correction(", x0); + SERIAL_CHAR(',') SERIAL_ECHO(y0); - SERIAL_CHAR(')'); + SERIAL_ECHOPGM(") = "); + SERIAL_ECHO_F(z0, 6); + } + #endif + + #if ENABLED(DEBUG_LEVELING_FEATURE) + if (DEBUGGING(MESH_ADJUST)) { + SERIAL_ECHOPGM(" >>>---> "); + SERIAL_ECHO_F(z0, 6); SERIAL_EOL; } #endif - } - return z0; // there used to be a +state.z_offset on this line - } - - /** - * This routine is used to scale the Z correction depending upon the current nozzle height. It is - * optimized for speed. It avoids floating point operations by checking if the requested scaling - * factor is going to be the same as the last time the function calculated a value. If so, it just - * returns it. - * - * 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' - */ - #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) - 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; - fade_scaling_factor_for_current_height = - state.active && rz < state.g29_correction_fade_height - ? 1.0 - (rz * state.g29_fade_height_multiplier) - : 0.0; + if (isnan(z0)) { // if part of the Mesh is undefined, it will show up as NAN + z0 = 0.0; // in ubl.z_values[][] and propagate through the + // calculations. If our correction is NAN, we throw it out + // because part of the Mesh is undefined and we don't have the + // information we need to complete the height correction. + + #if ENABLED(DEBUG_LEVELING_FEATURE) + if (DEBUGGING(MESH_ADJUST)) { + SERIAL_ECHOPAIR("??? Yikes! NAN in get_z_correction(", x0); + SERIAL_CHAR(','); + SERIAL_ECHO(y0); + SERIAL_CHAR(')'); + SERIAL_EOL; + } + #endif } - return fade_scaling_factor_for_current_height; + return z0; // there used to be a +state.z_offset on this line } - #else + /** + * This routine is used to scale the Z correction depending upon the current nozzle height. It is + * optimized for speed. It avoids floating point operations by checking if the requested scaling + * factor is going to be the same as the last time the function calculated a value. If so, it just + * returns it. + * + * 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' + */ + #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) + + 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; + 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; + } - static constexpr float fade_scaling_factor_for_z(const float &lz) { UNUSED(lz); return 1.0; } + #else - #endif + static constexpr float fade_scaling_factor_for_z(const float &lz) { UNUSED(lz); return 1.0; } + + #endif }; // class unified_bed_leveling @@ -355,5 +352,4 @@ #define UBL_LAST_EEPROM_INDEX (E2END - sizeof(unified_bed_leveling::state)) #endif // AUTO_BED_LEVELING_UBL - #endif // UNIFIED_BED_LEVELING_H diff --git a/Marlin/UBL_Bed_Leveling.cpp b/Marlin/UBL_Bed_Leveling.cpp index 98c5a445a..a81a16bc0 100644 --- a/Marlin/UBL_Bed_Leveling.cpp +++ b/Marlin/UBL_Bed_Leveling.cpp @@ -57,23 +57,26 @@ } } - /** - * 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 - * 'member data'. So, in the interest of speed, we do it this way. On a 32-bit CPU they can be - * moved back inside the bed leveling class. - */ - float mesh_index_to_x_location[UBL_MESH_NUM_X_POINTS + 1], // +1 just because of paranoia that we might end up on the - mesh_index_to_y_location[UBL_MESH_NUM_Y_POINTS + 1]; // the last Mesh Line and that is the start of a whole new cell + ubl_state unified_bed_leveling::state, unified_bed_leveling::pre_initialized; - unified_bed_leveling::unified_bed_leveling() { - for (uint8_t i = 0; i <= UBL_MESH_NUM_X_POINTS; i++) // We go one past what we expect to ever need for safety - mesh_index_to_x_location[i] = double(UBL_MESH_MIN_X) + double(MESH_X_DIST) * double(i); + float unified_bed_leveling::z_values[UBL_MESH_NUM_X_POINTS][UBL_MESH_NUM_Y_POINTS], + unified_bed_leveling::last_specified_z, + unified_bed_leveling::fade_scaling_factor_for_current_height, + unified_bed_leveling::mesh_index_to_xpos[UBL_MESH_NUM_X_POINTS + 1], // +1 safety margin for now, until determinism prevails + unified_bed_leveling::mesh_index_to_ypos[UBL_MESH_NUM_Y_POINTS + 1]; + + bool unified_bed_leveling::g26_debug_flag = false, + unified_bed_leveling::has_control_of_lcd_panel = false; - for (uint8_t i = 0; i <= UBL_MESH_NUM_Y_POINTS; i++) // We go one past what we expect to ever need for safety - mesh_index_to_y_location[i] = double(UBL_MESH_MIN_Y) + double(MESH_Y_DIST) * double(i); + int8_t unified_bed_leveling::eeprom_start = -1; + volatile int unified_bed_leveling::encoder_diff; + + unified_bed_leveling::unified_bed_leveling() { + for (uint8_t i = 0; i < COUNT(mesh_index_to_xpos); i++) + mesh_index_to_xpos[i] = UBL_MESH_MIN_X + i * (MESH_X_DIST); + for (uint8_t i = 0; i < COUNT(mesh_index_to_ypos); i++) + mesh_index_to_ypos[i] = UBL_MESH_MIN_Y + i * (MESH_Y_DIST); reset(); } @@ -161,9 +164,6 @@ } void unified_bed_leveling::invalidate() { - print_hex_word((uint16_t)this); - SERIAL_EOL; - state.active = false; state.z_offset = 0; for (int x = 0; x < UBL_MESH_NUM_X_POINTS; x++) diff --git a/Marlin/UBL_G29.cpp b/Marlin/UBL_G29.cpp index 8f2d1b095..e1835e74c 100644 --- a/Marlin/UBL_G29.cpp +++ b/Marlin/UBL_G29.cpp @@ -750,8 +750,8 @@ location = find_closest_mesh_point_of_type(INVALID, lx, ly, 1, NULL, do_furthest ); // the '1' says we want the location to be relative to the probe if (location.x_index >= 0 && location.y_index >= 0) { - const float rawx = ubl.map_x_index_to_bed_location(location.x_index), - rawy = ubl.map_y_index_to_bed_location(location.y_index); + const float rawx = ubl.mesh_index_to_xpos[location.x_index], + rawy = ubl.mesh_index_to_ypos[location.y_index]; // TODO: Change to use `position_is_reachable` (for SCARA-compatibility) if (rawx < (MIN_PROBE_X) || rawx > (MAX_PROBE_X) || rawy < (MIN_PROBE_Y) || rawy > (MAX_PROBE_Y)) { @@ -900,8 +900,8 @@ // It doesn't matter if the probe can't reach the NAN location. This is a manual probe. if (location.x_index < 0 && location.y_index < 0) continue; - const float rawx = ubl.map_x_index_to_bed_location(location.x_index), - rawy = ubl.map_y_index_to_bed_location(location.y_index); + const float rawx = ubl.mesh_index_to_xpos[location.x_index], + rawy = ubl.mesh_index_to_ypos[location.y_index]; // TODO: Change to use `position_is_reachable` (for SCARA-compatibility) if (rawx < (X_MIN_POS) || rawx > (X_MAX_POS) || rawy < (Y_MIN_POS) || rawy > (Y_MAX_POS)) { @@ -1137,7 +1137,7 @@ SERIAL_PROTOCOLPGM("X-Axis Mesh Points at: "); for (uint8_t i = 0; i < UBL_MESH_NUM_X_POINTS; i++) { - SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(ubl.map_x_index_to_bed_location(i)), 1); + SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(ubl.mesh_index_to_xpos[i]), 1); SERIAL_PROTOCOLPGM(" "); safe_delay(50); } @@ -1145,7 +1145,7 @@ SERIAL_PROTOCOLPGM("Y-Axis Mesh Points at: "); for (uint8_t i = 0; i < UBL_MESH_NUM_Y_POINTS; i++) { - SERIAL_PROTOCOL_F(LOGICAL_Y_POSITION(ubl.map_y_index_to_bed_location(i)), 1); + SERIAL_PROTOCOL_F(LOGICAL_Y_POSITION(ubl.mesh_index_to_ypos[i]), 1); SERIAL_PROTOCOLPGM(" "); safe_delay(50); } @@ -1283,8 +1283,8 @@ // We only get here if we found a Mesh Point of the specified type - const float rawx = ubl.map_x_index_to_bed_location(i), // Check if we can probe this mesh location - rawy = ubl.map_y_index_to_bed_location(j); + const float rawx = ubl.mesh_index_to_xpos[i], // Check if we can probe this mesh location + rawy = ubl.mesh_index_to_ypos[j]; // If using the probe as the reference there are some unreachable locations. // Prune them from the list and ignore them till the next Phase (manual nozzle probing). @@ -1350,8 +1350,8 @@ 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 - const float rawx = ubl.map_x_index_to_bed_location(location.x_index), - rawy = ubl.map_y_index_to_bed_location(location.y_index); + const float rawx = ubl.mesh_index_to_xpos[location.x_index], + rawy = ubl.mesh_index_to_ypos[location.y_index]; // TODO: Change to use `position_is_reachable` (for SCARA-compatibility) if (rawx < (X_MIN_POS) || rawx > (X_MAX_POS) || rawy < (Y_MIN_POS) || rawy > (Y_MAX_POS)) { // In theory, we don't need this check. diff --git a/Marlin/UBL_line_to_destination.cpp b/Marlin/UBL_line_to_destination.cpp index 7c8b6251d..7c5e9f4b8 100644 --- a/Marlin/UBL_line_to_destination.cpp +++ b/Marlin/UBL_line_to_destination.cpp @@ -167,16 +167,16 @@ * 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) - 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]); + const float xratio = (RAW_X_POSITION(x_end) - 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 * + (ubl.z_values[cell_dest_xi + 1][cell_dest_yi + 1] - ubl.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. - const float yratio = (RAW_Y_POSITION(y_end) - mesh_index_to_y_location[cell_dest_yi]) * (1.0 / (MESH_Y_DIST)); + const float yratio = (RAW_Y_POSITION(y_end) - ubl.mesh_index_to_ypos[cell_dest_yi]) * (1.0 / (MESH_Y_DIST)); float z0 = z1 + (z2 - z1) * yratio; @@ -274,7 +274,7 @@ 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; - const float next_mesh_line_y = LOGICAL_Y_POSITION(mesh_index_to_y_location[current_yi]); + const float next_mesh_line_y = LOGICAL_Y_POSITION(ubl.mesh_index_to_ypos[current_yi]); /** * inf_m_flag? the slope of the line is infinite, we won't do the calculations @@ -316,7 +316,7 @@ */ if (isnan(z0)) z0 = 0.0; - const float y = LOGICAL_Y_POSITION(mesh_index_to_y_location[current_yi]); + const float y = LOGICAL_Y_POSITION(ubl.mesh_index_to_ypos[current_yi]); /** * Without this check, it is possible for the algorithm to generate a zero length move in the case @@ -365,7 +365,7 @@ // edge of this cell for the first move. while (current_xi != cell_dest_xi + left_flag) { current_xi += dxi; - const float next_mesh_line_x = LOGICAL_X_POSITION(mesh_index_to_x_location[current_xi]), + 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); @@ -401,7 +401,7 @@ */ if (isnan(z0)) z0 = 0.0; - const float x = LOGICAL_X_POSITION(mesh_index_to_x_location[current_xi]); + const float x = LOGICAL_X_POSITION(ubl.mesh_index_to_xpos[current_xi]); /** * Without this check, it is possible for the algorithm to generate a zero length move in the case @@ -451,8 +451,8 @@ while (xi_cnt > 0 || yi_cnt > 0) { - 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]), + 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