probe_eddy_current: Initial support for PROBE command

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

2 files changed, 125 insertions, 1 deletions

diff --git a/klippy/extras/ldc1612.py b/klippy/extras/ldc1612.py
index 5c09940d..16b8cd47 100644
--- a/klippy/extras/ldc1612.py
+++ b/klippy/extras/ldc1612.py
@@ -89,6 +89,7 @@ class LDC1612:
         self.mcu = mcu = self.i2c.get_mcu()
         self.oid = oid = mcu.create_oid()
         self.query_ldc1612_cmd = None
+        self.ldc1612_setup_home_cmd = self.query_ldc1612_home_state_cmd = None
         mcu.add_config_cmd("config_ldc1612 oid=%d i2c_oid=%d"
                            % (oid, self.i2c.get_oid()))
         mcu.add_config_cmd("query_ldc1612 oid=%d rest_ticks=0"
@@ -115,6 +116,13 @@ 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)
+        self.ldc1612_setup_home_cmd = self.mcu.lookup_command(
+            "ldc1612_setup_home oid=%c clock=%u threshold=%u"
+            " trsync_oid=%c trigger_reason=%c", cq=cmdqueue)
+        self.query_ldc1612_home_state_cmd = self.mcu.lookup_query_command(
+            "query_ldc1612_home_state oid=%c",
+            "ldc1612_home_state oid=%c homing=%c trigger_clock=%u",
+            oid=self.oid, cq=cmdqueue)
     def get_mcu(self):
         return self.i2c.get_mcu()
     def read_reg(self, reg):
@@ -126,6 +134,19 @@ class LDC1612:
                            minclock=minclock)
     def add_client(self, cb):
         self.batch_bulk.add_client(cb)
+    # Homing
+    def setup_home(self, print_time, trigger_freq, trsync_oid, reason):
+        clock = self.mcu.print_time_to_clock(print_time)
+        tfreq = int(trigger_freq * (1<<28) / float(LDC1612_FREQ) + 0.5)
+        self.ldc1612_setup_home_cmd.send(
+            [self.oid, clock, tfreq, trsync_oid, reason])
+    def clear_home(self):
+        self.ldc1612_setup_home_cmd.send([self.oid, 0, 0, 0, 0])
+        if self.mcu.is_fileoutput():
+            return 0.
+        params = self.query_ldc1612_home_state_cmd.send([self.oid])
+        tclock = self.mcu.clock32_to_clock64(params['trigger_clock'])
+        return self.mcu.clock_to_print_time(tclock)
     # Measurement decoding
     def _extract_samples(self, raw_samples):
         # Load variables to optimize inner loop below
diff --git a/klippy/extras/probe_eddy_current.py b/klippy/extras/probe_eddy_current.py
index 5dc0f08b..d140d3ed 100644
--- a/klippy/extras/probe_eddy_current.py
+++ b/klippy/extras/probe_eddy_current.py
@@ -28,6 +28,8 @@ class EddyCalibration:
         gcode.register_mux_command("PROBE_EDDY_CURRENT_CALIBRATE", "CHIP",
                                    cname, self.cmd_EDDY_CALIBRATE,
                                    desc=self.cmd_EDDY_CALIBRATE_help)
+    def is_calibrated(self):
+        return len(self.cal_freqs) > 2
     def load_calibration(self, cal):
         cal = sorted([(c[1], c[0]) for c in cal])
         self.cal_freqs = [c[0] for c in cal]
@@ -40,7 +42,7 @@ class EddyCalibration:
             elif pos == 0:
                 zpos = 99.9
             else:
-                # XXX - optimize and avoid div by zero
+                # XXX - could further 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]
@@ -49,6 +51,21 @@ class EddyCalibration:
                 offset = prev_zpos - prev_freq * gain
                 zpos = freq * gain + offset
             samples[i] = (samp_time, freq, round(zpos, 6))
+    def height_to_freq(self, height):
+        # XXX - could optimize lookup
+        rev_zpos = list(reversed(self.cal_zpos))
+        rev_freqs = list(reversed(self.cal_freqs))
+        pos = bisect.bisect(rev_zpos, height)
+        if pos == 0 or pos >= len(rev_zpos):
+            raise self.printer.command_error(
+                "Invalid probe_eddy_current height")
+        this_freq = rev_freqs[pos]
+        prev_freq = rev_freqs[pos - 1]
+        this_zpos = rev_zpos[pos]
+        prev_zpos = rev_zpos[pos - 1]
+        gain = (this_freq - prev_freq) / (this_zpos - prev_zpos)
+        offset = prev_freq - prev_zpos * gain
+        return height * gain + offset
     def do_calibration_moves(self, move_speed):
         toolhead = self.printer.lookup_object('toolhead')
         kin = toolhead.get_kinematics()
