🐛 Fix Bed Distance Sensor reading (#24649)

This commit is contained in:
Mark 2022-08-26 06:50:03 +08:00 committed by Scott Lahteine
parent e24d5f9315
commit e8394c391e
2 changed files with 20 additions and 17 deletions

View File

@ -96,22 +96,23 @@ void BDS_Leveling::process() {
const float z_sensor = (tmp & 0x3FF) / 100.0f; const float z_sensor = (tmp & 0x3FF) / 100.0f;
if (cur_z < 0) config_state = 0; 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); //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 #if ENABLED(BABYSTEPPING)
&& config_state > 0 if (cur_z < config_state * 0.1f
&& old_cur_z == cur_z && config_state > 0
&& old_buf_z == current_position.z && old_cur_z == cur_z
&& z_sensor < (MAX_BD_HEIGHT) && 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) babystep.set_mm(Z_AXIS, cur_z - z_sensor);
SERIAL_ECHOLNPGM("BD:", z_sensor, ", Z:", cur_z, "|", current_position.z); #if ENABLED(DEBUG_OUT_BD)
#endif SERIAL_ECHOLNPGM("BD:", z_sensor, ", Z:", cur_z, "|", current_position.z);
} #endif
else { }
babystep.set_mm(Z_AXIS, 0); else {
//if (old_cur_z <= cur_z) Z_DIR_WRITE(!INVERT_Z_DIR); babystep.set_mm(Z_AXIS, 0); //if (old_cur_z <= cur_z) Z_DIR_WRITE(!INVERT_Z_DIR);
stepper.set_directions(); stepper.set_directions();
} }
#endif
old_cur_z = cur_z; old_cur_z = cur_z;
old_buf_z = current_position.z; old_buf_z = current_position.z;
endstops.bdp_state_update(z_sensor <= 0.01f); endstops.bdp_state_update(z_sensor <= 0.01f);

View File

@ -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 // Move the probe to the starting XYZ
do_blocking_move_to(npos, feedRate_t(XY_PROBE_FEEDRATE_MM_S)); 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; float measured_z = NAN;
if (!deploy()) { if (!deploy()) {