From 8edfff39e4549a92b9f2e202d73845064fc79ac2 Mon Sep 17 00:00:00 2001 From: Alex Voinea Date: Tue, 2 May 2023 10:47:06 +0200 Subject: [PATCH] MBL code cleanup --- Firmware/mesh_bed_calibration.cpp | 2 +- Firmware/mesh_bed_leveling.cpp | 110 +++++++++++++++++++++++------- Firmware/mesh_bed_leveling.h | 103 +++------------------------- 3 files changed, 96 insertions(+), 119 deletions(-) diff --git a/Firmware/mesh_bed_calibration.cpp b/Firmware/mesh_bed_calibration.cpp index d7fd2b4db..a04b3a44a 100644 --- a/Firmware/mesh_bed_calibration.cpp +++ b/Firmware/mesh_bed_calibration.cpp @@ -776,7 +776,7 @@ void world2machine_revert_to_uncorrected() static inline bool vec_undef(const float v[2]) { const uint32_t *vx = (const uint32_t*)v; - return vx[0] == 0x0FFFFFFFF || vx[1] == 0x0FFFFFFFF; + return vx[0] == 0xFFFFFFFF || vx[1] == 0xFFFFFFFF; } diff --git a/Firmware/mesh_bed_leveling.cpp b/Firmware/mesh_bed_leveling.cpp index 70c9816e5..60cd341c5 100644 --- a/Firmware/mesh_bed_leveling.cpp +++ b/Firmware/mesh_bed_leveling.cpp @@ -6,8 +6,6 @@ mesh_bed_leveling mbl; -mesh_bed_leveling::mesh_bed_leveling() { reset(); } - void mesh_bed_leveling::reset() { active = 0; for (uint8_t row = 0; row < MESH_NUM_Y_POINTS; ++row) { @@ -17,10 +15,91 @@ void mesh_bed_leveling::reset() { } } -static inline bool vec_undef(const float v[2]) -{ - const uint32_t *vx = (const uint32_t*)v; - return vx[0] == 0xFFFFFFFF || vx[1] == 0xFFFFFFFF; +float mesh_bed_leveling::get_z(float x, float y) { + int i, j; + float s, t; + +#if MESH_NUM_X_POINTS==3 && MESH_NUM_Y_POINTS==3 +#define MESH_MID_X (0.5f*(MESH_MIN_X+MESH_MAX_X)) +#define MESH_MID_Y (0.5f*(MESH_MIN_Y+MESH_MAX_Y)) + if (x < MESH_MID_X) { + i = 0; + s = (x - MESH_MIN_X) / MESH_X_DIST; + if (s > 1.f) + s = 1.f; + } else { + i = 1; + s = (x - MESH_MID_X) / MESH_X_DIST; + if (s < 0) + s = 0; + } + if (y < MESH_MID_Y) { + j = 0; + t = (y - MESH_MIN_Y) / MESH_Y_DIST; + if (t > 1.f) + t = 1.f; + } else { + j = 1; + t = (y - MESH_MID_Y) / MESH_Y_DIST; + if (t < 0) + t = 0; + } +#else + i = int(floor((x - MESH_MIN_X) / MESH_X_DIST)); + if (i < 0) { + i = 0; + s = (x - MESH_MIN_X) / MESH_X_DIST; + if (s > 1.f) + s = 1.f; + } + else if (i > MESH_NUM_X_POINTS - 2) { + i = MESH_NUM_X_POINTS - 2; + s = (x - get_x(i)) / MESH_X_DIST; + if (s < 0) + s = 0; + } else { + s = (x - get_x(i)) / MESH_X_DIST; + if (s < 0) + s = 0; + else if (s > 1.f) + s = 1.f; + } + j = int(floor((y - MESH_MIN_Y) / MESH_Y_DIST)); + if (j < 0) { + j = 0; + t = (y - MESH_MIN_Y) / MESH_Y_DIST; + if (t > 1.f) + t = 1.f; + } else if (j > MESH_NUM_Y_POINTS - 2) { + j = MESH_NUM_Y_POINTS - 2; + t = (y - get_y(j)) / MESH_Y_DIST; + if (t < 0) + t = 0; + } else { + t = (y - get_y(j)) / MESH_Y_DIST; + if (t < 0) + t = 0; + else if (t > 1.f) + t = 1.f; + } +#endif /* MESH_NUM_X_POINTS==3 && MESH_NUM_Y_POINTS==3 */ + + float si = 1.f-s; + float z0 = si * z_values[j ][i] + s * z_values[j ][i+1]; + float z1 = si * z_values[j+1][i] + s * z_values[j+1][i+1]; + return (1.f-t) * z0 + t * z1; +} + +int mesh_bed_leveling::select_x_index(float x) { + int i = 1; + while (x > get_x(i) && i < MESH_NUM_X_POINTS - 1) i++; + return i - 1; +} + +int mesh_bed_leveling::select_y_index(float y) { + int i = 1; + while (y > get_y(i) && i < MESH_NUM_Y_POINTS - 1) i++; + return i - 1; } #if MESH_NUM_X_POINTS>=5 && MESH_NUM_Y_POINTS>=5 && (MESH_NUM_X_POINTS&1)==1 && (MESH_NUM_Y_POINTS&1)==1 @@ -80,24 +159,7 @@ void mesh_bed_leveling::upsample_3x3() } } } - -/* - // Relax the non-measured points. - const float weight = 0.2f; - for (uint8_t iter = 0; iter < 20; ++ iter) { - for (int8_t j = 1; j < 6; ++ j) { - for (int8_t i = 1; i < 6; ++ i) { - if (i == 3 || j == 3) - continue; - if ((i % 3) == 0 && (j % 3) == 0) - continue; - float avg = 0.25f * (z_values[j][i-1]+z_values[j][i+1]+z_values[j-1][i]+z_values[j+1][i]); - z_values[j][i] = (1.f-weight)*z_values[j][i] + weight*avg; - } - } - } -*/ } -#endif +#endif // (MESH_NUM_X_POINTS>=5 && MESH_NUM_Y_POINTS>=5 && (MESH_NUM_X_POINTS&1)==1 && (MESH_NUM_Y_POINTS&1)==1) #endif // MESH_BED_LEVELING diff --git a/Firmware/mesh_bed_leveling.h b/Firmware/mesh_bed_leveling.h index 7df04844a..b7d33379a 100644 --- a/Firmware/mesh_bed_leveling.h +++ b/Firmware/mesh_bed_leveling.h @@ -13,106 +13,21 @@ public: uint8_t active; float z_values[MESH_NUM_Y_POINTS][MESH_NUM_X_POINTS]; - mesh_bed_leveling(); + mesh_bed_leveling() { reset(); } void reset(); + + static float get_x(int i) { return float(MESH_MIN_X) + float(MESH_X_DIST) * float(i); } + static float get_y(int i) { return float(MESH_MIN_Y) + float(MESH_Y_DIST) * float(i); } + float get_z(float x, float y); + void set_z(uint8_t ix, uint8_t iy, float z) { z_values[iy][ix] = z; } + + int select_x_index(float x); + int select_y_index(float y); #if MESH_NUM_X_POINTS>=5 && MESH_NUM_Y_POINTS>=5 && (MESH_NUM_X_POINTS&1)==1 && (MESH_NUM_Y_POINTS&1)==1 void upsample_3x3(); #endif - - static float get_x(int i) { return float(MESH_MIN_X) + float(MESH_X_DIST) * float(i); } - static float get_y(int i) { return float(MESH_MIN_Y) + float(MESH_Y_DIST) * float(i); } - - void set_z(uint8_t ix, uint8_t iy, float z) { z_values[iy][ix] = z; } - - int select_x_index(float x) { - int i = 1; - while (x > get_x(i) && i < MESH_NUM_X_POINTS - 1) i++; - return i - 1; - } - - int select_y_index(float y) { - int i = 1; - while (y > get_y(i) && i < MESH_NUM_Y_POINTS - 1) i++; - return i - 1; - } - - float get_z(float x, float y) { - int i, j; - float s, t; - -#if MESH_NUM_X_POINTS==3 && MESH_NUM_Y_POINTS==3 -#define MESH_MID_X (0.5f*(MESH_MIN_X+MESH_MAX_X)) -#define MESH_MID_Y (0.5f*(MESH_MIN_Y+MESH_MAX_Y)) - if (x < MESH_MID_X) { - i = 0; - s = (x - MESH_MIN_X) / MESH_X_DIST; - if (s > 1.f) - s = 1.f; - } else { - i = 1; - s = (x - MESH_MID_X) / MESH_X_DIST; - if (s < 0) - s = 0; - } - if (y < MESH_MID_Y) { - j = 0; - t = (y - MESH_MIN_Y) / MESH_Y_DIST; - if (t > 1.f) - t = 1.f; - } else { - j = 1; - t = (y - MESH_MID_Y) / MESH_Y_DIST; - if (t < 0) - t = 0; - } -#else - i = int(floor((x - MESH_MIN_X) / MESH_X_DIST)); - if (i < 0) { - i = 0; - s = (x - MESH_MIN_X) / MESH_X_DIST; - if (s > 1.f) - s = 1.f; - } - else if (i > MESH_NUM_X_POINTS - 2) { - i = MESH_NUM_X_POINTS - 2; - s = (x - get_x(i)) / MESH_X_DIST; - if (s < 0) - s = 0; - } else { - s = (x - get_x(i)) / MESH_X_DIST; - if (s < 0) - s = 0; - else if (s > 1.f) - s = 1.f; - } - j = int(floor((y - MESH_MIN_Y) / MESH_Y_DIST)); - if (j < 0) { - j = 0; - t = (y - MESH_MIN_Y) / MESH_Y_DIST; - if (t > 1.f) - t = 1.f; - } else if (j > MESH_NUM_Y_POINTS - 2) { - j = MESH_NUM_Y_POINTS - 2; - t = (y - get_y(j)) / MESH_Y_DIST; - if (t < 0) - t = 0; - } else { - t = (y - get_y(j)) / MESH_Y_DIST; - if (t < 0) - t = 0; - else if (t > 1.f) - t = 1.f; - } -#endif /* MESH_NUM_X_POINTS==3 && MESH_NUM_Y_POINTS==3 */ - - float si = 1.f-s; - float z0 = si * z_values[j ][i] + s * z_values[j ][i+1]; - float z1 = si * z_values[j+1][i] + s * z_values[j+1][i+1]; - return (1.f-t) * z0 + t * z1; - } - }; extern mesh_bed_leveling mbl;