probe_eddy_current: Support calibrating Z height to sensor frequency

Add a calibration tool that can be used to correlate sensor frequency
to bed Z height.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

2 files changed, 190 insertions, 3 deletions

diff --git a/klippy/extras/ldc1612.py b/klippy/extras/ldc1612.py
index b6a87156..5c09940d 100644
--- a/klippy/extras/ldc1612.py
+++ b/klippy/extras/ldc1612.py
@@ -77,8 +77,9 @@ class DriveCurrentCalibrate:
 
 # Interface class to LDC1612 mcu support
 class LDC1612:
-    def __init__(self, config):
+    def __init__(self, config, calibration=None):
         self.printer = config.get_printer()
+        self.calibration = calibration
         self.dccal = DriveCurrentCalibrate(config, self)
         self.data_rate = 250
         # Setup mcu sensor_ldc1612 bulk query code
@@ -105,7 +106,7 @@ class LDC1612:
             self.printer, self._process_batch,
             self._start_measurements, self._finish_measurements, BATCH_UPDATES)
         self.name = config.get_name().split()[-1]
-        hdr = ('time', 'frequency')
+        hdr = ('time', 'frequency', 'z')
         self.batch_bulk.add_mux_endpoint("ldc1612/dump_ldc1612", "sensor",
                                          self.name, {'header': hdr})
     def _build_config(self):
@@ -114,6 +115,8 @@ class LDC1612:
             "query_ldc1612 oid=%c rest_ticks=%u", cq=cmdqueue)
         self.clock_updater.setup_query_command(
             self.mcu, "query_ldc1612_status oid=%c", oid=self.oid, cq=cmdqueue)
+    def get_mcu(self):
+        return self.i2c.get_mcu()
     def read_reg(self, reg):
         params = self.i2c.i2c_read([reg], 2)
         response = bytearray(params['response'])
@@ -144,7 +147,7 @@ class LDC1612:
                     self.last_error_count += 1
                 val = ((v[0] & 0x0f) << 24) | (v[1] << 16) | (v[2] << 8) | v[3]
                 ptime = round(time_base + (msg_cdiff + i) * inv_freq, 6)
-                samples[count] = (ptime, round(freq_conv * val, 3))
+                samples[count] = (ptime, round(freq_conv * val, 3), 999.9)
                 count += 1
         self.clock_sync.set_last_chip_clock(seq * SAMPLES_PER_BLOCK + i)
         del samples[count:]
@@ -192,5 +195,7 @@ class LDC1612:
         samples = self._extract_samples(raw_samples)
         if not samples:
             return {}
+        if self.calibration is not None:
+            self.calibration.apply_calibration(samples)
         return {'data': samples, 'errors': self.last_error_count,
                 'overflows': self.clock_updater.get_last_overflows()}
