35
35
#include " ../../../lcd/extui/ui_api.h"
36
36
#endif
37
37
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;
41
46
42
47
/* *
43
48
* Extrapolate a single point from its neighbors
44
49
*/
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) {
46
51
if (!isnan (z_values[x][y])) return ;
47
52
if (DEBUGGING (LEVELING)) {
48
53
DEBUG_ECHOPGM (" Extrapolate [" );
@@ -92,11 +97,26 @@ static void extrapolate_one_point(const uint8_t x, const uint8_t y, const int8_t
92
97
#endif
93
98
#endif
94
99
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
+
95
115
/* *
96
116
* Fill in the unprobed points (corners of circular print surface)
97
117
* using linear extrapolation, away from the center.
98
118
*/
99
- void extrapolate_unprobed_bed_level () {
119
+ void LevelingBilinear:: extrapolate_unprobed_bed_level () {
100
120
#ifdef HALF_IN_X
101
121
constexpr uint8_t ctrx2 = 0 , xend = GRID_MAX_POINTS_X - 1 ;
102
122
#else
@@ -131,35 +151,31 @@ void extrapolate_unprobed_bed_level() {
131
151
#endif
132
152
extrapolate_one_point (x2, y2, -1 , -1 ); // right-above - -
133
153
}
134
-
135
154
}
136
155
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
138
158
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
142
167
}
143
168
144
169
#if ENABLED(ABL_BILINEAR_SUBDIVISION)
145
170
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
148
171
#define ABL_TEMP_POINTS_X (GRID_MAX_POINTS_X + 2 )
149
172
#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;
160
176
161
177
#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) {
163
179
uint8_t ep = 0 , ip = 1 ;
164
180
if (x > (GRID_MAX_POINTS_X) + 1 || y > (GRID_MAX_POINTS_Y) + 1 ) {
165
181
// The requested point requires extrapolating two points beyond the mesh.
@@ -204,7 +220,7 @@ void print_bilinear_leveling_grid() {
204
220
return z_values[x - 1 ][y - 1 ];
205
221
}
206
222
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) {
208
224
return (
209
225
p[i-1 ] * -t * sq (1 - t)
210
226
+ p[i] * (2 - 5 * sq (t) + 3 * t * sq (t))
@@ -213,7 +229,7 @@ void print_bilinear_leveling_grid() {
213
229
) * 0 .5f ;
214
230
}
215
231
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) {
217
233
float row[4 ], column[4 ];
218
234
LOOP_L_N (i, 4 ) {
219
235
LOOP_L_N (j, 4 ) {
@@ -224,9 +240,9 @@ void print_bilinear_leveling_grid() {
224
240
return bed_level_virt_cmr (row, 1 , tx);
225
241
}
226
242
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 ();
230
246
LOOP_L_N (y, GRID_MAX_POINTS_Y)
231
247
LOOP_L_N (x, GRID_MAX_POINTS_X)
232
248
LOOP_L_N (ty, BILINEAR_SUBDIVISIONS)
@@ -244,47 +260,49 @@ void print_bilinear_leveling_grid() {
244
260
}
245
261
#endif // ABL_BILINEAR_SUBDIVISION
246
262
263
+
247
264
// 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 () {
250
266
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 ;
251
269
}
252
270
253
271
#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
256
274
#define ABL_BG_POINTS_X ABL_GRID_POINTS_VIRT_X
257
275
#define ABL_BG_POINTS_Y ABL_GRID_POINTS_VIRT_Y
258
276
#define ABL_BG_GRID (X,Y ) z_values_virt[X][Y]
259
277
#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
262
280
#define ABL_BG_POINTS_X GRID_MAX_POINTS_X
263
281
#define ABL_BG_POINTS_Y GRID_MAX_POINTS_Y
264
282
#define ABL_BG_GRID (X,Y ) z_values[X][Y]
265
283
#endif
266
284
267
285
// 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) {
269
287
270
288
static float z1, d2, z3, d4, L, D;
271
289
272
- static xy_pos_t prev { - 999.999 , - 999.999 }, ratio;
290
+ static xy_pos_t ratio;
273
291
274
292
// 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;
276
294
277
295
// XY relative to the probed area
278
- xy_pos_t rel = raw - bilinear_start .asFloat ();
296
+ xy_pos_t rel = raw - grid_start .asFloat ();
279
297
280
298
#if ENABLED(EXTRAPOLATE_BEYOND_GRID)
281
299
#define FAR_EDGE_OR_BOX 2 // Keep using the last grid box
282
300
#else
283
301
#define FAR_EDGE_OR_BOX 1 // Just use the grid far edge
284
302
#endif
285
303
286
- if (prev .x != rel.x ) {
287
- prev .x = rel.x ;
304
+ if (cached_rel .x != rel.x ) {
305
+ cached_rel .x = rel.x ;
288
306
ratio.x = rel.x * ABL_BG_FACTOR (x);
289
307
const float gx = constrain (FLOOR (ratio.x ), 0 , ABL_BG_POINTS_X - (FAR_EDGE_OR_BOX));
290
308
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) {
298
316
nextg.x = _MIN (thisg.x + 1 , ABL_BG_POINTS_X - 1 );
299
317
}
300
318
301
- if (prev .y != rel.y || lastg .x != thisg.x ) {
319
+ if (cached_rel .y != rel.y || cached_g .x != thisg.x ) {
302
320
303
- if (prev .y != rel.y ) {
304
- prev .y = rel.y ;
321
+ if (cached_rel .y != rel.y ) {
322
+ cached_rel .y = rel.y ;
305
323
ratio.y = rel.y * ABL_BG_FACTOR (y);
306
324
const float gy = constrain (FLOOR (ratio.y ), 0 , ABL_BG_POINTS_Y - (FAR_EDGE_OR_BOX));
307
325
ratio.y -= gy;
@@ -315,8 +333,8 @@ float bilinear_z_offset(const xy_pos_t &raw) {
315
333
nextg.y = _MIN (thisg.y + 1 , ABL_BG_POINTS_Y - 1 );
316
334
}
317
335
318
- if (lastg != thisg) {
319
- lastg = thisg;
336
+ if (cached_g != thisg) {
337
+ cached_g = thisg;
320
338
// Z at the box corners
321
339
z1 = ABL_BG_GRID (thisg.x , thisg.y ); // left-front
322
340
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) {
336
354
/*
337
355
static float last_offset = 0;
338
356
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);
341
359
SERIAL_ECHOLNPGM(" ratio.x=", ratio.x, " ratio.y=", ratio.y);
342
360
SERIAL_ECHOLNPGM(" z1=", z1, " z2=", z2, " z3=", z3, " z4=", z4);
343
361
SERIAL_ECHOLNPGM(" L=", L, " R=", R, " offset=", offset);
@@ -350,13 +368,13 @@ float bilinear_z_offset(const xy_pos_t &raw) {
350
368
351
369
#if IS_CARTESIAN && DISABLED(SEGMENT_LEVELED_MOVES)
352
370
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))
354
372
355
373
/* *
356
374
* Prepare a bilinear-leveled linear move on Cartesian,
357
375
* splitting the move where it crosses grid borders.
358
376
*/
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) {
360
378
// Get current and destination cells for this line
361
379
xy_int_t c1 { CELL_INDEX (x, current_position.x ), CELL_INDEX (y, current_position.y ) },
362
380
c2 { CELL_INDEX (x, destination.x ), CELL_INDEX (y, destination.y ) };
@@ -384,7 +402,7 @@ float bilinear_z_offset(const xy_pos_t &raw) {
384
402
// Split on the X grid line
385
403
CBI (x_splits, gc.x );
386
404
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 ;
388
406
normalized_dist = (destination.x - current_position.x ) / (end.x - current_position.x );
389
407
destination.y = LINE_SEGMENT_END (y);
390
408
}
@@ -393,7 +411,7 @@ float bilinear_z_offset(const xy_pos_t &raw) {
393
411
// Split on the Y grid line
394
412
CBI (y_splits, gc.y );
395
413
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 ;
397
415
normalized_dist = (destination.y - current_position.y ) / (end.y - current_position.y );
398
416
destination.x = LINE_SEGMENT_END (x);
399
417
}
@@ -409,11 +427,11 @@ float bilinear_z_offset(const xy_pos_t &raw) {
409
427
destination.e = LINE_SEGMENT_END (e);
410
428
411
429
// 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);
413
431
414
432
// Restore destination from stack
415
433
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);
417
435
}
418
436
419
437
#endif // IS_CARTESIAN && !SEGMENT_LEVELED_MOVES
0 commit comments