Skip to content

Commit 637a4ad

Browse files
tombrazierLCh-77
authored andcommitted
♻️ Refactor and fix ABL Bilinear (MarlinFirmware#23868)
1 parent d2707e4 commit 637a4ad

File tree

13 files changed

+201
-163
lines changed

13 files changed

+201
-163
lines changed

Marlin/src/core/utility.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -135,7 +135,7 @@ void safe_delay(millis_t ms) {
135135
const float rz = ubl.get_z_correction(current_position);
136136
#elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
137137
SERIAL_ECHOPGM("ABL Adjustment Z");
138-
const float rz = bilinear_z_offset(current_position);
138+
const float rz = bbl.get_z_correction(current_position);
139139
#endif
140140
SERIAL_ECHO(ftostr43sign(rz, '+'));
141141
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)

Marlin/src/feature/bedlevel/abl/abl.h

-45
This file was deleted.

Marlin/src/feature/bedlevel/abl/abl.cpp Marlin/src/feature/bedlevel/abl/bbl.cpp

+71-53
Original file line numberDiff line numberDiff line change
@@ -35,14 +35,19 @@
3535
#include "../../../lcd/extui/ui_api.h"
3636
#endif
3737

38-
xy_pos_t bilinear_grid_spacing, bilinear_start;
39-
xy_float_t bilinear_grid_factor;
40-
bed_mesh_t z_values;
38+
LevelingBilinear bbl;
39+
40+
xy_pos_t LevelingBilinear::grid_spacing,
41+
LevelingBilinear::grid_start;
42+
xy_float_t LevelingBilinear::grid_factor;
43+
bed_mesh_t LevelingBilinear::z_values;
44+
xy_pos_t LevelingBilinear::cached_rel;
45+
xy_int8_t LevelingBilinear::cached_g;
4146

4247
/**
4348
* Extrapolate a single point from its neighbors
4449
*/
45-
static void extrapolate_one_point(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir) {
50+
void LevelingBilinear::extrapolate_one_point(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir) {
4651
if (!isnan(z_values[x][y])) return;
4752
if (DEBUGGING(LEVELING)) {
4853
DEBUG_ECHOPGM("Extrapolate [");
@@ -92,11 +97,26 @@ static void extrapolate_one_point(const uint8_t x, const uint8_t y, const int8_t
9297
#endif
9398
#endif
9499

100+
void LevelingBilinear::reset() {
101+
grid_start.reset();
102+
grid_spacing.reset();
103+
GRID_LOOP(x, y) {
104+
z_values[x][y] = NAN;
105+
TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, 0));
106+
}
107+
}
108+
109+
void LevelingBilinear::set_grid(const xy_pos_t& _grid_spacing, const xy_pos_t& _grid_start) {
110+
grid_spacing = _grid_spacing;
111+
grid_start = _grid_start;
112+
grid_factor = grid_spacing.reciprocal();
113+
}
114+
95115
/**
96116
* Fill in the unprobed points (corners of circular print surface)
97117
* using linear extrapolation, away from the center.
98118
*/
99-
void extrapolate_unprobed_bed_level() {
119+
void LevelingBilinear::extrapolate_unprobed_bed_level() {
100120
#ifdef HALF_IN_X
101121
constexpr uint8_t ctrx2 = 0, xend = GRID_MAX_POINTS_X - 1;
102122
#else
@@ -131,35 +151,31 @@ void extrapolate_unprobed_bed_level() {
131151
#endif
132152
extrapolate_one_point(x2, y2, -1, -1); // right-above - -
133153
}
134-
135154
}
136155

137-
void print_bilinear_leveling_grid() {
156+
void LevelingBilinear::print_leveling_grid(const bed_mesh_t* _z_values /*= NULL*/) {
157+
// print internal grid(s) or just the one passed as a parameter
138158
SERIAL_ECHOLNPGM("Bilinear Leveling Grid:");
139-
print_2d_array(GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y, 3,
140-
[](const uint8_t ix, const uint8_t iy) { return z_values[ix][iy]; }
141-
);
159+
print_2d_array(GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y, 3, _z_values ? *_z_values[0] : z_values[0]);
160+
161+
#if ENABLED(ABL_BILINEAR_SUBDIVISION)
162+
if (!_z_values) {
163+
SERIAL_ECHOLNPGM("Subdivided with CATMULL ROM Leveling Grid:");
164+
print_2d_array(ABL_GRID_POINTS_VIRT_X, ABL_GRID_POINTS_VIRT_Y, 5, z_values_virt[0]);
165+
}
166+
#endif
142167
}
143168

144169
#if ENABLED(ABL_BILINEAR_SUBDIVISION)
145170

146-
#define ABL_GRID_POINTS_VIRT_X GRID_MAX_CELLS_X * (BILINEAR_SUBDIVISIONS) + 1
147-
#define ABL_GRID_POINTS_VIRT_Y GRID_MAX_CELLS_Y * (BILINEAR_SUBDIVISIONS) + 1
148171
#define ABL_TEMP_POINTS_X (GRID_MAX_POINTS_X + 2)
149172
#define ABL_TEMP_POINTS_Y (GRID_MAX_POINTS_Y + 2)
150-
float z_values_virt[ABL_GRID_POINTS_VIRT_X][ABL_GRID_POINTS_VIRT_Y];
151-
xy_pos_t bilinear_grid_spacing_virt;
152-
xy_float_t bilinear_grid_factor_virt;
153-
154-
void print_bilinear_leveling_grid_virt() {
155-
SERIAL_ECHOLNPGM("Subdivided with CATMULL ROM Leveling Grid:");
156-
print_2d_array(ABL_GRID_POINTS_VIRT_X, ABL_GRID_POINTS_VIRT_Y, 5,
157-
[](const uint8_t ix, const uint8_t iy) { return z_values_virt[ix][iy]; }
158-
);
159-
}
173+
float LevelingBilinear::z_values_virt[ABL_GRID_POINTS_VIRT_X][ABL_GRID_POINTS_VIRT_Y];
174+
xy_pos_t LevelingBilinear::grid_spacing_virt;
175+
xy_float_t LevelingBilinear::grid_factor_virt;
160176

161177
#define LINEAR_EXTRAPOLATION(E, I) ((E) * 2 - (I))
162-
float bed_level_virt_coord(const uint8_t x, const uint8_t y) {
178+
float LevelingBilinear::bed_level_virt_coord(const uint8_t x, const uint8_t y) {
163179
uint8_t ep = 0, ip = 1;
164180
if (x > (GRID_MAX_POINTS_X) + 1 || y > (GRID_MAX_POINTS_Y) + 1) {
165181
// The requested point requires extrapolating two points beyond the mesh.
@@ -204,7 +220,7 @@ void print_bilinear_leveling_grid() {
204220
return z_values[x - 1][y - 1];
205221
}
206222

207-
static float bed_level_virt_cmr(const float p[4], const uint8_t i, const float t) {
223+
float LevelingBilinear::bed_level_virt_cmr(const float p[4], const uint8_t i, const float t) {
208224
return (
209225
p[i-1] * -t * sq(1 - t)
210226
+ p[i] * (2 - 5 * sq(t) + 3 * t * sq(t))
@@ -213,7 +229,7 @@ void print_bilinear_leveling_grid() {
213229
) * 0.5f;
214230
}
215231

216-
static float bed_level_virt_2cmr(const uint8_t x, const uint8_t y, const_float_t tx, const_float_t ty) {
232+
float LevelingBilinear::bed_level_virt_2cmr(const uint8_t x, const uint8_t y, const_float_t tx, const_float_t ty) {
217233
float row[4], column[4];
218234
LOOP_L_N(i, 4) {
219235
LOOP_L_N(j, 4) {
@@ -224,9 +240,9 @@ void print_bilinear_leveling_grid() {
224240
return bed_level_virt_cmr(row, 1, tx);
225241
}
226242

227-
void bed_level_virt_interpolate() {
228-
bilinear_grid_spacing_virt = bilinear_grid_spacing / (BILINEAR_SUBDIVISIONS);
229-
bilinear_grid_factor_virt = bilinear_grid_spacing_virt.reciprocal();
243+
void LevelingBilinear::bed_level_virt_interpolate() {
244+
grid_spacing_virt = grid_spacing / (BILINEAR_SUBDIVISIONS);
245+
grid_factor_virt = grid_spacing_virt.reciprocal();
230246
LOOP_L_N(y, GRID_MAX_POINTS_Y)
231247
LOOP_L_N(x, GRID_MAX_POINTS_X)
232248
LOOP_L_N(ty, BILINEAR_SUBDIVISIONS)
@@ -244,47 +260,49 @@ void print_bilinear_leveling_grid() {
244260
}
245261
#endif // ABL_BILINEAR_SUBDIVISION
246262

263+
247264
// Refresh after other values have been updated
248-
void refresh_bed_level() {
249-
bilinear_grid_factor = bilinear_grid_spacing.reciprocal();
265+
void LevelingBilinear::refresh_bed_level() {
250266
TERN_(ABL_BILINEAR_SUBDIVISION, bed_level_virt_interpolate());
267+
cached_rel.x = cached_rel.y = -999.999;
268+
cached_g.x = cached_g.y = -99;
251269
}
252270

253271
#if ENABLED(ABL_BILINEAR_SUBDIVISION)
254-
#define ABL_BG_SPACING(A) bilinear_grid_spacing_virt.A
255-
#define ABL_BG_FACTOR(A) bilinear_grid_factor_virt.A
272+
#define ABL_BG_SPACING(A) grid_spacing_virt.A
273+
#define ABL_BG_FACTOR(A) grid_factor_virt.A
256274
#define ABL_BG_POINTS_X ABL_GRID_POINTS_VIRT_X
257275
#define ABL_BG_POINTS_Y ABL_GRID_POINTS_VIRT_Y
258276
#define ABL_BG_GRID(X,Y) z_values_virt[X][Y]
259277
#else
260-
#define ABL_BG_SPACING(A) bilinear_grid_spacing.A
261-
#define ABL_BG_FACTOR(A) bilinear_grid_factor.A
278+
#define ABL_BG_SPACING(A) grid_spacing.A
279+
#define ABL_BG_FACTOR(A) grid_factor.A
262280
#define ABL_BG_POINTS_X GRID_MAX_POINTS_X
263281
#define ABL_BG_POINTS_Y GRID_MAX_POINTS_Y
264282
#define ABL_BG_GRID(X,Y) z_values[X][Y]
265283
#endif
266284

267285
// Get the Z adjustment for non-linear bed leveling
268-
float bilinear_z_offset(const xy_pos_t &raw) {
286+
float LevelingBilinear::get_z_correction(const xy_pos_t &raw) {
269287

270288
static float z1, d2, z3, d4, L, D;
271289

272-
static xy_pos_t prev { -999.999, -999.999 }, ratio;
290+
static xy_pos_t ratio;
273291

274292
// Whole units for the grid line indices. Constrained within bounds.
275-
static xy_int8_t thisg, nextg, lastg { -99, -99 };
293+
static xy_int8_t thisg, nextg;
276294

277295
// XY relative to the probed area
278-
xy_pos_t rel = raw - bilinear_start.asFloat();
296+
xy_pos_t rel = raw - grid_start.asFloat();
279297

280298
#if ENABLED(EXTRAPOLATE_BEYOND_GRID)
281299
#define FAR_EDGE_OR_BOX 2 // Keep using the last grid box
282300
#else
283301
#define FAR_EDGE_OR_BOX 1 // Just use the grid far edge
284302
#endif
285303

286-
if (prev.x != rel.x) {
287-
prev.x = rel.x;
304+
if (cached_rel.x != rel.x) {
305+
cached_rel.x = rel.x;
288306
ratio.x = rel.x * ABL_BG_FACTOR(x);
289307
const float gx = constrain(FLOOR(ratio.x), 0, ABL_BG_POINTS_X - (FAR_EDGE_OR_BOX));
290308
ratio.x -= gx; // Subtract whole to get the ratio within the grid box
@@ -298,10 +316,10 @@ float bilinear_z_offset(const xy_pos_t &raw) {
298316
nextg.x = _MIN(thisg.x + 1, ABL_BG_POINTS_X - 1);
299317
}
300318

301-
if (prev.y != rel.y || lastg.x != thisg.x) {
319+
if (cached_rel.y != rel.y || cached_g.x != thisg.x) {
302320

303-
if (prev.y != rel.y) {
304-
prev.y = rel.y;
321+
if (cached_rel.y != rel.y) {
322+
cached_rel.y = rel.y;
305323
ratio.y = rel.y * ABL_BG_FACTOR(y);
306324
const float gy = constrain(FLOOR(ratio.y), 0, ABL_BG_POINTS_Y - (FAR_EDGE_OR_BOX));
307325
ratio.y -= gy;
@@ -315,8 +333,8 @@ float bilinear_z_offset(const xy_pos_t &raw) {
315333
nextg.y = _MIN(thisg.y + 1, ABL_BG_POINTS_Y - 1);
316334
}
317335

318-
if (lastg != thisg) {
319-
lastg = thisg;
336+
if (cached_g != thisg) {
337+
cached_g = thisg;
320338
// Z at the box corners
321339
z1 = ABL_BG_GRID(thisg.x, thisg.y); // left-front
322340
d2 = ABL_BG_GRID(thisg.x, nextg.y) - z1; // left-back (delta)
@@ -336,8 +354,8 @@ float bilinear_z_offset(const xy_pos_t &raw) {
336354
/*
337355
static float last_offset = 0;
338356
if (ABS(last_offset - offset) > 0.2) {
339-
SERIAL_ECHOLNPGM("Sudden Shift at x=", rel.x, " / ", bilinear_grid_spacing.x, " -> thisg.x=", thisg.x);
340-
SERIAL_ECHOLNPGM(" y=", rel.y, " / ", bilinear_grid_spacing.y, " -> thisg.y=", thisg.y);
357+
SERIAL_ECHOLNPGM("Sudden Shift at x=", rel.x, " / ", grid_spacing.x, " -> thisg.x=", thisg.x);
358+
SERIAL_ECHOLNPGM(" y=", rel.y, " / ", grid_spacing.y, " -> thisg.y=", thisg.y);
341359
SERIAL_ECHOLNPGM(" ratio.x=", ratio.x, " ratio.y=", ratio.y);
342360
SERIAL_ECHOLNPGM(" z1=", z1, " z2=", z2, " z3=", z3, " z4=", z4);
343361
SERIAL_ECHOLNPGM(" L=", L, " R=", R, " offset=", offset);
@@ -350,13 +368,13 @@ float bilinear_z_offset(const xy_pos_t &raw) {
350368

351369
#if IS_CARTESIAN && DISABLED(SEGMENT_LEVELED_MOVES)
352370

353-
#define CELL_INDEX(A,V) ((V - bilinear_start.A) * ABL_BG_FACTOR(A))
371+
#define CELL_INDEX(A,V) ((V - grid_start.A) * ABL_BG_FACTOR(A))
354372

355373
/**
356374
* Prepare a bilinear-leveled linear move on Cartesian,
357375
* splitting the move where it crosses grid borders.
358376
*/
359-
void bilinear_line_to_destination(const_feedRate_t scaled_fr_mm_s, uint16_t x_splits, uint16_t y_splits) {
377+
void LevelingBilinear::line_to_destination(const_feedRate_t scaled_fr_mm_s, uint16_t x_splits, uint16_t y_splits) {
360378
// Get current and destination cells for this line
361379
xy_int_t c1 { CELL_INDEX(x, current_position.x), CELL_INDEX(y, current_position.y) },
362380
c2 { CELL_INDEX(x, destination.x), CELL_INDEX(y, destination.y) };
@@ -384,7 +402,7 @@ float bilinear_z_offset(const xy_pos_t &raw) {
384402
// Split on the X grid line
385403
CBI(x_splits, gc.x);
386404
end = destination;
387-
destination.x = bilinear_start.x + ABL_BG_SPACING(x) * gc.x;
405+
destination.x = grid_start.x + ABL_BG_SPACING(x) * gc.x;
388406
normalized_dist = (destination.x - current_position.x) / (end.x - current_position.x);
389407
destination.y = LINE_SEGMENT_END(y);
390408
}
@@ -393,7 +411,7 @@ float bilinear_z_offset(const xy_pos_t &raw) {
393411
// Split on the Y grid line
394412
CBI(y_splits, gc.y);
395413
end = destination;
396-
destination.y = bilinear_start.y + ABL_BG_SPACING(y) * gc.y;
414+
destination.y = grid_start.y + ABL_BG_SPACING(y) * gc.y;
397415
normalized_dist = (destination.y - current_position.y) / (end.y - current_position.y);
398416
destination.x = LINE_SEGMENT_END(x);
399417
}
@@ -409,11 +427,11 @@ float bilinear_z_offset(const xy_pos_t &raw) {
409427
destination.e = LINE_SEGMENT_END(e);
410428

411429
// Do the split and look for more borders
412-
bilinear_line_to_destination(scaled_fr_mm_s, x_splits, y_splits);
430+
line_to_destination(scaled_fr_mm_s, x_splits, y_splits);
413431

414432
// Restore destination from stack
415433
destination = end;
416-
bilinear_line_to_destination(scaled_fr_mm_s, x_splits, y_splits);
434+
line_to_destination(scaled_fr_mm_s, x_splits, y_splits);
417435
}
418436

419437
#endif // IS_CARTESIAN && !SEGMENT_LEVELED_MOVES

0 commit comments

Comments
 (0)