diff --git a/klippy/extras/probe_eddy_current.py b/klippy/extras/probe_eddy_current.py
new file mode 100644
index 00000000..5dc0f08b
--- /dev/null
+++ b/klippy/extras/probe_eddy_current.py
@@ -0,0 +1,182 @@
+# Support for eddy current based Z probes
+#
+# Copyright (C) 2021-2024  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging, math, bisect
+import mcu
+from . import ldc1612, probe, manual_probe
+
+# Tool for calibrating the sensor Z detection and applying that calibration
+class EddyCalibration:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.name = config.get_name()
+        # Current calibration data
+        self.cal_freqs = []
+        self.cal_zpos = []
+        cal = config.get('calibrate', None)
+        if cal is not None:
+            cal = [list(map(float, d.strip().split(':', 1)))
+                   for d in cal.split(',')]
+            self.load_calibration(cal)
+        # Probe calibrate state
+        self.probe_speed = 0.
+        # Register commands
+        cname = self.name.split()[-1]
+        gcode = self.printer.lookup_object('gcode')
+        gcode.register_mux_command("PROBE_EDDY_CURRENT_CALIBRATE", "CHIP",
+                                   cname, self.cmd_EDDY_CALIBRATE,
+                                   desc=self.cmd_EDDY_CALIBRATE_help)
+    def load_calibration(self, cal):
+        cal = sorted([(c[1], c[0]) for c in cal])
+        self.cal_freqs = [c[0] for c in cal]
+        self.cal_zpos = [c[1] for c in cal]
+    def apply_calibration(self, samples):
+        for i, (samp_time, freq, dummy_z) in enumerate(samples):
+            pos = bisect.bisect(self.cal_freqs, freq)
+            if pos >= len(self.cal_zpos):
+                zpos = -99.9
+            elif pos == 0:
+                zpos = 99.9
+            else:
+                # XXX - optimize and avoid div by zero
+                this_freq = self.cal_freqs[pos]
+                prev_freq = self.cal_freqs[pos - 1]
+                this_zpos = self.cal_zpos[pos]
+                prev_zpos = self.cal_zpos[pos - 1]
+                gain = (this_zpos - prev_zpos) / (this_freq - prev_freq)
+                offset = prev_zpos - prev_freq * gain
+                zpos = freq * gain + offset
+            samples[i] = (samp_time, freq, round(zpos, 6))
+    def do_calibration_moves(self, move_speed):
+        toolhead = self.printer.lookup_object('toolhead')
+        kin = toolhead.get_kinematics()
+        move = toolhead.manual_move
+        # Start data collection
+        msgs = []
+        is_finished = False
+        def handle_batch(msg):
+            if is_finished:
+                return False
+            msgs.append(msg)
+            return True
+        self.printer.lookup_object(self.name).add_client(handle_batch)
+        toolhead.dwell(1.)
+        # Move to each 50um position
+        max_z = 4
+        samp_dist = 0.050
+        num_steps = int(max_z / samp_dist + .5) + 1
+        start_pos = toolhead.get_position()
+        times = []
+        for i in range(num_steps):
+            # Move to next position (always descending to reduce backlash)
+            hop_pos = list(start_pos)
+            hop_pos[2] += i * samp_dist + 0.500
+            move(hop_pos, move_speed)
+            next_pos = list(start_pos)
+            next_pos[2] += i * samp_dist
+            move(next_pos, move_speed)
+            # Note sample timing
+            start_query_time = toolhead.get_last_move_time() + 0.050
+            end_query_time = start_query_time + 0.100
+            toolhead.dwell(0.200)
+            # Find Z position based on actual commanded stepper position
+            toolhead.flush_step_generation()
+            kin_spos = {s.get_name(): s.get_commanded_position()
+                        for s in kin.get_steppers()}
+            kin_pos = kin.calc_position(kin_spos)
+            times.append((start_query_time, end_query_time, kin_pos[2]))
+        toolhead.dwell(1.0)
+        toolhead.wait_moves()
+        # Finish data collection
+        is_finished = True
+        # Correlate query responses
+        cal = {}
+        step = 0
+        for msg in msgs:
+            for query_time, freq, old_z in msg['data']:
+                # Add to step tracking
+                while step < len(times) and query_time > times[step][1]:
+                    step += 1
+                if step < len(times) and query_time >= times[step][0]:
+                    cal.setdefault(times[step][2], []).append(freq)
+        if len(cal) != len(times):
+            raise self.printer.command_error(
+                "Failed calibration - incomplete sensor data")
+        return cal
+    def calc_freqs(self, meas):
+        total_count = total_variance = 0
+        positions = {}
+        for pos, freqs in meas.items():
+            count = len(freqs)
+            freq_avg = float(sum(freqs)) / count
+            positions[pos] = freq_avg
+            total_count += count
+            total_variance += sum([(f - freq_avg)**2 for f in freqs])
+        return positions, math.sqrt(total_variance / total_count), total_count
+    def post_manual_probe(self, kin_pos):
+        if kin_pos is None:
+            # Manual Probe was aborted
+            return
+        curpos = list(kin_pos)
+        move = self.printer.lookup_object('toolhead').manual_move
+        # Move away from the bed
+        probe_calibrate_z = curpos[2]
+        curpos[2] += 5.
+        move(curpos, self.probe_speed)
+        # Move sensor over nozzle position
+        pprobe = self.printer.lookup_object("probe")
+        x_offset, y_offset, z_offset = pprobe.get_offsets()
+        curpos[0] -= x_offset
+        curpos[1] -= y_offset
+        move(curpos, self.probe_speed)
+        # Descend back to bed
+        curpos[2] -= 5. - 0.050
+        move(curpos, self.probe_speed)
+        # Perform calibration movement and capture
+        cal = self.do_calibration_moves(self.probe_speed)
+        # Calculate each sample position average and variance
+        positions, std, total = self.calc_freqs(cal)
+        last_freq = 0.
+        for pos, freq in reversed(sorted(positions.items())):
+            if freq <= last_freq:
+                raise self.printer.command_error(
+                    "Failed calibration - frequency not increasing each step")
+            last_freq = freq
+        gcode = self.printer.lookup_object("gcode")
+        gcode.respond_info(
+            "probe_eddy_current: stddev=%.3f in %d queries\n"
+            "The SAVE_CONFIG command will update the printer config file\n"
+            "and restart the printer." % (std, total))
+        # Save results
+        cal_contents = []
+        for i, (pos, freq) in enumerate(sorted(positions.items())):
+            if not i % 3:
+                cal_contents.append('\n')
+            cal_contents.append("%.6f:%.3f" % (pos - probe_calibrate_z, freq))
+            cal_contents.append(',')
+        cal_contents.pop()
+        configfile = self.printer.lookup_object('configfile')
+        configfile.set(self.name, 'calibrate', ''.join(cal_contents))
+    cmd_EDDY_CALIBRATE_help = "Calibrate eddy current probe"
+    def cmd_EDDY_CALIBRATE(self, gcmd):
+        self.probe_speed = gcmd.get_float("PROBE_SPEED", 5., above=0.)
+        # Start manual probe
+        manual_probe.ManualProbeHelper(self.printer, gcmd,
+                                       self.post_manual_probe)
+
+# Main "printer object"
+class PrinterEddyProbe:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.calibration = EddyCalibration(config)
+        # Sensor type
+        sensors = { "ldc1612": ldc1612.LDC1612 }
+        sensor_type = config.getchoice('sensor_type', {s: s for s in sensors})
+        self.sensor_helper = sensors[sensor_type](config, self.calibration)
+    def add_client(self, cb):
+        self.sensor_helper.add_client(cb)
+
+def load_config_prefix(config):
+    return PrinterEddyProbe(config)