From 0795fb014175386ef22bc2d9c42b745e0183b506 Mon Sep 17 00:00:00 2001 From: Timofey Titovets Date: Thu, 20 Nov 2025 00:53:14 +0100 Subject: [PATCH] probe_eddy_current: analyze tap data To cancel out any lag, filter data on the host Then to avoid derivatives lag, compute central difference. Assume that peak velocity is always the moment right before collision happens. Signed-off-by: Timofey Titovets Signed-off-by: Kevin O'Connor --- klippy/extras/probe_eddy_current.py | 68 ++++++++++++++++++++++++++--- klippy/extras/trigger_analog.py | 3 ++ 2 files changed, 65 insertions(+), 6 deletions(-) diff --git a/klippy/extras/probe_eddy_current.py b/klippy/extras/probe_eddy_current.py index 509fe923f..e3dd7270e 100644 --- a/klippy/extras/probe_eddy_current.py +++ b/klippy/extras/probe_eddy_current.py @@ -516,8 +516,60 @@ class EddyTap: raw_threshold = convert_frequency(self._tap_threshold) self._trigger_analog.set_trigger('diff_peak_gt', raw_threshold) # Measurement analysis to determine "tap" position - def _analyze_tap(self, measures, trig_pos): - # XXX - for now just use trigger position (this is not very accurate) + def central_diff(self, times, values): + velocity = [0.0] * len(values) + for i in range(1, len(values) - 1): + delta_v = (values[i+1] - values[i-1]) + delta_t = (times[i+1] - times[i-1]) + velocity[i] = delta_v / delta_t + velocity[0] = (values[1] - values[0]) / (times[1] - times[0]) + velocity[-1] = (values[-1] - values[-2]) / (times[-1] - times[-2]) + return velocity + def validate_samples_time(self, timestamps): + sps = self._sensor_helper.get_samples_per_second() + cycle_time = 1.0 / sps + SYNC_SLACK = 0.001 + for i in range(1, len(timestamps)): + tdiff = timestamps[i] - timestamps[i-1] + if cycle_time + SYNC_SLACK < tdiff: + logging.error("Eddy: Gaps in the data: %.3f < %.3f" % ( + (cycle_time + SYNC_SLACK, tdiff) + )) + break + if cycle_time - SYNC_SLACK > tdiff: + logging.error( + "Eddy: CLKIN frequency too low: %.3f > %.3f" % ( + (cycle_time - SYNC_SLACK, tdiff) + )) + break + def _pull_tap_time(self, measures): + tap_time = [] + tap_value = [] + for time, freq, z in measures: + tap_time.append(time) + tap_value.append(freq) + # If samples have gaps this will not produce adequate data + self.validate_samples_time(tap_time) + # Do the same filtering as on the MCU but without induced lag + main_design = self._filter_design.get_main_filter() + try: + fvals = main_design.filtfilt(tap_value) + except ValueError as e: + raise self._printer.command_error(str(e)) + velocity = self.central_diff(tap_time, fvals) + peak_velocity = max(velocity) + i = velocity.index(peak_velocity) + return tap_time[i] + def _lookup_toolhead_pos(self, pos_time): + toolhead = self._printer.lookup_object('toolhead') + kin = toolhead.get_kinematics() + kin_spos = {s.get_name(): s.mcu_to_commanded_position( + s.get_past_mcu_position(pos_time)) + for s in kin.get_steppers()} + return kin.calc_position(kin_spos) + def _analyze_tap(self, measures): + pos_time = self._pull_tap_time(measures) + trig_pos = self._lookup_toolhead_pos(pos_time) return manual_probe.ProbeResult(trig_pos[0], trig_pos[1], trig_pos[2], trig_pos[0], trig_pos[1], trig_pos[2]) # Probe session interface @@ -530,14 +582,18 @@ class EddyTap: pos = toolhead.get_position() pos[2] = self._z_min_position speed = self._param_helper.get_probe_params(gcmd)['probe_speed'] + move_start_time = toolhead.get_last_move_time() # Perform probing move phoming = self._printer.lookup_object('homing') trig_pos = phoming.probing_move(self._trigger_analog, pos, speed) # Extract samples - start_time = self._trigger_analog.get_last_trigger_time() - 0.025 - end_time = start_time + 0.025 - self._gather.add_probe_request(self._analyze_tap, - start_time, end_time, trig_pos) + trigger_time = self._trigger_analog.get_last_trigger_time() + start_time = trigger_time - 0.250 + if start_time < move_start_time: + # Filter short move + start_time = move_start_time + end_time = trigger_time + self._gather.add_probe_request(self._analyze_tap, start_time, end_time) def pull_probed_results(self): return self._gather.pull_probed() def end_probe_session(self): diff --git a/klippy/extras/trigger_analog.py b/klippy/extras/trigger_analog.py index 223f83768..6f79c0486 100644 --- a/klippy/extras/trigger_analog.py +++ b/klippy/extras/trigger_analog.py @@ -81,6 +81,9 @@ class DerivativeFilter: def __init__(self, main_filter): self._main_filter = main_filter + def get_main_filter(self): + return self._main_filter + def get_filter_sections(self): s = list(self._main_filter.get_filter_sections()) return s + [(1., -1., 0., 1., 0., 0.)]