diff --git a/Marlin/src/feature/bedlevel/bdl/bdl.cpp b/Marlin/src/feature/bedlevel/bdl/bdl.cpp index 14989a07d3..5345bb78c5 100644 --- a/Marlin/src/feature/bedlevel/bdl/bdl.cpp +++ b/Marlin/src/feature/bedlevel/bdl/bdl.cpp @@ -78,8 +78,8 @@ bool BDS_Leveling::check(const uint16_t data, const bool raw_data/*=false*/, con return true; // error } if (raw_data == true) { - if (hicheck && (data & 0x3FF) > 550) - SERIAL_ECHOLNPGM("BD Sensor mounted too high!"); + if (hicheck && (data & 0x3FF) > 400) + SERIAL_ECHOLNPGM("Bad BD Sensor height! Recommended distance 0.5-2.0mm"); else if (!good_data(data)) SERIAL_ECHOLNPGM("Invalid data, please calibrate."); else @@ -109,7 +109,8 @@ void BDS_Leveling::process() { static float zpos = 0.0f; const millis_t ms = millis(); if (ELAPSED(ms, next_check_ms)) { // timed out (or first run) - next_check_ms = ms + (config_state < BDS_IDLE ? 200 : 50); // check at 5Hz or 20Hz + // Check at 1KHz, 5Hz, or 20Hz + next_check_ms = ms + (config_state == BDS_HOMING_Z ? 1 : (config_state < BDS_IDLE ? 200 : 50)); uint16_t tmp = 0; const float cur_z = planner.get_axis_position_mm(Z_AXIS) - pos_zero_offset; @@ -127,16 +128,14 @@ void BDS_Leveling::process() { babystep.set_mm(Z_AXIS, cur_z - z_sensor); DEBUG_ECHOLNPGM("BD:", z_sensor, ", Z:", cur_z, "|", current_position.z); } - else { - babystep.set_mm(Z_AXIS, 0); //if (old_cur_z <= cur_z) Z_DIR_WRITE(HIGH); - //stepper.apply_directions(); // TODO: Remove this line as probably not needed - } + else + babystep.set_mm(Z_AXIS, 0); } #endif old_cur_z = cur_z; old_buf_z = current_position.z; - endstops.bdp_state_update(z_sensor <= 0.01f); + endstops.bdp_state_update(z_sensor <= BD_SENSOR_HOME_Z_POSITION); #if HAS_STATUS_MESSAGE static float old_z_sensor = 0; @@ -149,8 +148,10 @@ void BDS_Leveling::process() { } #endif } - else - stepper.apply_directions(); + else if (config_state == BDS_HOMING_Z) { + SERIAL_ECHOLNPGM("Read:", tmp); + kill(F("BDsensor connect Err!")); + } DEBUG_ECHOLNPGM("BD:", tmp & 0x3FF, " Z:", cur_z, "|", current_position.z); if (TERN0(DEBUG_OUT_BD, BD_I2C_SENSOR.BD_Check_OddEven(tmp) == 0)) DEBUG_ECHOLNPGM("CRC error"); @@ -233,11 +234,13 @@ void BDS_Leveling::process() { sprintf_P(tmp_1, PSTR("G1Z%d.%d"), int(zpos), int(zpos * 10) % 10); gcode.process_subcommands_now(tmp_1); SERIAL_ECHO(tmp_1); SERIAL_ECHOLNPGM(", Z:", current_position.z); - for (float tmp_k = 0; abs(zpos - tmp_k) > 0.004f;) { + uint16_t failcount = 300; + for (float tmp_k = 0; abs(zpos - tmp_k) > 0.006f && failcount--;) { tmp_k = planner.get_axis_position_mm(Z_AXIS) - pos_zero_offset; safe_delay(10); + if (!failcount--) break; } - safe_delay(zpos <= 0.4f ? 600 : 100); + safe_delay(600); tmp = uint16_t((zpos + 0.00001f) * 10); BD_I2C_SENSOR.BD_i2c_write(tmp); SERIAL_ECHOLNPGM("w:", tmp, ", Z:", zpos); diff --git a/Marlin/src/feature/bedlevel/bdl/bdl.h b/Marlin/src/feature/bedlevel/bdl/bdl.h index b3037bc1c2..ed91d7081b 100644 --- a/Marlin/src/feature/bedlevel/bdl/bdl.h +++ b/Marlin/src/feature/bedlevel/bdl/bdl.h @@ -23,6 +23,10 @@ #include +#ifndef BD_SENSOR_HOME_Z_POSITION + #define BD_SENSOR_HOME_Z_POSITION 0.5 +#endif + enum BDS_State : int8_t { BDS_IDLE, BDS_VERSION = -1, diff --git a/Marlin/src/gcode/bedlevel/abl/G29.cpp b/Marlin/src/gcode/bedlevel/abl/G29.cpp index 5ecb6af66c..d6d648c125 100644 --- a/Marlin/src/gcode/bedlevel/abl/G29.cpp +++ b/Marlin/src/gcode/bedlevel/abl/G29.cpp @@ -748,6 +748,7 @@ G29_TYPE GcodeSuite::G29() { } //if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM_P(axis == Y_AXIS ? PSTR("Y=") : PSTR("X=", pos); + safe_delay(4); abl.measured_z = current_position.z - bdl.read(); if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("x_cur ", planner.get_axis_position_mm(X_AXIS), " z ", abl.measured_z); diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp index 80729df759..272b16c9ac 100644 --- a/Marlin/src/module/motion.cpp +++ b/Marlin/src/module/motion.cpp @@ -2479,7 +2479,12 @@ void set_axis_is_at_home(const AxisEnum axis) { #if HAS_BED_PROBE && Z_HOME_TO_MIN if (axis == Z_AXIS) { #if HOMING_Z_WITH_PROBE - current_position.z -= probe.offset.z; + #if ENABLED(BD_SENSOR) + safe_delay(100); + current_position.z = bdl.read(); + #else + current_position.z -= probe.offset.z; + #endif if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("*** Z homed with PROBE" TERN_(Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN, " (Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN)") " ***\n> (M851 Z", probe.offset.z, ")"); #else if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("*** Z homed to ENDSTOP ***"); diff --git a/Marlin/src/module/probe.cpp b/Marlin/src/module/probe.cpp index 68fcfd00e0..989965d6d8 100644 --- a/Marlin/src/module/probe.cpp +++ b/Marlin/src/module/probe.cpp @@ -980,6 +980,7 @@ float Probe::probe_at_point(const_float_t rx, const_float_t ry, const ProbePtRai #if ENABLED(BD_SENSOR) + safe_delay(4); return current_position.z - bdl.read(); // Difference between Z-home-relative Z and sensor reading #else // !BD_SENSOR