2026-11-02 16:58:20 +01:00
|
|
|
# Nozzle touch with bd_pressure, and a comparison with Beacon contact.
|
|
|
|
|
#
|
|
|
|
|
# The bd_pressure endstop output is not wired, so it cannot stop a homing
|
2026-11-02 17:24:12 +01:00
|
|
|
# move. BDP_TOUCH instead lowers the nozzle in one slow continuous move
|
|
|
|
|
# (drip mode, like a homing move) while a timer reads the force (chk_delta,
|
|
|
|
|
# register 64-65 of the load-check firmware) as fast as I2C allows, and
|
|
|
|
|
# stops the move when the force reaches STOP. The Z of each reading comes
|
|
|
|
|
# from stepper_z's position at that time. It has to be one quick press:
|
|
|
|
|
# the sensor sets a new baseline by itself while the force looks steady,
|
|
|
|
|
# so stepping down slowly (tried: 0.005 mm steps, 0.25 mm in 33 s) kept
|
|
|
|
|
# the reading near zero. A quick press measured 2026-11-02 at 150C: about
|
|
|
|
|
# 25 per 0.1 mm (0.1 mm 27, 0.2 mm 52, 0.3 mm 73, 0.4 mm 92).
|
|
|
|
|
# From the curve it reports two touch heights:
|
|
|
|
|
# threshold z: where the force first reaches THRESHOLD (3 readings in
|
|
|
|
|
# a row, interpolated), like an endstop would
|
2026-11-02 16:58:20 +01:00
|
|
|
# fit z: where a straight line through the readings at or above
|
|
|
|
|
# THRESHOLD reaches the no-contact level (the mean of the
|
|
|
|
|
# readings before the descent): the zero-force touch point
|
|
|
|
|
# Z is the toolhead Z (the frame of the last Z homing).
|
|
|
|
|
#
|
|
|
|
|
# TOUCH_COMPARE alternates Beacon's contact probe (PROBE_METHOD=contact,
|
|
|
|
|
# one sample) and BDP_TOUCH at the current X/Y, N times, and reports the
|
|
|
|
|
# mean and spread of each and of the per-pair difference. Run it through
|
|
|
|
|
# the TOUCH_COMPARE macro (macros/sensors/touch_compare.cfg), which heats
|
|
|
|
|
# the nozzle and homes first.
|
|
|
|
|
#
|
2026-11-02 17:32:43 +01:00
|
|
|
# Safety: the move stops when the force reaches STOP (default 60, about
|
|
|
|
|
# 0.27 mm into the bed at 2.2 per 0.01 mm; 80 was out of reach within
|
|
|
|
|
# FLOOR from Z0.15: peak 70 at -0.35; a purge peaks at 64-246) and
|
|
|
|
|
# overshoots by what was already queued (at most ~0.15 s of the move,
|
|
|
|
|
# ~0.05 mm at 0.3 mm/s). The descent ends at FLOOR below the start, and
|
|
|
|
|
# the nozzle always goes back up to the start height, also after an error.
|
|
|
|
|
# It refuses to run while the bdp_guard is armed (in stop mode the touch
|
|
|
|
|
# would shut Klipper down) or while the sensor is in PA mode.
|
2026-11-02 16:58:20 +01:00
|
|
|
#
|
|
|
|
|
# Install: copy to ~/klipper/klippy/extras/bdp_touch.py (install.sh links
|
|
|
|
|
# it) and add
|
|
|
|
|
#
|
|
|
|
|
# [bdp_touch]
|
|
|
|
|
# sensor: bd_pa # name after "bdpressure" in [bdpressure bd_pa]
|
|
|
|
|
#
|
|
|
|
|
# Commands:
|
2026-11-02 17:32:43 +01:00
|
|
|
# BDP_TOUCH [THRESHOLD=15] [STOP=60] [SPEED=0.3] [FLOOR=0.5]
|
2026-11-02 17:24:12 +01:00
|
|
|
# touch at the current X/Y, starting at the current Z (SPEED in mm/s)
|
|
|
|
|
# _TOUCH_COMPARE [N=10] [START=0.1] [THRESHOLD=..] [STOP=..] [SPEED=..]
|
2026-11-02 16:58:20 +01:00
|
|
|
# the comparison loop; START is the BDP_TOUCH start height above the
|
|
|
|
|
# Beacon contact result of the same pair
|
|
|
|
|
#
|
|
|
|
|
# Every touch writes its whole curve to klippy.log ("bdp_touch: curve"),
|
2026-11-02 17:24:12 +01:00
|
|
|
# also when it found no touch, so it can be analysed later.
|
2026-11-02 16:58:20 +01:00
|
|
|
#
|
|
|
|
|
# Status (printer.bdp_touch): threshold_z, fit_z, peak, noise, slope
|
|
|
|
|
# (force per 0.01 mm), points, of the last BDP_TOUCH
|
|
|
|
|
|
|
|
|
|
import logging
|
|
|
|
|
import math
|
|
|
|
|
|
|
|
|
|
REG_MODE = 48 # status_clk: 7 = PA mode, 2 = probe (endstop) mode
|
|
|
|
|
REG_RESET_PROBE = 52 # write 1 = take the current reading as zero
|
|
|
|
|
MODE_PA = 7
|
|
|
|
|
REG_EXT = 54 # extension marker 0xBD + version
|
|
|
|
|
REG_DELTA = 64 # chk_delta, int16 little-endian
|
|
|
|
|
EXT_MAGIC = 0xBD
|
|
|
|
|
LIFT_SPEED = 5.
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _stats(values):
|
|
|
|
|
n = len(values)
|
|
|
|
|
mean = sum(values) / n
|
|
|
|
|
std = math.sqrt(sum((v - mean) ** 2 for v in values) / n)
|
|
|
|
|
return mean, std, max(values) - min(values)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class BDPTouch:
|
|
|
|
|
def __init__(self, config):
|
|
|
|
|
self.printer = config.get_printer()
|
|
|
|
|
self.reactor = self.printer.get_reactor()
|
|
|
|
|
self.sensor_name = 'bdpressure ' + config.get('sensor', 'bd_pa')
|
|
|
|
|
self.sensor = None
|
|
|
|
|
self.toolhead = None
|
|
|
|
|
self.last = {'threshold_z': None, 'fit_z': None, 'peak': 0,
|
|
|
|
|
'noise': 0., 'slope': None, 'points': 0}
|
|
|
|
|
self.printer.register_event_handler('klippy:connect',
|
|
|
|
|
self._handle_connect)
|
|
|
|
|
gcode = self.printer.lookup_object('gcode')
|
|
|
|
|
self.gcode = gcode
|
|
|
|
|
gcode.register_command('BDP_TOUCH', self.cmd_BDP_TOUCH,
|
|
|
|
|
desc="Touch the bed with the nozzle and "
|
|
|
|
|
"measure it with bd_pressure")
|
|
|
|
|
gcode.register_command('_TOUCH_COMPARE', self.cmd_TOUCH_COMPARE,
|
|
|
|
|
desc="Compare Beacon contact and bd_pressure "
|
|
|
|
|
"touches (use the TOUCH_COMPARE macro)")
|
|
|
|
|
|
|
|
|
|
def _handle_connect(self):
|
|
|
|
|
self.sensor = self.printer.lookup_object(self.sensor_name, None)
|
|
|
|
|
self.toolhead = self.printer.lookup_object('toolhead')
|
|
|
|
|
|
|
|
|
|
# Looked up at every use: bdp_guard puts a locked wrapper on the sensor
|
|
|
|
|
@property
|
|
|
|
|
def i2c(self):
|
|
|
|
|
return getattr(self.sensor, 'i2c', None)
|
|
|
|
|
|
|
|
|
|
def _check_ready(self, gcmd):
|
|
|
|
|
if self.i2c is None:
|
|
|
|
|
raise gcmd.error("BDP_TOUCH: no [%s]" % self.sensor_name)
|
|
|
|
|
status = self.toolhead.get_status(self.reactor.monotonic())
|
|
|
|
|
if 'z' not in status['homed_axes']:
|
|
|
|
|
raise gcmd.error("BDP_TOUCH: home Z first")
|
|
|
|
|
guard = self.printer.lookup_object('bdp_guard', None)
|
|
|
|
|
if guard is not None and guard.timer is not None:
|
|
|
|
|
raise gcmd.error("BDP_TOUCH: the bdp_guard is armed "
|
|
|
|
|
"(BDP_GUARD_DISARM first)")
|
|
|
|
|
if getattr(self.sensor, 'last_state', 0):
|
|
|
|
|
raise gcmd.error("BDP_TOUCH: a bd_pressure calibration runs")
|
|
|
|
|
ext = bytearray(self.i2c.i2c_read([REG_EXT], 2)['response'])
|
|
|
|
|
if len(ext) != 2 or ext[0] != EXT_MAGIC:
|
|
|
|
|
raise gcmd.error("BDP_TOUCH: bd_pressure load-check firmware "
|
|
|
|
|
"not found")
|
|
|
|
|
mode = bytearray(self.i2c.i2c_read([REG_MODE], 1)['response'])
|
|
|
|
|
if len(mode) == 1 and mode[0] == MODE_PA:
|
|
|
|
|
raise gcmd.error("BDP_TOUCH: bd_pressure is in PA mode "
|
|
|
|
|
"(home Z once to switch it back)")
|
|
|
|
|
|
|
|
|
|
def _read_delta(self, reads):
|
|
|
|
|
total = 0
|
|
|
|
|
for i in range(reads):
|
|
|
|
|
data = bytearray(self.i2c.i2c_read([REG_DELTA], 2)['response'])
|
|
|
|
|
if len(data) != 2:
|
|
|
|
|
raise self.printer.command_error("BDP_TOUCH: short read")
|
|
|
|
|
d = data[0] | (data[1] << 8)
|
|
|
|
|
total += d - 0x10000 if d & 0x8000 else d
|
|
|
|
|
return total / float(reads)
|
|
|
|
|
|
|
|
|
|
def _move_z(self, z, speed):
|
|
|
|
|
self.toolhead.manual_move([None, None, z], speed)
|
|
|
|
|
self.toolhead.wait_moves()
|
|
|
|
|
|
2026-11-02 17:24:12 +01:00
|
|
|
def touch(self, gcmd, threshold, stop, speed, floor):
|
2026-11-02 16:58:20 +01:00
|
|
|
"""Lower the nozzle from the current Z; returns the result dict."""
|
|
|
|
|
self._check_ready(gcmd)
|
2026-11-02 17:24:12 +01:00
|
|
|
toolhead = self.toolhead
|
|
|
|
|
toolhead.wait_moves()
|
|
|
|
|
startpos = toolhead.get_position()
|
|
|
|
|
start = startpos[2]
|
2026-11-02 16:58:20 +01:00
|
|
|
# New zero with the nozzle free, then the no-contact level and noise
|
|
|
|
|
self.i2c.i2c_write([REG_RESET_PROBE, 1])
|
|
|
|
|
self.reactor.pause(self.reactor.monotonic() + 0.3)
|
|
|
|
|
base = []
|
|
|
|
|
for i in range(10):
|
|
|
|
|
base.append(self._read_delta(1))
|
|
|
|
|
self.reactor.pause(self.reactor.monotonic() + 0.01)
|
|
|
|
|
base_mean, noise, _ = _stats(base)
|
2026-11-02 17:24:12 +01:00
|
|
|
# One continuous move down to the floor in drip mode (like a homing
|
|
|
|
|
# move), stopped early from a timer as soon as the force reaches
|
|
|
|
|
# STOP. Stepping with pauses does not work: the sensor sets a new
|
|
|
|
|
# baseline by itself while the force looks steady, so a slow press
|
|
|
|
|
# (seen: 0.25 mm in 33 s) stays near zero.
|
|
|
|
|
toolhead.flush_step_generation()
|
|
|
|
|
kin = toolhead.get_kinematics()
|
|
|
|
|
steppers = kin.get_steppers()
|
|
|
|
|
kin_spos = {s.get_name(): s.get_commanded_position() for s in steppers}
|
|
|
|
|
start_mcu = {s.get_name(): s.get_mcu_position() for s in steppers}
|
|
|
|
|
zstep = [s for s in steppers if s.get_name() == 'stepper_z'][0]
|
|
|
|
|
done = self.reactor.completion()
|
|
|
|
|
samples = [] # (print_time, delta)
|
|
|
|
|
state = {'errors': 0}
|
|
|
|
|
|
|
|
|
|
def poll(eventtime):
|
|
|
|
|
# Never raise here: an exception in a timer shuts Klipper down
|
|
|
|
|
if done.test():
|
|
|
|
|
return self.reactor.NEVER
|
|
|
|
|
try:
|
|
|
|
|
t0 = self.reactor.monotonic()
|
|
|
|
|
data = bytearray(
|
|
|
|
|
self.i2c.i2c_read([REG_DELTA], 2)['response'])
|
|
|
|
|
t1 = self.reactor.monotonic()
|
|
|
|
|
if len(data) == 2:
|
|
|
|
|
d = data[0] | (data[1] << 8)
|
|
|
|
|
d = d - 0x10000 if d & 0x8000 else d
|
|
|
|
|
pt = toolhead.mcu.estimated_print_time((t0 + t1) / 2.)
|
|
|
|
|
samples.append((pt, d))
|
|
|
|
|
if abs(d - base_mean) >= stop:
|
|
|
|
|
done.complete(True)
|
|
|
|
|
return self.reactor.NEVER
|
|
|
|
|
else:
|
|
|
|
|
state['errors'] += 1
|
|
|
|
|
except Exception:
|
|
|
|
|
state['errors'] += 1
|
|
|
|
|
if state['errors'] == 1:
|
|
|
|
|
logging.exception("bdp_touch: read failed")
|
|
|
|
|
if state['errors'] > 20:
|
|
|
|
|
done.complete(False) # stop the move: no sensor
|
|
|
|
|
return self.reactor.NEVER
|
|
|
|
|
return eventtime + 0.002
|
|
|
|
|
timer = self.reactor.register_timer(poll, self.reactor.NOW)
|
|
|
|
|
error = None
|
2026-11-02 16:58:20 +01:00
|
|
|
try:
|
2026-11-02 17:24:12 +01:00
|
|
|
movepos = list(startpos)
|
|
|
|
|
movepos[2] = start - floor
|
|
|
|
|
toolhead.drip_move(movepos, speed, done)
|
|
|
|
|
except self.printer.command_error as e:
|
|
|
|
|
error = str(e)
|
2026-11-02 16:58:20 +01:00
|
|
|
finally:
|
2026-11-02 17:24:12 +01:00
|
|
|
touched = done.test() and done.wait() is True
|
|
|
|
|
if not done.test():
|
|
|
|
|
done.complete(None)
|
|
|
|
|
self.reactor.unregister_timer(timer)
|
|
|
|
|
# Where the steppers stopped (as homing.py does after a drip
|
|
|
|
|
# move), then back up to the start height
|
|
|
|
|
toolhead.flush_step_generation()
|
|
|
|
|
halt = dict(kin_spos)
|
|
|
|
|
for s in steppers:
|
|
|
|
|
name = s.get_name()
|
|
|
|
|
halt[name] += ((s.get_mcu_position() - start_mcu[name])
|
|
|
|
|
* s.get_step_dist())
|
|
|
|
|
haltpos = list(kin.calc_position(halt))[:3] + startpos[3:]
|
|
|
|
|
toolhead.set_position(haltpos)
|
2026-11-02 16:58:20 +01:00
|
|
|
self._move_z(start, LIFT_SPEED)
|
2026-11-02 17:24:12 +01:00
|
|
|
if error is not None:
|
|
|
|
|
raise gcmd.error("BDP_TOUCH: %s" % error)
|
|
|
|
|
# Z at each reading, from stepper_z's position at that time
|
|
|
|
|
z0 = zstep.mcu_to_commanded_position(start_mcu['stepper_z'])
|
|
|
|
|
curve = [(round(start + zstep.mcu_to_commanded_position(
|
|
|
|
|
zstep.get_past_mcu_position(pt)) - z0, 5), d)
|
|
|
|
|
for pt, d in samples]
|
|
|
|
|
logging.info("bdp_touch: curve start=%.4f base=%.2f noise=%.2f "
|
|
|
|
|
"touched=%s lowest=%.4f errors=%d %s", start, base_mean,
|
|
|
|
|
noise, touched, haltpos[2], state['errors'],
|
|
|
|
|
" ".join("%.4f:%d" % p for p in curve))
|
|
|
|
|
if state['errors'] > 20:
|
|
|
|
|
raise gcmd.error("BDP_TOUCH: bd_pressure stopped answering")
|
|
|
|
|
if not touched:
|
|
|
|
|
raise gcmd.error(
|
|
|
|
|
"BDP_TOUCH: no touch within %.3f mm (peak %.0f)"
|
|
|
|
|
% (floor, max([abs(d - base_mean) for zz, d in curve] or [0])))
|
2026-11-02 16:58:20 +01:00
|
|
|
# The sign of the force depends on the gauge: take it from the end
|
|
|
|
|
sign = 1. if curve[-1][1] - base_mean >= 0 else -1.
|
|
|
|
|
force = [(zz, sign * (d - base_mean)) for zz, d in curve]
|
2026-11-02 17:24:12 +01:00
|
|
|
# Threshold: the first of 3 readings in a row at or above it
|
|
|
|
|
# (one noisy reading does not count), interpolated
|
2026-11-02 16:58:20 +01:00
|
|
|
thr_z = None
|
2026-11-02 17:24:12 +01:00
|
|
|
for i in range(1, len(force) - 2):
|
|
|
|
|
if all(f >= threshold for zz, f in force[i:i + 3]):
|
|
|
|
|
(pz, pf), (zz, f) = force[i - 1], force[i]
|
|
|
|
|
thr_z = zz if f == pf else (
|
|
|
|
|
pz + (zz - pz) * (threshold - pf) / (f - pf))
|
2026-11-02 16:58:20 +01:00
|
|
|
break
|
|
|
|
|
fit_z = slope = None
|
|
|
|
|
pts = [(zz, f) for zz, f in force if f >= threshold]
|
|
|
|
|
if len(pts) >= 3:
|
|
|
|
|
n = len(pts)
|
|
|
|
|
mz = sum(p[0] for p in pts) / n
|
|
|
|
|
mf = sum(p[1] for p in pts) / n
|
|
|
|
|
szz = sum((p[0] - mz) ** 2 for p in pts)
|
|
|
|
|
if szz > 0:
|
|
|
|
|
a = sum((p[0] - mz) * (p[1] - mf) for p in pts) / szz
|
|
|
|
|
if a < 0:
|
|
|
|
|
fit_z = mz - mf / a # where the line reaches force 0
|
|
|
|
|
slope = -a * 0.01
|
|
|
|
|
self.last = {'threshold_z': thr_z, 'fit_z': fit_z,
|
|
|
|
|
'peak': max(f for zz, f in force), 'noise': noise,
|
|
|
|
|
'slope': slope, 'points': len(pts)}
|
|
|
|
|
return self.last
|
|
|
|
|
|
|
|
|
|
def _touch_params(self, gcmd):
|
|
|
|
|
return dict(
|
|
|
|
|
threshold=gcmd.get_float('THRESHOLD', 15., above=0.),
|
2026-11-02 17:32:43 +01:00
|
|
|
stop=gcmd.get_float('STOP', 60., above=0., maxval=150.),
|
2026-11-02 17:24:12 +01:00
|
|
|
speed=gcmd.get_float('SPEED', 0.3, minval=0.05, maxval=1.),
|
|
|
|
|
floor=gcmd.get_float('FLOOR', 0.5, above=0., maxval=1.))
|
2026-11-02 16:58:20 +01:00
|
|
|
|
|
|
|
|
def _fmt(self, v):
|
|
|
|
|
return "-" if v is None else "%.4f" % v
|
|
|
|
|
|
|
|
|
|
def cmd_BDP_TOUCH(self, gcmd):
|
|
|
|
|
p = self._touch_params(gcmd)
|
|
|
|
|
if p['threshold'] >= p['stop']:
|
|
|
|
|
raise gcmd.error("BDP_TOUCH: THRESHOLD must be below STOP")
|
|
|
|
|
r = self.touch(gcmd, **p)
|
|
|
|
|
gcmd.respond_info(
|
|
|
|
|
"BDP_TOUCH: threshold z %s, fit z %s, peak %.0f, noise %.1f, "
|
|
|
|
|
"%s per 0.01 mm, %d points"
|
|
|
|
|
% (self._fmt(r['threshold_z']), self._fmt(r['fit_z']), r['peak'],
|
2026-11-02 17:24:12 +01:00
|
|
|
r['noise'], "-" if r["slope"] is None else "%.1f" % r["slope"],
|
2026-11-02 16:58:20 +01:00
|
|
|
r['points']))
|
|
|
|
|
|
|
|
|
|
def cmd_TOUCH_COMPARE(self, gcmd):
|
|
|
|
|
beacon = self.printer.lookup_object('beacon', None)
|
|
|
|
|
if beacon is None:
|
|
|
|
|
raise gcmd.error("_TOUCH_COMPARE: no [beacon]")
|
|
|
|
|
n = gcmd.get_int('N', 10, minval=2, maxval=50)
|
|
|
|
|
start_above = gcmd.get_float('START', 0.1, minval=0.05, maxval=1.)
|
|
|
|
|
p = self._touch_params(gcmd)
|
|
|
|
|
if p['threshold'] >= p['stop']:
|
|
|
|
|
raise gcmd.error("_TOUCH_COMPARE: THRESHOLD must be below STOP")
|
|
|
|
|
self._check_ready(gcmd)
|
|
|
|
|
rows = []
|
|
|
|
|
for i in range(n):
|
|
|
|
|
self._move_z(2., LIFT_SPEED)
|
|
|
|
|
probe_cmd = self.gcode.create_gcode_command(
|
|
|
|
|
"PROBE", "PROBE", {'PROBE_METHOD': 'contact', 'SAMPLES': '1'})
|
|
|
|
|
bz = beacon.run_probe(probe_cmd)[2]
|
|
|
|
|
# Beacon lifts at least 1 mm after its touch; come down from
|
|
|
|
|
# above to the BDP start height, so both approach downwards
|
|
|
|
|
self._move_z(bz + start_above, LIFT_SPEED)
|
|
|
|
|
r = self.touch(gcmd, **p)
|
|
|
|
|
rows.append((bz, r['threshold_z'], r['fit_z'], r['peak']))
|
|
|
|
|
gcmd.respond_info(
|
|
|
|
|
"touch %d/%d: beacon %.4f, bdp threshold %s, bdp fit %s, "
|
|
|
|
|
"peak %.0f" % (i + 1, n, bz, self._fmt(r['threshold_z']),
|
|
|
|
|
self._fmt(r['fit_z']), r['peak']))
|
|
|
|
|
self._move_z(5., LIFT_SPEED)
|
|
|
|
|
lines = ["TOUCH_COMPARE: %d pairs (mean / std / range, mm)" % n]
|
|
|
|
|
cols = [("beacon contact", [r[0] for r in rows]),
|
|
|
|
|
("bdp threshold", [r[1] for r in rows]),
|
|
|
|
|
("bdp fit", [r[2] for r in rows])]
|
|
|
|
|
for name, vals in cols:
|
|
|
|
|
vals = [v for v in vals if v is not None]
|
|
|
|
|
if len(vals) >= 2:
|
|
|
|
|
lines.append(" %s: %.4f / %.4f / %.4f (%d)"
|
|
|
|
|
% ((name,) + _stats(vals) + (len(vals),)))
|
|
|
|
|
for name, k in (("threshold - beacon", 1), ("fit - beacon", 2)):
|
|
|
|
|
diffs = [r[k] - r[0] for r in rows if r[k] is not None]
|
|
|
|
|
if len(diffs) >= 2:
|
|
|
|
|
lines.append(" %s: %.4f / %.4f / %.4f (%d)"
|
|
|
|
|
% ((name,) + _stats(diffs) + (len(diffs),)))
|
|
|
|
|
msg = "\n".join(lines)
|
|
|
|
|
logging.info("bdp_touch: %s", msg)
|
|
|
|
|
gcmd.respond_info(msg)
|
|
|
|
|
|
|
|
|
|
def get_status(self, eventtime):
|
|
|
|
|
return dict(self.last)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def load_config(config):
|
|
|
|
|
return BDPTouch(config)
|