load_cell: Load cell gram scale (#6729)

* Add gram scale features to load_cell
* Convert sensor counts to grams and make this available via unix socket and object status
* Basic GCodes for tearing and reading the load cell
* Guided Calibration
* Diagnostic gcode to check the health of the load cell
* Update load_cell Documentation
* Add API server load_cell/dump_force endpoint
* Update [load_cell] config with calibration fields
* Add G-Code commands for working with load cells
* Add status reference for load_cell objects

Signed-off-by: Gareth Farrington <gareth@waves.ky>

3 files changed, 498 insertions, 11 deletions

diff --git a/klippy/extras/ads1220.py b/klippy/extras/ads1220.py
index fcae78bc..75fcbfd2 100644
--- a/klippy/extras/ads1220.py
+++ b/klippy/extras/ads1220.py
@@ -95,10 +95,6 @@ class ADS1220:
         self.batch_bulk = bulk_sensor.BatchBulkHelper(
             self.printer, self._process_batch, self._start_measurements,
             self._finish_measurements, UPDATE_INTERVAL)
-        # publish raw samples to the socket
-        hdr = {'header': ('time', 'counts', 'value')}
-        self.batch_bulk.add_mux_endpoint("ads1220/dump_ads1220", "sensor",
-                                         self.name, hdr)
         # Command Configuration
         mcu.add_config_cmd(
             "config_ads1220 oid=%d spi_oid=%d data_ready_pin=%s"
diff --git a/klippy/extras/hx71x.py b/klippy/extras/hx71x.py
index 5c59ed0b..4ef90d6f 100644
--- a/klippy/extras/hx71x.py
+++ b/klippy/extras/hx71x.py
@@ -51,10 +51,6 @@ class HX71xBase:
         self.batch_bulk = bulk_sensor.BatchBulkHelper(
             self.printer, self._process_batch, self._start_measurements,
             self._finish_measurements, UPDATE_INTERVAL)
-        # publish raw samples to the socket
-        dump_path = "%s/dump_%s" % (sensor_type, sensor_type)
-        hdr = {'header': ('time', 'counts', 'value')}
-        self.batch_bulk.add_mux_endpoint(dump_path, "sensor", self.name, hdr)
         # Command Configuration
         self.query_hx71x_cmd = None
         mcu.add_config_cmd(
diff --git a/klippy/extras/load_cell.py b/klippy/extras/load_cell.py
index 14f3c298..4117a92c 100644
--- a/klippy/extras/load_cell.py
+++ b/klippy/extras/load_cell.py
@@ -3,21 +3,516 @@
 # Copyright (C) 2024 Gareth Farrington <gareth@waves.ky>
 #
 # This file may be distributed under the terms of the GNU GPLv3 license.
+
 from . import hx71x
 from . import ads1220
+from .bulk_sensor import BatchWebhooksClient
+import collections, itertools
+# We want either Python 3's zip() or Python 2's izip() but NOT 2's zip():
+zip_impl = zip
+try:
+    from itertools import izip as zip_impl # python 2.x izip
+except ImportError: # will be Python 3.x
+    pass
+
+# alternative to numpy's column selection:
+def select_column(data, column_idx):
+    return list(zip_impl(*data))[column_idx]
+
+def avg(data):
+    return sum(data) / len(data)
+
+# Helper for event driven webhooks and subscription based API clients
+class ApiClientHelper(object):
+    def __init__(self, printer):
+        self.printer = printer
+        self.client_cbs = []
+        self.webhooks_start_resp = {}
+
+    # send data to clients
+    def send(self, msg):
+        for client_cb in list(self.client_cbs):
+            res = client_cb(msg)
+            if not res:
+                # This client no longer needs updates - unregister it
+                self.client_cbs.remove(client_cb)
+
+    # Add a client that gets data callbacks
+    def add_client(self, client_cb):
+        self.client_cbs.append(client_cb)
+
+    # Add Webhooks client and send header
+    def _add_webhooks_client(self, web_request):
+        whbatch = BatchWebhooksClient(web_request)
+        self.add_client(whbatch.handle_batch)
+        web_request.send(self.webhooks_start_resp)
+
+    # Set up a webhooks endpoint with a static header
+    def add_mux_endpoint(self, path, key, value, webhooks_start_resp):
+        self.webhooks_start_resp = webhooks_start_resp
+        wh = self.printer.lookup_object('webhooks')
+        wh.register_mux_endpoint(path, key, value, self._add_webhooks_client)
+
+# Class for handling commands related ot load cells
+class LoadCellCommandHelper:
+    def __init__(self, config, load_cell):
+        self.printer = config.get_printer()
+        self.load_cell = load_cell
+        name_parts = config.get_name().split()
+        self.name = name_parts[-1]
+        self.register_commands(self.name)
+        if len(name_parts) == 1:
+            self.register_commands(None)
+
+    def register_commands(self, name):
+        # Register commands
+        gcode = self.printer.lookup_object('gcode')
+        gcode.register_mux_command("LOAD_CELL_TARE", "LOAD_CELL", name,
+                                   self.cmd_LOAD_CELL_TARE,
+                                   desc=self.cmd_LOAD_CELL_TARE_help)
+        gcode.register_mux_command("LOAD_CELL_CALIBRATE", "LOAD_CELL", name,
+                                   self.cmd_LOAD_CELL_CALIBRATE,
+                                   desc=self.cmd_CALIBRATE_LOAD_CELL_help)
+        gcode.register_mux_command("LOAD_CELL_READ", "LOAD_CELL", name,
+                                   self.cmd_LOAD_CELL_READ,
+                                   desc=self.cmd_LOAD_CELL_READ_help)
+        gcode.register_mux_command("LOAD_CELL_DIAGNOSTIC", "LOAD_CELL", name,
+                                   self.cmd_LOAD_CELL_DIAGNOSTIC,
+                                   desc=self.cmd_LOAD_CELL_DIAGNOSTIC_help)
+
+    cmd_LOAD_CELL_TARE_help = "Set the Zero point of the load cell"
+    def cmd_LOAD_CELL_TARE(self, gcmd):
+        tare_counts = self.load_cell.avg_counts()
+        self.load_cell.tare(tare_counts)
+        tare_percent = self.load_cell.counts_to_percent(tare_counts)
+        gcmd.respond_info("Load cell tare value: %.2f%% (%i)"
+                          % (tare_percent, tare_counts))
+
+    cmd_CALIBRATE_LOAD_CELL_help = "Start interactive calibration tool"
+    def cmd_LOAD_CELL_CALIBRATE(self, gcmd):
+        LoadCellGuidedCalibrationHelper(self.printer, self.load_cell)
+
+    cmd_LOAD_CELL_READ_help = "Take a reading from the load cell"
+    def cmd_LOAD_CELL_READ(self, gcmd):
+        counts = self.load_cell.avg_counts()
+        percent = self.load_cell.counts_to_percent(counts)
+        force = self.load_cell.counts_to_grams(counts)
+        if percent >= 100 or percent <= -100:
+            gcmd.respond_info("Err (%.2f%%)" % (percent,))
+        if force is None:
+            gcmd.respond_info("---.-g (%.2f%%)" % (percent,))
+        else:
+            gcmd.respond_info("%.1fg (%.2f%%)" % (force, percent))
+
+    cmd_LOAD_CELL_DIAGNOSTIC_help = "Check the health of the load cell"
+    def cmd_LOAD_CELL_DIAGNOSTIC(self, gcmd):
+        gcmd.respond_info("Collecting load cell data for 10 seconds...")
+        collector = self.load_cell.get_collector()
+        reactor = self.printer.get_reactor()
+        collector.start_collecting()
+        reactor.pause(reactor.monotonic() + 10.)
+        samples, errors = collector.stop_collecting()
+        if errors:
+            gcmd.respond_info("Sensor reported errors: %i errors,"
+                              " %i overflows" % (errors[0], errors[1]))
+        else:
+            gcmd.respond_info("Sensor reported no errors")
+        if not samples:
+            raise gcmd.error("No samples returned from sensor!")
+        counts = select_column(samples, 2)
+        range_min, range_max = self.load_cell.saturation_range()
+        good_count = 0
+        saturation_count = 0
+        for sample in counts:
+            if sample >= range_max or sample <= range_min:
+                saturation_count += 1
+            else:
+                good_count += 1
+        gcmd.respond_info("Samples Collected: %i" % (len(samples)))
+        if len(samples) > 2:
+            sensor_sps = self.load_cell.sensor.get_samples_per_second()
+            sps = float(len(samples)) / (samples[-1][0] - samples[0][0])
+            gcmd.respond_info("Measured samples per second: %.1f, "
+                              "configured: %.1f" % (sps, sensor_sps))
+        gcmd.respond_info("Good samples: %i, Saturated samples: %i, Unique"
+                          " values: %i" % (good_count, saturation_count,
+                          len(set(counts))))
+        max_pct = self.load_cell.counts_to_percent(max(counts))
+        min_pct = self.load_cell.counts_to_percent(min(counts))
+        gcmd.respond_info("Sample range: [%.2f%% to %.2f%%]"
+                          % (min_pct, max_pct))
+        gcmd.respond_info("Sample range / sensor capacity: %.5f%%"
+                          % ((max_pct - min_pct) / 2.))
+
+# Class to guide the user through calibrating a load cell
+class LoadCellGuidedCalibrationHelper:
+    def __init__(self, printer, load_cell):
+        self.printer = printer
+        self.gcode = printer.lookup_object('gcode')
+        self.load_cell = load_cell
+        self._tare_counts = self._counts_per_gram = None
+        self.tare_percent = 0.
+        self.register_commands()
+        self.gcode.respond_info(
+            "Starting load cell calibration. \n"
+            "1.) Remove all load and run TARE. \n"
+            "2.) Apply a known load, run CALIBRATE GRAMS=nnn. \n"
+            "Complete calibration with the ACCEPT command.\n"
+            "Use the ABORT command to quit.")
+
+    def verify_no_active_calibration(self,):
+        try:
+            self.gcode.register_command('TARE', 'dummy')
+        except self.printer.config_error as e:
+            raise self.gcode.error(
+                "Already Calibrating a Load Cell. Use ABORT to quit.")
+        self.gcode.register_command('TARE', None)
+
+    def register_commands(self):
+        self.verify_no_active_calibration()
+        register_command = self.gcode.register_command
+        register_command("ABORT", self.cmd_ABORT, desc=self.cmd_ABORT_help)
+        register_command("ACCEPT", self.cmd_ACCEPT, desc=self.cmd_ACCEPT_help)
+        register_command("TARE", self.cmd_TARE, desc=self.cmd_TARE_help)
+        register_command("CALIBRATE", self.cmd_CALIBRATE,
+                         desc=self.cmd_CALIBRATE_help)
+
+    # convert the delta of counts to a counts/gram metric
+    def counts_per_gram(self, grams, cal_counts):
+        return float(abs(int(self._tare_counts - cal_counts))) / grams
+
+    # calculate max force that the load cell can register
+    # given tare bias, at saturation in kilograms
+    def capacity_kg(self, counts_per_gram):
+        range_min, range_max = self.load_cell.saturation_range()
+        return (int((range_max - abs(self._tare_counts)) / counts_per_gram)
+                / 1000.)
+
+    def finalize(self, save_results=False):
+        for name in ['ABORT', 'ACCEPT', 'TARE', 'CALIBRATE']:
+            self.gcode.register_command(name, None)
+        if not save_results:
+            self.gcode.respond_info("Load cell calibration aborted")
+            return
+        if self._counts_per_gram is None or self._tare_counts is None:
+            self.gcode.respond_info("Calibration process is incomplete, "
+                                    "aborting")
+        self.load_cell.set_calibration(self._counts_per_gram, self._tare_counts)
+        self.gcode.respond_info("Load cell calibration settings:\n\n"
+            "counts_per_gram: %.6f\n"
+            "reference_tare_counts: %i\n\n"
+            "The SAVE_CONFIG command will update the printer config file"
+            " with the above and restart the printer."
+            % (self._counts_per_gram, self._tare_counts))
+        self.load_cell.tare(self._tare_counts)
+
+    cmd_ABORT_help = "Abort load cell calibration tool"
+    def cmd_ABORT(self, gcmd):
+        self.finalize(False)
+
+    cmd_ACCEPT_help = "Accept calibration results and apply to load cell"
+    def cmd_ACCEPT(self, gcmd):
+        self.finalize(True)
+
+    cmd_TARE_help = "Tare the load cell"
+    def cmd_TARE(self, gcmd):
+        self._tare_counts = self.load_cell.avg_counts()
+        self._counts_per_gram = None  # require re-calibration on tare
+        self.tare_percent = self.load_cell.counts_to_percent(self._tare_counts)
+        gcmd.respond_info("Load cell tare value: %.2f%% (%i)"
+                          % (self.tare_percent, self._tare_counts))
+        if self.tare_percent > 2.:
+            gcmd.respond_info(
+                "WARNING: tare value is more than 2% away from 0!\n"
+                "The load cell's range will be impacted.\n"
+                "Check for external force on the load cell.")
+        gcmd.respond_info("Now apply a known force to the load cell and enter \
+                         the force value with:\n CALIBRATE GRAMS=nnn")
+
+    cmd_CALIBRATE_help = "Enter the load cell value in grams"
+    def cmd_CALIBRATE(self, gcmd):
+        if self._tare_counts is None:
+            gcmd.respond_info("You must use TARE first.")
+            return
+        grams = gcmd.get_float("GRAMS", minval=50., maxval=25000.)
+        cal_counts = self.load_cell.avg_counts()
+        cal_percent = self.load_cell.counts_to_percent(cal_counts)
+        c_per_g = self.counts_per_gram(grams, cal_counts)
+        cap_kg = self.capacity_kg(c_per_g)
+        gcmd.respond_info("Calibration value: %.2f%% (%i), Counts/gram: %.5f, \
+            Total capacity: +/- %0.2fKg"
+                % (cal_percent, cal_counts, c_per_g, cap_kg))
+        range_min, range_max = self.load_cell.saturation_range()
+        if cal_counts >= range_max or cal_counts <= range_min:
+            raise self.printer.command_error(
+                "ERROR: Sensor is saturated with too much load!\n"
+                "Use less force to calibrate the load cell.")
+        if cal_counts == self._tare_counts:
+            raise self.printer.command_error(
+                "ERROR: Tare and Calibration readings are the same!\n"
+                "Check wiring and validate sensor with READ_LOAD_CELL command.")
+        if (abs(cal_percent - self.tare_percent)) < 1.:
+            raise self.printer.command_error(
+                "ERROR: Tare and Calibration readings are less than 1% "
+                "different!\n"
+                "Use more force when calibrating or a higher sensor gain.")
+        # only set _counts_per_gram after all errors are raised
+        self._counts_per_gram = c_per_g
+        if cap_kg < 1.:
+            gcmd.respond_info("WARNING: Load cell capacity is less than 1kg!\n"
+                "Check wiring and consider using a lower sensor gain.")
+        if cap_kg > 25.:
+            gcmd.respond_info("WARNING: Load cell capacity is more than 25Kg!\n"
+                "Check wiring and consider using a higher sensor gain.")
+        gcmd.respond_info("Accept calibration with the ACCEPT command.")
+
+
+# Utility to collect some samples from the LoadCell for later analysis
+# Optionally blocks execution while collecting with reactor.pause()
+# can collect a minimum n samples or collect until a specific print_time
+# samples returned in [[time],[force],[counts]] arrays for easy processing
+RETRY_DELAY = 0.05  # 20Hz
+class LoadCellSampleCollector:
+    def __init__(self, printer, load_cell):
+        self._printer = printer
+        self._load_cell = load_cell
+        self._reactor = printer.get_reactor()
+        self._mcu = load_cell.sensor.get_mcu()
+        self.min_time = 0.
+        self.max_time = float("inf")
+        self.min_count = float("inf")  # In Python 3.5 math.inf is better
+        self.is_started = False
+        self._samples = []
+        self._errors = 0
+        self._overflows = 0
 
-# Printer class that controls a load cell
+    def _on_samples(self, msg):
+        if not self.is_started:
+            return False  # already stopped, ignore
+        self._errors += msg['errors']
+        self._overflows += msg['overflows']
+        samples = msg['data']
+        for sample in samples:
+            time = sample[0]
+            if self.min_time <= time <= self.max_time:
+                self._samples.append(sample)
+            if time > self.max_time:
+                self.is_started = False
+        if len(self._samples) >= self.min_count:
+            self.is_started = False
+        return self.is_started
+
+    def _finish_collecting(self):
+        self.is_started = False
+        self.min_time = 0.
+        self.max_time = float("inf")
+        self.min_count = float("inf")  # In Python 3.5 math.inf is better
+        samples = self._samples
+        self._samples = []
+        errors = self._errors
+        self._errors = 0
+        overflows = self._overflows
+        self._overflows = 0
+        return samples, (errors, overflows) if errors or overflows else 0
+
+    def _collect_until(self, timeout):
+        self.start_collecting()
+        while self.is_started:
+            now = self._reactor.monotonic()
+            if self._mcu.estimated_print_time(now) > timeout:
+                self._finish_collecting()
+                raise self._printer.command_error(
+                    "LoadCellSampleCollector timed out! Errors: %i,"
+                    " Overflows: %i" % (self._errors, self._overflows))
+            self._reactor.pause(now + RETRY_DELAY)
+        return self._finish_collecting()
+
+    # start collecting with no automatic end to collection
+    def start_collecting(self, min_time=None):
+        if self.is_started:
+            return
+        self.min_time = min_time if min_time is not None else self.min_time
+        self.is_started = True
+        self._load_cell.add_client(self._on_samples)
+
+    # stop collecting immediately and return results
+    def stop_collecting(self):
+        return self._finish_collecting()
+
+    # block execution until at least min_count samples are collected
+    # will return all samples collected, not just up to min_count
+    def collect_min(self, min_count=1):
+        self.min_count = min_count
+        if len(self._samples) >= min_count:
+            return self._finish_collecting()
+        print_time = self._mcu.estimated_print_time(self._reactor.monotonic())
+        start_time = max(print_time, self.min_time)
+        sps = self._load_cell.sensor.get_samples_per_second()
+        return self._collect_until(start_time + 1. + (min_count / sps))
+
+    # returns when a sample is collected with a timestamp after print_time
+    def collect_until(self, print_time=None):
+        self.max_time = print_time
+        if len(self._samples) and self._samples[-1][0] >= print_time:
+            return self._finish_collecting()
+        return self._collect_until(self.max_time + 1.)
+
+# Printer class that controls the load cell
+MIN_COUNTS_PER_GRAM = 1.
 class LoadCell:
     def __init__(self, config, sensor):
         self.printer = printer = config.get_printer()
-        self.sensor = sensor   # must implement BulkAdcSensor
+        self.config_name = config.get_name()
+        self.name = config.get_name().split()[-1]
+        self.sensor = sensor   # must implement BulkSensorAdc
+        buffer_size = sensor.get_samples_per_second() // 2
+        self._force_buffer = collections.deque(maxlen=buffer_size)
+        self.reference_tare_counts = config.getint('reference_tare_counts',
+                                                   default=None)
+        self.tare_counts = self.reference_tare_counts
+        self.counts_per_gram = config.getfloat('counts_per_gram',
+                                   minval=MIN_COUNTS_PER_GRAM, default=None)
+        self.invert = config.getchoice('sensor_orientation',
+                        {'normal': 1., 'inverted': -1.}, default="normal")
+        LoadCellCommandHelper(config, self)
+        # Client support:
+        self.clients = ApiClientHelper(printer)
+        header = {"header": ["time", "force (g)", "counts", "tare_counts"]}
+        self.clients.add_mux_endpoint("load_cell/dump_force",
+                                      "load_cell", self.name, header)
+        # startup, when klippy is ready, start capturing data
+        printer.register_event_handler("klippy:ready", self._handle_ready)
+
+    def _handle_ready(self):
+        self.sensor.add_client(self._sensor_data_event)
+        self.add_client(self._track_force)
+        # announce calibration status on ready
+        if self.is_calibrated():
+            self.printer.send_event("load_cell:calibrate", self)
+        if self.is_tared():
+            self.printer.send_event("load_cell:tare", self)
+
+    # convert raw counts to grams and broadcast to clients
+    def _sensor_data_event(self, msg):
+        data = msg.get("data")
+        errors = msg.get("errors")
+        overflows = msg.get("overflows")
+        if data is None:
+            return None
+        samples = []
+        for row in data:
+            # [time, grams, counts, tare_counts]
+            samples.append([row[0], self.counts_to_grams(row[1]), row[1],
+                            self.tare_counts])
+        msg = {'data': samples, 'errors': errors, 'overflows': overflows}
+        self.clients.send(msg)
+        return True
+
+    # get internal events of force data
+    def add_client(self, callback):
+        self.clients.add_client(callback)
+
+    def tare(self, tare_counts):
+        self.tare_counts = int(tare_counts)
+        self.printer.send_event("load_cell:tare", self)
+
+    def set_calibration(self, counts_per_gram, tare_counts):
+        if (counts_per_gram is None
+                or abs(counts_per_gram) < MIN_COUNTS_PER_GRAM):
+            raise self.printer.command_error("Invalid counts per gram value")
+        if tare_counts is None:
+            raise self.printer.command_error("Missing tare counts")
+        self.counts_per_gram = counts_per_gram
+        self.reference_tare_counts = int(tare_counts)
+        configfile = self.printer.lookup_object('configfile')
+        configfile.set(self.config_name, 'counts_per_gram',
+                       "%.5f" % (self.counts_per_gram,))
+        configfile.set(self.config_name, 'reference_tare_counts',
+                       "%i" % (self.reference_tare_counts,))
+        self.printer.send_event("load_cell:calibrate", self)
 
-    def _on_sample(self, msg):
+    def counts_to_grams(self, sample):
+        if not self.is_calibrated() or not self.is_tared():
+            return None
+        sample_delta = float(sample - self.tare_counts)
+        return self.invert * (sample_delta / self.counts_per_gram)
+
+    # The maximum range of the sensor based on its bit width
+    def saturation_range(self):
+        return self.sensor.get_range()
+
+    # convert raw counts to a +/- percentage of the sensors range
+    def counts_to_percent(self, counts):
+        range_min, range_max = self.saturation_range()
+        return (float(counts) / float(range_max)) * 100.
+
+    # read 1 second of load cell data and average it
+    # performs safety checks for saturation
+    def avg_counts(self, num_samples=None):
+        if num_samples is None:
+            num_samples = self.sensor.get_samples_per_second()
+        samples, errors = self.get_collector().collect_min(num_samples)
+        if errors:
+            raise self.printer.command_error(
+                "Sensor reported %i errors while sampling"
+                    % (errors[0] + errors[1]))
+        # check samples for saturated readings
+        range_min, range_max = self.saturation_range()
+        for sample in samples:
+            if sample[2] >= range_max or sample[2] <= range_min:
+                raise self.printer.command_error(
+                    "Some samples are saturated (+/-100%)")
+        return avg(select_column(samples, 2))
+
+    # Provide ongoing force tracking/averaging for status updates
+    def _track_force(self, msg):
+        if not (self.is_calibrated() and self.is_tared()):
+            return True
+        samples = msg['data']
+        # selectColumn unusable here because Python 2 lacks deque.extend
+        for sample in samples:
+            self._force_buffer.append(sample[1])
         return True
 
+    def _force_g(self):
+        if (self.is_calibrated() and self.is_tared()
+                and len(self._force_buffer) > 0):
+            return {"force_g": round(avg(self._force_buffer), 1),
+                    "min_force_g": round(min(self._force_buffer), 1),
+                    "max_force_g": round(max(self._force_buffer), 1)}
+        return {}
+
+    def is_tared(self):
+        return self.tare_counts is not None
+
+    def is_calibrated(self):
+        return (self.counts_per_gram is not None
+                and self.reference_tare_counts is not None)
+
     def get_sensor(self):
         return self.sensor
 
+    def get_reference_tare_counts(self):
+        return self.reference_tare_counts
+
+    def get_tare_counts(self):
+        return self.tare_counts
+
+    def get_counts_per_gram(self):
+        return self.counts_per_gram
+
+    def get_collector(self):
+        return LoadCellSampleCollector(self.printer, self)
+
+    def get_status(self, eventtime):
+        status = self._force_g()
+        status.update({'is_calibrated': self.is_calibrated(),
+                       'counts_per_gram': self.counts_per_gram,
+                       'reference_tare_counts': self.reference_tare_counts,
+                       'tare_counts': self.tare_counts})
+        return status
+
+
 def load_config(config):
     # Sensor types
     sensors = {}