@@ -166,6 +183,88 @@ class EddyCalibration:
         manual_probe.ManualProbeHelper(self.printer, gcmd,
                                        self.post_manual_probe)
 
+# Helper for implementing PROBE style commands
+class EddyEndstopWrapper:
+    def __init__(self, config, sensor_helper, calibration):
+        self._printer = config.get_printer()
+        self._sensor_helper = sensor_helper
+        self._mcu = sensor_helper.get_mcu()
+        self._calibration = calibration
+        self._z_offset = config.getfloat('z_offset', minval=0.)
+        self._dispatch = mcu.TriggerDispatch(self._mcu)
+        self._samples = []
+        self._is_sampling = self._start_from_home = self._need_stop = False
+        self._printer.register_event_handler('klippy:mcu_identify',
+                                             self._handle_mcu_identify)
+    def _handle_mcu_identify(self):
+        kin = self._printer.lookup_object('toolhead').get_kinematics()
+        for stepper in kin.get_steppers():
+            if stepper.is_active_axis('z'):
+                self.add_stepper(stepper)
+    # Measurement gathering
+    def _start_measurements(self, is_home=False):
+        self._need_stop = False
+        if self._is_sampling:
+            return
+        self._is_sampling = True
+        self._is_from_home = is_home
+        self._sensor_helper.add_client(self._add_measurement)
+    def _stop_measurements(self, is_home=False):
+        if not self._is_sampling or (is_home and not self._start_from_home):
+            return
+        self._need_stop = True
+    def _add_measurement(self, msg):
+        if self._need_stop:
+            del self._samples[:]
+            self._is_sampling = self._need_stop = False
+            return False
+        self._samples.append(msg)
+        return True
+    # Interface for MCU_endstop
+    def get_mcu(self):
+        return self._mcu
+    def add_stepper(self, stepper):
+        self._dispatch.add_stepper(stepper)
+    def get_steppers(self):
+        return self._dispatch.get_steppers()
+    def home_start(self, print_time, sample_time, sample_count, rest_time,
+                   triggered=True):
+        self._start_measurements(is_home=True)
+        trigger_freq = self._calibration.height_to_freq(self._z_offset)
+        trigger_completion = self._dispatch.start(print_time)
+        self._sensor_helper.setup_home(
+            print_time, trigger_freq, self._dispatch.get_oid(),
+            mcu.MCU_trsync.REASON_ENDSTOP_HIT)
+        return trigger_completion
+    def home_wait(self, home_end_time):
+        self._dispatch.wait_end(home_end_time)
+        trigger_time = self._sensor_helper.clear_home()
+        self._stop_measurements(is_home=True)
+        res = self._dispatch.stop()
+        if res == mcu.MCU_trsync.REASON_COMMS_TIMEOUT:
+            return -1.
+        if res != mcu.MCU_trsync.REASON_ENDSTOP_HIT:
+            return 0.
+        if self._mcu.is_fileoutput():
+            return home_end_time
+        return trigger_time
+    def query_endstop(self, print_time):
+        return False # XXX
+    # Interface for ProbeEndstopWrapper
+    def multi_probe_begin(self):
+        if not self._calibration.is_calibrated():
+            raise self._printer.command_error(
+                "Must calibrate probe_eddy_current first")
+        self._start_measurements()
+    def multi_probe_end(self):
+        self._stop_measurements()
+    def probe_prepare(self, hmove):
+        pass
+    def probe_finish(self, hmove):
+        pass
+    def get_position_endstop(self):
+        return self._z_offset
+
 # Main "printer object"
 class PrinterEddyProbe:
     def __init__(self, config):
@@ -175,6 +274,10 @@ class PrinterEddyProbe:
         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)
+        # Probe interface
+        self.probe = EddyEndstopWrapper(config, self.sensor_helper,
+                                        self.calibration)
+        self.printer.add_object('probe', probe.PrinterProbe(config, self.probe))
     def add_client(self, cb):
         self.sensor_helper.add_client(cb)