From e8394c391ef7bcdf62d268d7ab453e8ad7a1e743 Mon Sep 17 00:00:00 2001
From: Mark <niujl123@sina.com>
Date: Fri, 26 Aug 2022 06:50:03 +0800
Subject: [PATCH] =?UTF-8?q?=F0=9F=90=9B=20Fix=20Bed=20Distance=20Sensor=20?=
 =?UTF-8?q?reading=20(#24649)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Marlin/src/feature/bedlevel/bdl/bdl.cpp | 33 +++++++++++++------------
 Marlin/src/module/probe.cpp             |  4 ++-
 2 files changed, 20 insertions(+), 17 deletions(-)

diff --git a/Marlin/src/feature/bedlevel/bdl/bdl.cpp b/Marlin/src/feature/bedlevel/bdl/bdl.cpp
index 0668eb705c..1a27011a4b 100644
--- a/Marlin/src/feature/bedlevel/bdl/bdl.cpp
+++ b/Marlin/src/feature/bedlevel/bdl/bdl.cpp
@@ -96,22 +96,23 @@ void BDS_Leveling::process() {
       const float z_sensor = (tmp & 0x3FF) / 100.0f;
       if (cur_z < 0) config_state = 0;
       //float abs_z = current_position.z > cur_z ? (current_position.z - cur_z) : (cur_z - current_position.z);
-      if ( cur_z < config_state * 0.1f
-        && config_state > 0
-        && old_cur_z == cur_z
-        && old_buf_z == current_position.z
-        && z_sensor < (MAX_BD_HEIGHT)
-      ) {
-        babystep.set_mm(Z_AXIS, cur_z - z_sensor);
-        #if ENABLED(DEBUG_OUT_BD)
-          SERIAL_ECHOLNPGM("BD:", z_sensor, ", Z:", cur_z, "|", current_position.z);
-        #endif
-      }
-      else {
-        babystep.set_mm(Z_AXIS, 0);
-        //if (old_cur_z <= cur_z) Z_DIR_WRITE(!INVERT_Z_DIR);
-        stepper.set_directions();
-      }
+      #if ENABLED(BABYSTEPPING)
+        if (cur_z < config_state * 0.1f
+          && config_state > 0
+          && old_cur_z == cur_z
+          && old_buf_z == current_position.z
+          && z_sensor < (MAX_BD_HEIGHT)
+        ) {
+          babystep.set_mm(Z_AXIS, cur_z - z_sensor);
+          #if ENABLED(DEBUG_OUT_BD)
+            SERIAL_ECHOLNPGM("BD:", z_sensor, ", Z:", cur_z, "|", current_position.z);
+          #endif
+        }
+        else {
+          babystep.set_mm(Z_AXIS, 0);          //if (old_cur_z <= cur_z) Z_DIR_WRITE(!INVERT_Z_DIR);
+          stepper.set_directions();
+        }
+      #endif
       old_cur_z = cur_z;
       old_buf_z = current_position.z;
       endstops.bdp_state_update(z_sensor <= 0.01f);
diff --git a/Marlin/src/module/probe.cpp b/Marlin/src/module/probe.cpp
index 3baef31479..fa92ae1fb5 100644
--- a/Marlin/src/module/probe.cpp
+++ b/Marlin/src/module/probe.cpp
@@ -882,7 +882,9 @@ float Probe::probe_at_point(const_float_t rx, const_float_t ry, const ProbePtRai
   // Move the probe to the starting XYZ
   do_blocking_move_to(npos, feedRate_t(XY_PROBE_FEEDRATE_MM_S));
 
-  TERN_(BD_SENSOR, return bdl.read());
+  #if ENABLED(BD_SENSOR)
+    return current_position.z - bdl.read(); // Difference between Z-home-relative Z and sensor reading
+  #endif
 
   float measured_z = NAN;
   if (!deploy()) {