From 79ae9ace16c672a4f7515dfb0dd2e6c7b86c4fec Mon Sep 17 00:00:00 2001 From: Timofey Titovets Date: Fri, 31 Oct 2025 02:18:01 +0100 Subject: [PATCH] ldc1612: implement tap support Simple MCU side negative peak detection. Host uses the raw MCU side timings for now. Signed-off-by: Timofey Titovets --- docs/Config_Reference.md | 3 ++ klippy/extras/ldc1612.py | 73 +++++++++++++++++++++----- klippy/extras/probe_eddy_current.py | 48 +++++++++++++---- src/Kconfig | 2 +- src/sensor_ldc1612.c | 80 ++++++++++++++++++++++++++--- 5 files changed, 176 insertions(+), 30 deletions(-) diff --git a/docs/Config_Reference.md b/docs/Config_Reference.md index 528465e64..1ad45003b 100644 --- a/docs/Config_Reference.md +++ b/docs/Config_Reference.md @@ -2293,6 +2293,9 @@ sensor_type: ldc1612 #samples_tolerance: #samples_tolerance_retries: # See the "probe" section for information on these parameters. +#tap_threshold: 0 +# Tap sensitivity, increase value to make it less sensitive +# Zero is disabled ``` ### [axis_twist_compensation] diff --git a/klippy/extras/ldc1612.py b/klippy/extras/ldc1612.py index 973556af1..98e33b248 100644 --- a/klippy/extras/ldc1612.py +++ b/klippy/extras/ldc1612.py @@ -4,7 +4,7 @@ # # This file may be distributed under the terms of the GNU GPLv3 license. import logging -from . import bus, bulk_sensor +from . import bus, bulk_sensor, sos_filter MIN_MSG_TIME = 0.100 @@ -84,24 +84,56 @@ class LDC1612: default_addr=LDC1612_ADDR, default_speed=400000) self.mcu = mcu = self.i2c.get_mcu() - self.oid = oid = mcu.create_oid() + self.oid = mcu.create_oid() self.query_ldc1612_cmd = None self.ldc1612_setup_home_cmd = self.query_ldc1612_home_state_cmd = None self.frequency = config.getint("frequency", DEFAULT_LDC1612_FREQ, 2000000, 40000000) - if config.get('intb_pin', None) is not None: + intb_pin = config.get('intb_pin', None) + pin_params = {} + if intb_pin is not None: ppins = config.get_printer().lookup_object("pins") - pin_params = ppins.lookup_pin(config.get('intb_pin')) + pin_params = ppins.lookup_pin(intb_pin) if pin_params['chip'] != mcu: raise config.error("ldc1612 intb_pin must be on same mcu") - mcu.add_config_cmd( - "config_ldc1612_with_intb oid=%d i2c_oid=%d intb_pin=%s" - % (oid, self.i2c.get_oid(), pin_params['pin'])) + # Tap setup + self.tap_threshold = config.getint('tap_threshold', minval=0, + default=0) + # Set empty filter + self._design = sos_filter.DigitalFilter(self.data_rate, config.error) + initial_state = self._design.get_initial_state() + sections = self._design.get_filter_sections() + fixed_filter = sos_filter.FixedPointSosFilter(sections, initial_state) + self._qfrac_bits = 4 + if self.tap_threshold: + design = sos_filter.DigitalFilter(self.data_rate, config.error, + lowpass=25.0, + lowpass_order=4) + self._design = design + def_coil_freq = 3000000 + conv_value = self._coil_freq2raw_fixed_point(def_coil_freq) + # Make initial state represent raw value at frequency + initial_state = self._design.get_initial_state() * conv_value + sections = self._design.get_filter_sections() + fixed_filter = sos_filter.FixedPointSosFilter( + sections, initial_state, + value_int_bits=(31 - self._qfrac_bits)) + # Init with sos + cmdqueue = self.i2c.get_command_queue() + self.sos_filter = sos_filter.SosFilter(mcu, cmdqueue, fixed_filter) + self.sos_filter.create_filter() + if pin_params: + self.mcu.add_config_cmd( + "config_ldc1612_with_intb oid=%d i2c_oid=%d sos_filter_oid=%d" + " intb_pin=%s" % ( + self.oid, self.i2c.get_oid(), self.sos_filter.get_oid(), + pin_params['pin'],)) else: - mcu.add_config_cmd("config_ldc1612 oid=%d i2c_oid=%d" - % (oid, self.i2c.get_oid())) - mcu.add_config_cmd("query_ldc1612 oid=%d rest_ticks=0" - % (oid,), on_restart=True) + self.mcu.add_config_cmd( + "config_ldc1612 oid=%d i2c_oid=%d sos_filter_oid=%d" % ( + self.oid, self.i2c.get_oid(), self.sos_filter.get_oid())) + self.mcu.add_config_cmd("query_ldc1612 oid=%d rest_ticks=0" + % (self.oid,), on_restart=True) mcu.register_config_callback(self._build_config) # Bulk sample message reading chip_smooth = self.data_rate * BATCH_UPDATES * 2 @@ -122,12 +154,20 @@ class LDC1612: self.ffreader.setup_query_command("query_status_ldc1612 oid=%c", oid=self.oid, cq=cmdqueue) self.ldc1612_setup_home_cmd = self.mcu.lookup_command( - "ldc1612_setup_home oid=%c clock=%u threshold=%u" + "ldc1612_setup_home oid=%c clock=%u threshold=%i" " trsync_oid=%c trigger_reason=%c error_reason=%c", cq=cmdqueue) self.query_ldc1612_home_state_cmd = self.mcu.lookup_query_command( "query_ldc1612_home_state oid=%c", "ldc1612_home_state oid=%c homing=%c trigger_clock=%u", oid=self.oid, cq=cmdqueue) + def _coil_freq2raw_fixed_point(self, coil_freq): + freq2raw = 1 / (self.frequency / (1 << 28)) + # If there will be the ability to set the filter state + # right before tap to actual value + # It is done this way + raw_value = coil_freq * freq2raw + fixed_point_raw_value = raw_value / (1 << self._qfrac_bits) + return fixed_point_raw_value def get_mcu(self): return self.i2c.get_mcu() def read_reg(self, reg): @@ -144,6 +184,8 @@ class LDC1612: trsync_oid, hit_reason, err_reason): clock = self.mcu.print_time_to_clock(print_time) tfreq = int(trigger_freq * (1<<28) / float(self.frequency) + 0.5) + if tfreq >= 0x0fffffff: + self.printer.command_error("Trigger frequency is too high") self.ldc1612_setup_home_cmd.send( [self.oid, clock, tfreq, trsync_oid, hit_reason, err_reason]) def clear_home(self): @@ -153,6 +195,13 @@ class LDC1612: params = self.query_ldc1612_home_state_cmd.send([self.oid]) tclock = self.mcu.clock32_to_clock64(params['trigger_clock']) return self.mcu.clock_to_print_time(tclock) + def setup_home_tap(self, print_time, trsync_oid, hit_reason, err_reason): + if not self.tap_threshold: + raise self.printer.command_error("tap_threshold is zero") + clock = self.mcu.print_time_to_clock(print_time) + threshold = -1 * self.tap_threshold + self.ldc1612_setup_home_cmd.send( + [self.oid, clock, threshold, trsync_oid, hit_reason, err_reason]) # Measurement decoding def _convert_samples(self, samples): freq_conv = float(self.frequency) / (1<<28) diff --git a/klippy/extras/probe_eddy_current.py b/klippy/extras/probe_eddy_current.py index 779a904fb..607e18d85 100644 --- a/klippy/extras/probe_eddy_current.py +++ b/klippy/extras/probe_eddy_current.py @@ -320,6 +320,8 @@ class EddyGatherSamples: # No sensor readings - raise error in pull_probed() return 0. return samp_sum / samp_count + def _pull_tap_time(self, start_time, end_time): + return (start_time + end_time) / 2 def _lookup_toolhead_pos(self, pos_time): toolhead = self._printer.lookup_object('toolhead') kin = toolhead.get_kinematics() @@ -332,12 +334,18 @@ class EddyGatherSamples: start_time, end_time, pos_time, toolhead_pos = self._probe_times[0] if self._samples[-1]['data'][-1][0] < end_time: break - freq = self._pull_freq(start_time, end_time) - if pos_time is not None: + # Scanning/Probe + if pos_time or toolhead_pos: + if pos_time is not None: + toolhead_pos = self._lookup_toolhead_pos(pos_time) + sensor_z = None + freq = self._pull_freq(start_time, end_time) + if freq: + sensor_z = self._calibration.freq_to_height(freq) + else: + pos_time = self._pull_tap_time(start_time, end_time) toolhead_pos = self._lookup_toolhead_pos(pos_time) - sensor_z = None - if freq: - sensor_z = self._calibration.freq_to_height(freq) + sensor_z = toolhead_pos[2] self._probe_results.append((sensor_z, toolhead_pos)) self._probe_times.pop(0) def pull_probed(self): @@ -350,12 +358,18 @@ class EddyGatherSamples: if sensor_z <= -OUT_OF_RANGE or sensor_z >= OUT_OF_RANGE: raise self._printer.command_error( "probe_eddy_current sensor not in valid range") + if sensor_z is toolhead_pos[2]: + results.append(toolhead_pos) + continue # Callers expect position relative to z_offset, so recalculate bed_deviation = toolhead_pos[2] - sensor_z toolhead_pos[2] = self._z_offset + bed_deviation results.append(toolhead_pos) del self._probe_results[:] return results + def note_tap(self, start_time, end_time): + self._probe_times.append((start_time, end_time, None, None)) + self._check_samples() def note_probe(self, start_time, end_time, toolhead_pos): self._probe_times.append((start_time, end_time, None, toolhead_pos)) self._check_samples() @@ -377,6 +391,7 @@ class EddyDescend: self._dispatch = mcu.TriggerDispatch(self._mcu) self._trigger_time = 0. self._gather = None + self._is_tap = False probe.LookupZSteppers(config, self._dispatch.add_stepper) # Interface for phoming.probing_move() def get_steppers(self): @@ -384,11 +399,16 @@ class EddyDescend: def home_start(self, print_time, sample_time, sample_count, rest_time, triggered=True): self._trigger_time = 0. - trigger_freq = self._calibration.height_to_freq(self._z_offset) trigger_completion = self._dispatch.start(print_time) - self._sensor_helper.setup_home( - print_time, trigger_freq, self._dispatch.get_oid(), - mcu.MCU_trsync.REASON_ENDSTOP_HIT, self.REASON_SENSOR_ERROR) + if self._is_tap: + self._sensor_helper.setup_home_tap( + print_time, self._dispatch.get_oid(), + mcu.MCU_trsync.REASON_ENDSTOP_HIT, self.REASON_SENSOR_ERROR) + else: + trigger_freq = self._calibration.height_to_freq(self._z_offset) + self._sensor_helper.setup_home( + print_time, trigger_freq, self._dispatch.get_oid(), + mcu.MCU_trsync.REASON_ENDSTOP_HIT, self.REASON_SENSOR_ERROR) return trigger_completion def home_wait(self, home_end_time): self._dispatch.wait_end(home_end_time) @@ -407,6 +427,8 @@ class EddyDescend: return trigger_time # Probe session interface def start_probe_session(self, gcmd): + method = gcmd.get('METHOD', 'automatic').lower() if gcmd else None + self._is_tap = method == 'tap' self._gather = EddyGatherSamples(self._printer, self._sensor_helper, self._calibration, self._z_offset) return self @@ -416,11 +438,19 @@ class EddyDescend: pos[2] = self._z_min_position speed = self._param_helper.get_probe_params(gcmd)['probe_speed'] # Perform probing move + if self._is_tap: + # Give the SOS filter time to stabilize + toolhead.dwell(0.035) phoming = self._printer.lookup_object('homing') trig_pos = phoming.probing_move(self, pos, speed) if not self._trigger_time: return trig_pos # Extract samples + if self._is_tap: + start_time = self._trigger_time - 0.075 + end_time = self._trigger_time + 0.075 + self._gather.note_tap(start_time, end_time) + return start_time = self._trigger_time + 0.050 end_time = start_time + 0.100 toolhead_pos = toolhead.get_position() diff --git a/src/Kconfig b/src/Kconfig index 6740946d8..36a6cd215 100644 --- a/src/Kconfig +++ b/src/Kconfig @@ -183,7 +183,7 @@ config WANT_LOAD_CELL_PROBE default y config NEED_SOS_FILTER bool - depends on WANT_LOAD_CELL_PROBE + depends on WANT_LOAD_CELL_PROBE || WANT_LDC1612 default y menu "Optional features (to reduce code size)" depends on HAVE_LIMITED_CODE_SIZE diff --git a/src/sensor_ldc1612.c b/src/sensor_ldc1612.c index 8b67884f1..d19399bea 100644 --- a/src/sensor_ldc1612.c +++ b/src/sensor_ldc1612.c @@ -14,10 +14,12 @@ #include "sched.h" // DECL_TASK #include "sensor_bulk.h" // sensor_bulk_report #include "trsync.h" // trsync_do_trigger +#include "sos_filter.h" // sos_filter_oid_lookup enum { LDC_PENDING = 1<<0, LDC_HAVE_INTB = 1<<1, - LH_AWAIT_HOMING = 1<<1, LH_CAN_TRIGGER = 1<<2 + LH_AWAIT_HOMING = 1<<1, LH_CAN_TRIGGER = 1<<2, + LT_AWAIT_HOMING = 1<<3, LT_CAN_TRIGGER = 1<<4, }; struct ldc1612 { @@ -31,8 +33,13 @@ struct ldc1612 { struct trsync *ts; uint8_t homing_flags; uint8_t trigger_reason, error_reason; - uint32_t trigger_threshold; + int32_t trigger_threshold; uint32_t homing_clock; + // tap + struct sos_filter *sf; + int32_t prev_sample; + int32_t peak_instant_delta; + uint32_t trigger_start; }; static struct task_wake ldc1612_wake; @@ -66,19 +73,22 @@ command_config_ldc1612(uint32_t *args) , sizeof(*ld)); ld->timer.func = ldc1612_event; ld->i2c = i2cdev_oid_lookup(args[1]); + ld->sf = sos_filter_oid_lookup(args[2]); } -DECL_COMMAND(command_config_ldc1612, "config_ldc1612 oid=%c i2c_oid=%c"); +DECL_COMMAND(command_config_ldc1612, + "config_ldc1612 oid=%c i2c_oid=%c sos_filter_oid=%c"); void command_config_ldc1612_with_intb(uint32_t *args) { command_config_ldc1612(args); struct ldc1612 *ld = oid_lookup(args[0], command_config_ldc1612); - ld->intb_pin = gpio_in_setup(args[2], 1); + ld->intb_pin = gpio_in_setup(args[3], 1); ld->flags = LDC_HAVE_INTB; } DECL_COMMAND(command_config_ldc1612_with_intb, - "config_ldc1612_with_intb oid=%c i2c_oid=%c intb_pin=%c"); + "config_ldc1612_with_intb oid=%c i2c_oid=%c sos_filter_oid=%c" + " intb_pin=%c"); void command_ldc1612_setup_home(uint32_t *args) @@ -95,22 +105,75 @@ command_ldc1612_setup_home(uint32_t *args) ld->ts = trsync_oid_lookup(args[3]); ld->trigger_reason = args[4]; ld->error_reason = args[5]; - ld->homing_flags = LH_AWAIT_HOMING | LH_CAN_TRIGGER; + if (ld->trigger_threshold > 0) + ld->homing_flags = LH_AWAIT_HOMING | LH_CAN_TRIGGER; + else + ld->homing_flags = LT_AWAIT_HOMING | LT_CAN_TRIGGER; + ld->prev_sample = 0; + ld->peak_instant_delta = 0; } DECL_COMMAND(command_ldc1612_setup_home, - "ldc1612_setup_home oid=%c clock=%u threshold=%u" + "ldc1612_setup_home oid=%c clock=%u threshold=%i" " trsync_oid=%c trigger_reason=%c error_reason=%c"); void command_query_ldc1612_home_state(uint32_t *args) { struct ldc1612 *ld = oid_lookup(args[0], command_config_ldc1612); + uint8_t flags = ld->homing_flags; + uint8_t in_progress = !!(flags & (LH_CAN_TRIGGER | LT_CAN_TRIGGER)); sendf("ldc1612_home_state oid=%c homing=%c trigger_clock=%u" - , args[0], !!(ld->homing_flags & LH_CAN_TRIGGER), ld->homing_clock); + , args[0], in_progress, ld->homing_clock); } DECL_COMMAND(command_query_ldc1612_home_state, "query_ldc1612_home_state oid=%c"); +// Check if a sample should trigger a tap event +static void +check_tap(struct ldc1612 *ld, uint32_t data) +{ + uint8_t flags = ld->homing_flags; + if (!(flags & LT_CAN_TRIGGER)) + return; + // Filter Amplitude error bit + data &= ~(1 << 28); + if (data > 0x0fffffff) { + // Sensor reports an issue - cancel homing + ld->homing_flags = 0; + trsync_do_trigger(ld->ts, ld->error_reason); + return; + } + // Raw data is noisy, it is filtered and stabilized + int32_t idata = sosfilt(ld->sf, data); + // Wait until homing sequence starts + uint32_t time = timer_read_time(); + if ((flags & LT_AWAIT_HOMING) && timer_is_before(time, ld->homing_clock)) { + ld->prev_sample = idata; + return; + } + // Homing move started + flags &= ~LT_AWAIT_HOMING; + int32_t delta = (idata - ld->prev_sample); + ld->prev_sample = idata; + // As long as the conductive bed is moving towards the sensor + // Frequency is stable or increasing + // Delta is >= 0 and increasing + // Backtrack delta peak + if (delta > ld->peak_instant_delta) { + ld->trigger_start = time; + ld->peak_instant_delta = delta; + } + int32_t delta_to_peak = delta - ld->peak_instant_delta; + // Upon tap delta would start to decrease + // the delta after the peak is now negative + if (delta_to_peak < ld->trigger_threshold) { + flags = 0; + ld->homing_clock = ld->trigger_start; + trsync_do_trigger(ld->ts, ld->trigger_reason); + } + ld->homing_flags = flags; +} + // Check if a sample should trigger a homing event static void check_home(struct ldc1612 *ld, uint32_t data) @@ -185,6 +248,7 @@ ldc1612_query(struct ldc1612 *ld, uint8_t oid) | ((uint32_t)d[2] << 8) | ((uint32_t)d[3]); check_home(ld, data); + check_tap(ld, data); // Flush local buffer if needed if (ld->sb.data_count + BYTES_PER_SAMPLE > ARRAY_SIZE(ld->sb.data))