|
@@ -490,7 +490,7 @@ static uint8_t target_extruder;
|
490
|
490
|
#endif
|
491
|
491
|
|
492
|
492
|
#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
|
493
|
|
- int bilinear_grid_spacing[2] = { 0 };
|
|
493
|
+ int bilinear_grid_spacing[2] = { 0 }, bilinear_start[2] = { 0 };
|
494
|
494
|
float bed_level_grid[ABL_GRID_POINTS_X][ABL_GRID_POINTS_Y];
|
495
|
495
|
#endif
|
496
|
496
|
|
|
@@ -3679,9 +3679,15 @@ inline void gcode_G28() {
|
3679
|
3679
|
float zoffset = zprobe_zoffset;
|
3680
|
3680
|
if (code_seen('Z')) zoffset += code_value_axis_units(Z_AXIS);
|
3681
|
3681
|
|
3682
|
|
- if (xGridSpacing != bilinear_grid_spacing[X_AXIS] || yGridSpacing != bilinear_grid_spacing[Y_AXIS]) {
|
|
3682
|
+ if ( xGridSpacing != bilinear_grid_spacing[X_AXIS]
|
|
3683
|
+ || yGridSpacing != bilinear_grid_spacing[Y_AXIS]
|
|
3684
|
+ || left_probe_bed_position != bilinear_start[X_AXIS]
|
|
3685
|
+ || front_probe_bed_position != bilinear_start[Y_AXIS]
|
|
3686
|
+ ) {
|
3683
|
3687
|
bilinear_grid_spacing[X_AXIS] = xGridSpacing;
|
3684
|
3688
|
bilinear_grid_spacing[Y_AXIS] = yGridSpacing;
|
|
3689
|
+ bilinear_start[X_AXIS] = RAW_X_POSITION(left_probe_bed_position);
|
|
3690
|
+ bilinear_start[Y_AXIS] = RAW_Y_POSITION(front_probe_bed_position);
|
3685
|
3691
|
// Can't re-enable (on error) until the new grid is written
|
3686
|
3692
|
abl_should_enable = false;
|
3687
|
3693
|
}
|
|
@@ -7930,38 +7936,43 @@ void ok_to_send() {
|
7930
|
7936
|
// Get the Z adjustment for non-linear bed leveling
|
7931
|
7937
|
float bilinear_z_offset(float cartesian[XYZ]) {
|
7932
|
7938
|
|
7933
|
|
- int half_x = (ABL_GRID_POINTS_X - 1) / 2,
|
7934
|
|
- half_y = (ABL_GRID_POINTS_Y - 1) / 2;
|
7935
|
|
- float hx2 = half_x - 0.001, hx1 = -hx2,
|
7936
|
|
- hy2 = half_y - 0.001, hy1 = -hy2,
|
7937
|
|
- grid_x = max(hx1, min(hx2, RAW_X_POSITION(cartesian[X_AXIS]) / bilinear_grid_spacing[X_AXIS])),
|
7938
|
|
- grid_y = max(hy1, min(hy2, RAW_Y_POSITION(cartesian[Y_AXIS]) / bilinear_grid_spacing[Y_AXIS]));
|
7939
|
|
- int floor_x = floor(grid_x), floor_y = floor(grid_y);
|
7940
|
|
- float ratio_x = grid_x - floor_x, ratio_y = grid_y - floor_y,
|
7941
|
|
- z1 = bed_level_grid[floor_x + half_x][floor_y + half_y],
|
7942
|
|
- z2 = bed_level_grid[floor_x + half_x][floor_y + half_y + 1],
|
7943
|
|
- z3 = bed_level_grid[floor_x + half_x + 1][floor_y + half_y],
|
7944
|
|
- z4 = bed_level_grid[floor_x + half_x + 1][floor_y + half_y + 1],
|
7945
|
|
- left = (1 - ratio_y) * z1 + ratio_y * z2,
|
7946
|
|
- right = (1 - ratio_y) * z3 + ratio_y * z4;
|
|
7939
|
+ int gridx = (cartesian[X_AXIS] - bilinear_start[X_AXIS]) / bilinear_grid_spacing[X_AXIS],
|
|
7940
|
+ gridy = (cartesian[Y_AXIS] - bilinear_start[Y_AXIS]) / bilinear_grid_spacing[Y_AXIS];
|
|
7941
|
+
|
|
7942
|
+ // What grid box is xy inside?
|
|
7943
|
+ if (gridx < 0) gridx = 0;
|
|
7944
|
+ if (gridx > ABL_GRID_POINTS_X - 1) gridx = ABL_GRID_POINTS_X - 1;
|
|
7945
|
+ if (gridy < 0) gridy = 0;
|
|
7946
|
+ if (gridy > ABL_GRID_POINTS_Y - 1) gridy = ABL_GRID_POINTS_Y - 1;
|
|
7947
|
+
|
|
7948
|
+ // Ratio within the grid box
|
|
7949
|
+ float ratio_x = cartesian[X_AXIS] / bilinear_grid_spacing[X_AXIS] - gridx,
|
|
7950
|
+ ratio_y = cartesian[Y_AXIS] / bilinear_grid_spacing[Y_AXIS] - gridy,
|
|
7951
|
+
|
|
7952
|
+ // Z at the box corners
|
|
7953
|
+ z1 = bed_level_grid[gridx][gridy], // left-front
|
|
7954
|
+ z2 = bed_level_grid[gridx][gridy + 1], // left-back
|
|
7955
|
+ z3 = bed_level_grid[gridx + 1][gridy], // right-front
|
|
7956
|
+ z4 = bed_level_grid[gridx + 1][gridy + 1], // right-back
|
|
7957
|
+
|
|
7958
|
+ L = z1 + (z2 - z1) * ratio_y, // Linear interp. LF -> LB
|
|
7959
|
+ R = z3 + (z4 - z3) * ratio_y; // Linear interp. RF -> RB
|
7947
|
7960
|
|
7948
|
7961
|
/*
|
7949
|
|
- SERIAL_ECHOPAIR("grid_x=", grid_x);
|
7950
|
|
- SERIAL_ECHOPAIR(" grid_y=", grid_y);
|
7951
|
|
- SERIAL_ECHOPAIR(" floor_x=", floor_x);
|
7952
|
|
- SERIAL_ECHOPAIR(" floor_y=", floor_y);
|
|
7962
|
+ SERIAL_ECHOPAIR("gridx=", gridx);
|
|
7963
|
+ SERIAL_ECHOPAIR(" gridy=", gridy);
|
7953
|
7964
|
SERIAL_ECHOPAIR(" ratio_x=", ratio_x);
|
7954
|
7965
|
SERIAL_ECHOPAIR(" ratio_y=", ratio_y);
|
7955
|
7966
|
SERIAL_ECHOPAIR(" z1=", z1);
|
7956
|
7967
|
SERIAL_ECHOPAIR(" z2=", z2);
|
7957
|
7968
|
SERIAL_ECHOPAIR(" z3=", z3);
|
7958
|
7969
|
SERIAL_ECHOPAIR(" z4=", z4);
|
7959
|
|
- SERIAL_ECHOPAIR(" left=", left);
|
7960
|
|
- SERIAL_ECHOPAIR(" right=", right);
|
7961
|
|
- SERIAL_ECHOPAIR(" offset=", (1 - ratio_x) * left + ratio_x * right);
|
|
7970
|
+ SERIAL_ECHOPAIR(" L=", L);
|
|
7971
|
+ SERIAL_ECHOPAIR(" R=", R);
|
|
7972
|
+ SERIAL_ECHOPAIR(" offset=", L + ratio_x * (R - L);
|
7962
|
7973
|
//*/
|
7963
|
7974
|
|
7964
|
|
- return (1 - ratio_x) * left + ratio_x * right;
|
|
7975
|
+ return L + ratio_x * (R - L);
|
7965
|
7976
|
}
|
7966
|
7977
|
|
7967
|
7978
|
#endif // AUTO_BED_LEVELING_BILINEAR
|