diff --git a/klippy/extras/ldc1612.py b/klippy/extras/ldc1612.py index b6a871568..5c09940d8 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 000000000..5dc0f08bd --- /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 +# +# 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)