From 4db1bf8ac689191a02c2f2a37e57c2315cf761a1 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Tue, 3 Mar 2026 20:02:15 -0500 Subject: [PATCH] probe_eddy_current: Calculate "tap" position from lift movement Don't calculate the nozzle/bed contact position from the descent movement. Instead, obtain the data during the lifting movement and determine the contact point by analyzing where the nozzle separates from the bed. This improves the precision and repeatability of the "tap" results. Signed-off-by: Kevin O'Connor --- klippy/extras/probe_eddy_current.py | 132 +++++++++++++++++++--------- 1 file changed, 90 insertions(+), 42 deletions(-) diff --git a/klippy/extras/probe_eddy_current.py b/klippy/extras/probe_eddy_current.py index 586b3a7b5..94bf9a7f8 100644 --- a/klippy/extras/probe_eddy_current.py +++ b/klippy/extras/probe_eddy_current.py @@ -1,9 +1,9 @@ # Support for eddy current based Z probes # -# Copyright (C) 2021-2024 Kevin O'Connor +# Copyright (C) 2021-2026 Kevin O'Connor # # This file may be distributed under the terms of the GNU GPLv3 license. -import logging, math, bisect +import sys, logging, math, bisect import mcu from . import ldc1612, trigger_analog, probe, manual_probe @@ -553,31 +553,6 @@ class EddyTap: if tmin < cycle_time * 0.75: raise cmderr("Eddy: CLKIN frequency too low: %.3f < %.3f" % (tmin, cycle_time * 0.75)) - 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 _pull_tap_time(self, measures): - tap_time = [] - tap_value = [] - for time, freq, z in measures: - tap_time.append(time) - tap_value.append(freq) - # 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() @@ -585,12 +560,89 @@ class EddyTap: s.get_past_mcu_position(pos_time)) for s in kin.get_steppers()} return kin.calc_position(kin_spos) - def _analyze_tap(self, measures, start_time, end_time): + def _calc_least_squares(self, eqs, ans, est_z_contact): + # XXX - this implementation is not efficient + len_data = len(eqs) + import numpy + for i in range(len_data): + eq = eqs[i] + step_z = eq[1] + if step_z < est_z_contact: + eq[2] = eq[3] = 0. + continue + eq[2] = (step_z - est_z_contact) + eq[3] = (step_z - est_z_contact)**2 + res = numpy.linalg.lstsq(eqs, ans, rcond=None) + coeffs = list(res[0]) + if coeffs[3] < 0.: + # z**2 factor can't be negative - retry using only linear + res = numpy.linalg.lstsq(eqs[:][:,:3], ans, rcond=None) + coeffs = list(res[0]) + [0.] + if not res[1]: + err = sys.float_info.max + else: + err = res[1][0] + #logging.info("z=%.6f err=%.3f coeffs=%s", est_z_contact, err, coeffs) + return err, coeffs + def _find_least_squares(self, data): + len_data = len(data) + import numpy + # Populate initial numpy linear least squares arrays + eqs = numpy.zeros((len_data, 4)) + ans = numpy.zeros((len_data,)) + for i, (sensor_freq, tool_pos) in enumerate(data): + ans[i] = sensor_freq + eq = eqs[i] + eq[0] = 1. + eq[1] = tool_pos[2] + #logging.info("sample: freq=%.3f z=%.6f", sensor_freq, tool_pos[2]) + # Run least squares with various z values to reduce residual error + min_z = best_z = eqs[0][1] + max_z = eqs[-1][1] + best_err = sys.float_info.max + best_coeffs = [0., 0., 0., 0.] + while max_z - min_z > 0.000250: + # Select z value to check + mid_z = (min_z + max_z) * .5 + if best_z < mid_z: + guess_z = (best_z + max_z) * .5 + else: + guess_z = (min_z + best_z) * .5 + # Calculate least squares error for given z + guess_err, coeffs = self._calc_least_squares(eqs, ans, guess_z) + # Update search bounds + if guess_err < best_err: + if guess_z > best_z: + min_z = best_z + else: + max_z = best_z + best_z = guess_z + best_err = guess_err + best_coeffs = coeffs + else: + if guess_z > best_z: + max_z = guess_z + else: + min_z = guess_z + best_coeffs = [float(v) for v in best_coeffs] + #logging.info("best: z=%.6f err=%.6f coeffs=%s", + # best_z, best_err, best_coeffs) + return float(best_z), best_coeffs + def _analyze_pullback(self, measures, start_time, end_time): self._validate_samples_time(measures, start_time, end_time) - pos_time = self._pull_tap_time(measures) - trig_pos = self._lookup_toolhead_pos(pos_time) - return manual_probe.create_probe_result(trig_pos, - (0., 0., self._tap_z_offset)) + # Correlate measurements to toolhead position at time of measurement + data = [(sensor_freq, self._lookup_toolhead_pos(samp_time)) + for samp_time, sensor_freq, sensor_z in measures] + # Find best fit for extracted measurements + z_contact, coeffs = self._find_least_squares(data) + # Report probe position + trig_idx = len(data)-1 + while trig_idx > 0 and data[trig_idx-1][1][2] > z_contact: + trig_idx -= 1 + trig_pos = data[trig_idx][1] + adj_z_contact = z_contact - self._tap_z_offset + return manual_probe.ProbeResult(trig_pos[0], trig_pos[1], adj_z_contact, + trig_pos[0], trig_pos[1], trig_pos[2]) # Probe session interface def start_probe_session(self, gcmd): self._prep_trigger_analog_tap(gcmd) @@ -604,23 +656,19 @@ class EddyTap: speed = params['probe_speed'] lift_speed = params['lift_speed'] lift_dist = gcmd.get_float('SAMPLE_RETRACT_DIST', 4., above=0.) - 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 - 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, - start_time, end_time) # Perform lifting move haltpos = toolhead.get_position() haltpos[2] += lift_dist + retract_start_time = toolhead.get_last_move_time() toolhead.manual_move(haltpos, lift_speed) + # Extract retract samples + start_time = retract_start_time - 0.010 + end_time = retract_start_time + 0.150 + self._gather.add_probe_request(self._analyze_pullback, start_time, + end_time, start_time, end_time) def pull_probed_results(self): return self._gather.pull_probed() def end_probe_session(self):