# Nozzle collision guard with bd_pressure (load-check firmware extension). # # The bd_pressure strain gauge sees the force on the nozzle. The load-check # firmware keeps the current force minus its baseline in register 64-65 # (chk_delta), also outside a load check; it is the same value the endstop # output switches on (|delta| >= thrhold). While the guard is armed this # module reads it every INTERVAL seconds. In stop mode, SAMPLES reads in a # row at or above THRESHOLD shut Klipper down at once (like M112, without # waiting for the running macro to finish). # # Arm it only around moves where the nozzle must not touch anything: not # around contact homing, extrusion, purges or AFC cuts and loads, which # press on the nozzle on purpose. ARM and DISARM act immediately, not in # the move queue: put an M400 before them (the _GUARD_ON/_GUARD_OFF macros # do this). # # Install: copy to ~/klipper/klippy/extras/bdp_guard.py and add # # [bdp_guard] # sensor: bd_pa # name after "bdpressure" in [bdpressure bd_pa] # mode: log # log: only measure; stop: shut down on a touch # threshold: 30 # |delta| that counts as a touch # samples: 3 # reads in a row at or above threshold # interval: 0.02 # seconds between reads while armed # timeout: 600 # disarm by itself when a phase runs this long # # (a macro that failed before its DISARM) # # Commands: # BDP_GUARD_ARM PHASE= [MODE=log|stop] start (or switch phase) # BDP_GUARD_DISARM stop and report the phase # BDP_GUARD_QUERY report without stopping # # Each phase reports (console and klippy.log, "bdp_guard:") the reads, the # peak |delta|, the longest run of reads at or above threshold and the # read errors, so a log-mode print start shows what each phase normally # measures before stop mode is switched on. # # Status (printer.bdp_guard): armed, mode, phase, threshold, samples, peak, # longest_run, reads, errors, available import logging REG_EXT = 54 # extension marker 0xBD + version REG_DELTA = 64 # chk_delta, int16 little-endian EXT_MAGIC = 0xBD MODES = ('log', 'stop') class BDPGuard: 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.mode = config.getchoice('mode', {m: m for m in MODES}, 'log') self.threshold = config.getint('threshold', 30, minval=1) self.samples = config.getint('samples', 3, minval=1) self.interval = config.getfloat('interval', 0.02, minval=0.005) self.timeout = config.getfloat('timeout', 600., minval=10.) self.armed_until = 0. self.sensor = self.i2c = None self.available = False self.timer = None self.phase_mode = self.mode self._reset_phase('') self.printer.register_event_handler('klippy:connect', self._handle_connect) self.printer.register_event_handler('klippy:shutdown', self._handle_shutdown) gcode = self.printer.lookup_object('gcode') gcode.register_command('BDP_GUARD_ARM', self.cmd_ARM, desc="Arm the bd_pressure nozzle collision guard") gcode.register_command('BDP_GUARD_DISARM', self.cmd_DISARM, desc="Disarm the bd_pressure nozzle collision guard") gcode.register_command('BDP_GUARD_QUERY', self.cmd_QUERY, desc="Report the bd_pressure guard values") def _handle_connect(self): self.sensor = self.printer.lookup_object(self.sensor_name, None) self.i2c = getattr(self.sensor, 'i2c', None) def _handle_shutdown(self): self._stop_timer() def _reset_phase(self, phase): self.phase = phase self.peak = self.longest_run = self.run = 0 self.reads = self.errors = 0 def _check_available(self): if self.i2c is None: return False try: data = bytearray(self.i2c.i2c_read([REG_EXT], 2)['response']) except Exception: logging.exception("bdp_guard: reading the extension marker failed") return False return len(data) == 2 and data[0] == EXT_MAGIC and data[1] >= 1 def _stop_timer(self): if self.timer is not None: self.reactor.unregister_timer(self.timer) self.timer = None def _poll(self, eventtime): # Never raise here: an exception in a timer shuts Klipper down. try: if self.printer.is_shutdown(): return self.reactor.NEVER if eventtime > self.armed_until: logging.info("bdp_guard: %s armed longer than %.0f s, " "disarmed (%d reads, peak %d)", self.phase, self.timeout, self.reads, self.peak) self._stop_timer() self.phase = '' return self.reactor.NEVER # PA mode (SET_BDPRESSURE START): the delta means nothing then if getattr(self.sensor, 'last_state', 0): return eventtime + self.interval data = bytearray(self.i2c.i2c_read([REG_DELTA], 2)['response']) if len(data) != 2: self.errors += 1 return eventtime + self.interval delta = data[0] | (data[1] << 8) if delta & 0x8000: delta -= 0x10000 mag = abs(delta) self.reads += 1 self.peak = max(self.peak, mag) self.run = self.run + 1 if mag >= self.threshold else 0 self.longest_run = max(self.longest_run, self.run) if self.run >= self.samples and self.phase_mode == 'stop': # klippy:shutdown unregisters this timer self.printer.invoke_shutdown( "bd_pressure guard: nozzle touched something during %s " "(delta %d, threshold %d)" % (self.phase or 'an armed move', delta, self.threshold)) return self.reactor.NEVER except Exception: self.errors += 1 if self.errors == 1: # 50 reads/s: only the first per phase logging.exception("bdp_guard: read failed") return eventtime + self.interval def _report(self, gcmd, prefix): msg = ("%s %s (%s): %d reads, peak %d, longest run >= %d: %d reads, " "%d errors" % (prefix, self.phase or '-', self.phase_mode, self.reads, self.peak, self.threshold, self.longest_run, self.errors)) logging.info("bdp_guard: %s", msg) gcmd.respond_info(msg) def cmd_ARM(self, gcmd): phase = gcmd.get('PHASE', 'armed') mode = gcmd.get('MODE', self.mode).lower() if mode not in MODES: raise gcmd.error("BDP_GUARD_ARM: MODE must be log or stop") if self.timer is not None: self._report(gcmd, "BDP_GUARD") else: self.available = self._check_available() if not self.available: gcmd.respond_info("BDP_GUARD_ARM: bd_pressure load-check " "extension not found, guard not armed") return self.phase_mode = mode self._reset_phase(phase) self.armed_until = self.reactor.monotonic() + self.timeout if self.timer is None: self.timer = self.reactor.register_timer( self._poll, self.reactor.monotonic() + self.interval) def cmd_DISARM(self, gcmd): if self.timer is None: return self._stop_timer() self._report(gcmd, "BDP_GUARD") self.phase = '' def cmd_QUERY(self, gcmd): self._report(gcmd, "BDP_GUARD_QUERY" + (" armed" if self.timer else " not armed")) def get_status(self, eventtime): return {'armed': self.timer is not None, 'mode': self.phase_mode, 'phase': self.phase, 'threshold': self.threshold, 'samples': self.samples, 'peak': self.peak, 'longest_run': self.longest_run, 'reads': self.reads, 'errors': self.errors, 'available': self.available} def load_config(config): return BDPGuard(config)