From d06661051434937660d274cd32863dfaadd4f763 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Thu, 29 Sep 2016 01:01:38 -0500
Subject: [PATCH] Patch up bilinear_z_offset
---
Marlin/Conditionals_post.h | 7 +++++-
Marlin/Marlin_main.cpp | 44 +++++++++++++++++++++++++-------------
2 files changed, 35 insertions(+), 16 deletions(-)
diff --git a/Marlin/Conditionals_post.h b/Marlin/Conditionals_post.h
index a7a8a6ccea..85da6c3a12 100644
--- a/Marlin/Conditionals_post.h
+++ b/Marlin/Conditionals_post.h
@@ -648,8 +648,13 @@
#define ABL_GRID (ENABLED(AUTO_BED_LEVELING_LINEAR) || ENABLED(AUTO_BED_LEVELING_BILINEAR))
#define HAS_ABL (ABL_PLANAR || ABL_GRID)
- #define HAS_PROBING_PROCEDURE (HAS_ABL || ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST))
#define PLANNER_LEVELING (HAS_ABL || ENABLED(MESH_BED_LEVELING))
+ #define HAS_PROBING_PROCEDURE (HAS_ABL || ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST))
+
+ #if HAS_PROBING_PROCEDURE
+ #define PROBE_BED_WIDTH abs(RIGHT_PROBE_BED_POSITION - (LEFT_PROBE_BED_POSITION))
+ #define PROBE_BED_HEIGHT abs(BACK_PROBE_BED_POSITION - (FRONT_PROBE_BED_POSITION))
+ #endif
/**
* Buzzer/Speaker
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 2006c2801c..ff7bc4abc1 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -7997,36 +7997,50 @@ void ok_to_send() {
ratio_y = y / bilinear_grid_spacing[Y_AXIS];
// Whole unit is the grid box index
- int gridx = constrain(int(ratio_x), 0, ABL_GRID_POINTS_X - 2),
- gridy = constrain(int(ratio_y), 0, ABL_GRID_POINTS_Y - 2);
+ const int gridx = constrain(floor(ratio_x), 0, ABL_GRID_POINTS_X - 2),
+ gridy = constrain(floor(ratio_y), 0, ABL_GRID_POINTS_Y - 2),
+ nextx = gridx + (x < PROBE_BED_WIDTH ? 1 : 0),
+ nexty = gridy + (y < PROBE_BED_HEIGHT ? 1 : 0);
// Subtract whole to get the ratio within the grid box
- ratio_x -= gridx, ratio_y -= gridy;
+ ratio_x = constrain(ratio_x - gridx, 0.0, 1.0);
+ ratio_y = constrain(ratio_y - gridy, 0.0, 1.0);
- // Z at the box corners
- const float z1 = bed_level_grid[gridx][gridy], // left-front
- z2 = bed_level_grid[gridx][gridy + 1], // left-back
- z3 = bed_level_grid[gridx + 1][gridy], // right-front
- z4 = bed_level_grid[gridx + 1][gridy + 1], // right-back
+ // Z at the box corners
+ const float z1 = bed_level_grid[gridx][gridy], // left-front
+ z2 = bed_level_grid[gridx][nexty], // left-back
+ z3 = bed_level_grid[nextx][gridy], // right-front
+ z4 = bed_level_grid[nextx][nexty], // right-back
+ // Bilinear interpolate
L = z1 + (z2 - z1) * ratio_y, // Linear interp. LF -> LB
- R = z3 + (z4 - z3) * ratio_y; // Linear interp. RF -> RB
+ R = z3 + (z4 - z3) * ratio_y, // Linear interp. RF -> RB
+ offset = L + ratio_x * (R - L);
/*
- SERIAL_ECHOPAIR("gridx=", gridx);
- SERIAL_ECHOPAIR(" gridy=", gridy);
+ static float last_offset = 0;
+ if (fabs(last_offset - offset) > 0.2) {
+ SERIAL_ECHOPGM("Sudden Shift at ");
+ SERIAL_ECHOPAIR("x=", x);
+ SERIAL_ECHOPAIR(" / ", bilinear_grid_spacing[X_AXIS]);
+ SERIAL_ECHOLNPAIR(" -> gridx=", gridx);
+ SERIAL_ECHOPAIR(" y=", y);
+ SERIAL_ECHOPAIR(" / ", bilinear_grid_spacing[Y_AXIS]);
+ SERIAL_ECHOLNPAIR(" -> gridy=", gridy);
SERIAL_ECHOPAIR(" ratio_x=", ratio_x);
- SERIAL_ECHOPAIR(" ratio_y=", ratio_y);
+ SERIAL_ECHOLNPAIR(" ratio_y=", ratio_y);
SERIAL_ECHOPAIR(" z1=", z1);
SERIAL_ECHOPAIR(" z2=", z2);
SERIAL_ECHOPAIR(" z3=", z3);
- SERIAL_ECHOPAIR(" z4=", z4);
+ SERIAL_ECHOLNPAIR(" z4=", z4);
SERIAL_ECHOPAIR(" L=", L);
SERIAL_ECHOPAIR(" R=", R);
- SERIAL_ECHOPAIR(" offset=", L + ratio_x * (R - L));
+ SERIAL_ECHOLNPAIR(" offset=", offset);
+ }
+ last_offset = offset;
//*/
- return L + ratio_x * (R - L);
+ return offset;
}
#endif // AUTO_BED_LEVELING_BILINEAR