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.)]