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 <nefelim4ag@gmail.com>
Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

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):

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