sensor_bulk: New C file with helper code for sending bulk sensor meas…

…urements

Refactor the low-level "bulk sensor" management code in the mcu.  This
updates the sensor_adxl345.c, sensor_mpu9250.c, sensor_lis2dw.c, and
sensor_angle.c code to use the same "bulk sensor" messages.  All of
these sensors will now send "sensor_bulk_data" and
"sensor_bulk_status" messages.

Signed-off-by: Kevin O'Connor <[email protected]>

klippy/extras/adxl345.py

@@ -187,7 +187,7 @@ def read_axes_map(config):
 MIN_MSG_TIME = 0.100
 
 BYTES_PER_SAMPLE = 5
-SAMPLES_PER_BLOCK = 10
+SAMPLES_PER_BLOCK = bulk_sensor.MAX_BULK_MSG_SIZE // BYTES_PER_SAMPLE
 
 BATCH_UPDATES = 0.100
 
@@ -205,13 +205,12 @@ def __init__(self, config):
         self.mcu = mcu = self.spi.get_mcu()
         self.oid = oid = mcu.create_oid()
         self.query_adxl345_cmd = None
-        self.query_adxl345_status_cmd = None
         mcu.add_config_cmd("config_adxl345 oid=%d spi_oid=%d"
                            % (oid, self.spi.get_oid()))
         mcu.add_config_cmd("query_adxl345 oid=%d clock=0 rest_ticks=0"
                            % (oid,), on_restart=True)
         mcu.register_config_callback(self._build_config)
-        self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "adxl345_data", oid)
+        self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, oid=oid)
         # Clock tracking
         chip_smooth = self.data_rate * BATCH_UPDATES * 2
         self.clock_sync = bulk_sensor.ClockSyncRegression(mcu, chip_smooth)
@@ -230,10 +229,8 @@ def _build_config(self):
         cmdqueue = self.spi.get_command_queue()
         self.query_adxl345_cmd = self.mcu.lookup_command(
             "query_adxl345 oid=%c clock=%u rest_ticks=%u", cq=cmdqueue)
-        self.query_adxl345_status_cmd = self.mcu.lookup_query_command(
-            "query_adxl345_status oid=%c",
-            "adxl345_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
-            " buffered=%c fifo=%c limit_count=%hu", oid=self.oid, cq=cmdqueue)
+        self.clock_updater.setup_query_command(
+            self.mcu, "query_adxl345_status oid=%c", oid=self.oid, cq=cmdqueue)
     def read_reg(self, reg):
         params = self.spi.spi_transfer([reg | REG_MOD_READ, 0x00])
         response = bytearray(params['response'])
@@ -286,17 +283,6 @@ def _extract_samples(self, raw_samples):
         self.clock_sync.set_last_chip_clock(seq * SAMPLES_PER_BLOCK + i)
         del samples[count:]
         return samples
-    def _update_clock(self, minclock=0):
-        # Query current state
-        for retry in range(5):
-            params = self.query_adxl345_status_cmd.send([self.oid],
-                                                        minclock=minclock)
-            fifo = params['fifo'] & 0x7f
-            if fifo <= 32:
-                break
-        else:
-            raise self.printer.command_error("Unable to query adxl345 fifo")
-        self.clock_updater.update_clock(params)
     # Start, stop, and process message batches
     def _start_measurements(self):
         # In case of miswiring, testing ADXL345 device ID prevents treating
@@ -325,24 +311,22 @@ def _start_measurements(self):
         logging.info("ADXL345 starting '%s' measurements", self.name)
         # Initialize clock tracking
         self.clock_updater.note_start(reqclock)
-        self._update_clock(minclock=reqclock)
-        self.clock_updater.clear_duration_filter()
         self.last_error_count = 0
     def _finish_measurements(self):
         # Halt bulk reading
         self.query_adxl345_cmd.send_wait_ack([self.oid, 0, 0])
         self.bulk_queue.clear_samples()
         logging.info("ADXL345 finished '%s' measurements", self.name)
     def _process_batch(self, eventtime):
-        self._update_clock()
+        self.clock_updater.update_clock()
         raw_samples = self.bulk_queue.pull_samples()
         if not raw_samples:
             return {}
         samples = self._extract_samples(raw_samples)
         if not samples:
             return {}
         return {'data': samples, 'errors': self.last_error_count,
-                'overflows': self.clock_updater.get_last_limit_count()}
+                'overflows': self.clock_updater.get_last_overflows()}
 
 def load_config(config):
     return ADXL345(config)

klippy/extras/angle.py

@@ -412,7 +412,7 @@ def cmd_ANGLE_DEBUG_WRITE(self, gcmd):
         self._write_reg(reg, val)
 
 BYTES_PER_SAMPLE = 3
-SAMPLES_PER_BLOCK = 16
+SAMPLES_PER_BLOCK = bulk_sensor.MAX_BULK_MSG_SIZE // BYTES_PER_SAMPLE
 
 SAMPLE_PERIOD = 0.000400
 BATCH_UPDATES = 0.100
@@ -445,7 +445,7 @@ def __init__(self, config):
             "query_spi_angle oid=%d clock=0 rest_ticks=0 time_shift=0"
             % (oid,), on_restart=True)
         mcu.register_config_callback(self._build_config)
-        self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "spi_angle_data", oid)
+        self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, oid=oid)
         # Process messages in batches
         self.batch_bulk = bulk_sensor.BatchBulkHelper(
             self.printer, self._process_batch,

klippy/extras/bulk_sensor.py

@@ -114,7 +114,7 @@ def handle_batch(self, msg):
 
 # Helper class to store incoming messages in a queue
 class BulkDataQueue:
-    def __init__(self, mcu, msg_name, oid):
+    def __init__(self, mcu, msg_name="sensor_bulk_data", oid=None):
         # Measurement storage (accessed from background thread)
         self.lock = threading.Lock()
         self.raw_samples = []
@@ -206,31 +206,37 @@ def __init__(self, clock_sync, bytes_per_sample):
         self.clock_sync = clock_sync
         self.bytes_per_sample = bytes_per_sample
         self.samples_per_block = MAX_BULK_MSG_SIZE // bytes_per_sample
-        self.mcu = clock_sync.mcu
         self.last_sequence = self.max_query_duration = 0
-        self.last_limit_count = 0
+        self.last_overflows = 0
+        self.mcu = self.oid = self.query_status_cmd = None
+    def setup_query_command(self, mcu, msgformat, oid, cq):
+        self.mcu = mcu
+        self.oid = oid
+        self.query_status_cmd = self.mcu.lookup_query_command(
+            msgformat, "sensor_bulk_status oid=%c clock=%u query_ticks=%u"
+            " next_sequence=%hu buffered=%u possible_overflows=%hu",
+            oid=oid, cq=cq)
     def get_last_sequence(self):
         return self.last_sequence
-    def get_last_limit_count(self):
-        return self.last_limit_count
+    def get_last_overflows(self):
+        return self.last_overflows
     def clear_duration_filter(self):
         self.max_query_duration = 1 << 31
     def note_start(self, reqclock):
         self.last_sequence = 0
-        self.last_limit_count = 0
+        self.last_overflows = 0
         self.clock_sync.reset(reqclock, 0)
         self.clear_duration_filter()
-    def update_clock(self, params):
-        # Handle a status response message of the form:
-        #   adxl345_status oid=x clock=x query_ticks=x next_sequence=x
-        #     buffered=x fifo=x limit_count=x
-        fifo = params['fifo']
+        self.update_clock(minclock=reqclock)
+        self.clear_duration_filter()
+    def update_clock(self, minclock=0):
+        params = self.query_status_cmd.send([self.oid], minclock=minclock)
         mcu_clock = self.mcu.clock32_to_clock64(params['clock'])
         seq_diff = (params['next_sequence'] - self.last_sequence) & 0xffff
         self.last_sequence += seq_diff
         buffered = params['buffered']
-        lc_diff = (params['limit_count'] - self.last_limit_count) & 0xffff
-        self.last_limit_count += lc_diff
+        po_diff = (params['possible_overflows'] - self.last_overflows) & 0xffff
+        self.last_overflows += po_diff
         duration = params['query_ticks']
         if duration > self.max_query_duration:
             # Skip measurement as a high query time could skew clock tracking
@@ -239,7 +245,7 @@ def update_clock(self, params):
             return
         self.max_query_duration = 2 * duration
         msg_count = (self.last_sequence * self.samples_per_block
-                     + buffered // self.bytes_per_sample + fifo)
+                     + buffered // self.bytes_per_sample)
         # The "chip clock" is the message counter plus .5 for average
         # inaccuracy of query responses and plus .5 for assumed offset
         # of hardware processing time.

klippy/extras/lis2dw.py

@@ -33,7 +33,7 @@
 MIN_MSG_TIME = 0.100
 
 BYTES_PER_SAMPLE = 6
-SAMPLES_PER_BLOCK = 8
+SAMPLES_PER_BLOCK = bulk_sensor.MAX_BULK_MSG_SIZE // BYTES_PER_SAMPLE
 
 BATCH_UPDATES = 0.100
 
@@ -49,13 +49,12 @@ def __init__(self, config):
         self.mcu = mcu = self.spi.get_mcu()
         self.oid = oid = mcu.create_oid()
         self.query_lis2dw_cmd = None
-        self.query_lis2dw_status_cmd = None
         mcu.add_config_cmd("config_lis2dw oid=%d spi_oid=%d"
                            % (oid, self.spi.get_oid()))
         mcu.add_config_cmd("query_lis2dw oid=%d clock=0 rest_ticks=0"
                            % (oid,), on_restart=True)
         mcu.register_config_callback(self._build_config)
-        self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "lis2dw_data", oid)
+        self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, oid=oid)
         # Clock tracking
         chip_smooth = self.data_rate * BATCH_UPDATES * 2
         self.clock_sync = bulk_sensor.ClockSyncRegression(mcu, chip_smooth)
@@ -75,10 +74,8 @@ def _build_config(self):
         cmdqueue = self.spi.get_command_queue()
         self.query_lis2dw_cmd = self.mcu.lookup_command(
             "query_lis2dw oid=%c clock=%u rest_ticks=%u", cq=cmdqueue)
-        self.query_lis2dw_status_cmd = self.mcu.lookup_query_command(
-            "query_lis2dw_status oid=%c",
-            "lis2dw_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
-            " buffered=%c fifo=%c limit_count=%hu", oid=self.oid, cq=cmdqueue)
+        self.clock_updater.setup_query_command(
+            self.mcu, "query_lis2dw_status oid=%c", oid=self.oid, cq=cmdqueue)
     def read_reg(self, reg):
         params = self.spi.spi_transfer([reg | REG_MOD_READ, 0x00])
         response = bytearray(params['response'])
@@ -133,10 +130,6 @@ def _extract_samples(self, raw_samples):
         self.clock_sync.set_last_chip_clock(seq * SAMPLES_PER_BLOCK + i)
         del samples[count:]
         return samples
-    def _update_clock(self, minclock=0):
-        params = self.query_lis2dw_status_cmd.send([self.oid],
-                                                   minclock=minclock)
-        self.clock_updater.update_clock(params)
     # Start, stop, and process message batches
     def _start_measurements(self):
         # In case of miswiring, testing LIS2DW device ID prevents treating
@@ -170,8 +163,6 @@ def _start_measurements(self):
         logging.info("LIS2DW starting '%s' measurements", self.name)
         # Initialize clock tracking
         self.clock_updater.note_start(reqclock)
-        self._update_clock(minclock=reqclock)
-        self.clock_updater.clear_duration_filter()
         self.last_error_count = 0
     def _finish_measurements(self):
         # Halt bulk reading
@@ -180,15 +171,15 @@ def _finish_measurements(self):
         logging.info("LIS2DW finished '%s' measurements", self.name)
         self.set_reg(REG_LIS2DW_FIFO_CTRL, 0x00)
     def _process_batch(self, eventtime):
-        self._update_clock()
+        self.clock_updater.update_clock()
         raw_samples = self.bulk_queue.pull_samples()
         if not raw_samples:
             return {}
         samples = self._extract_samples(raw_samples)
         if not samples:
             return {}
         return {'data': samples, 'errors': self.last_error_count,
-                'overflows': self.clock_updater.get_last_limit_count()}
+                'overflows': self.clock_updater.get_last_overflows()}
 
 def load_config(config):
     return LIS2DW(config)

klippy/extras/mpu9250.py

@@ -50,7 +50,7 @@
 MIN_MSG_TIME = 0.100
 
 BYTES_PER_SAMPLE = 6
-SAMPLES_PER_BLOCK = 8
+SAMPLES_PER_BLOCK = bulk_sensor.MAX_BULK_MSG_SIZE // BYTES_PER_SAMPLE
 
 BATCH_UPDATES = 0.100
 
@@ -70,9 +70,8 @@ def __init__(self, config):
         self.mcu = mcu = self.i2c.get_mcu()
         self.oid = oid = mcu.create_oid()
         self.query_mpu9250_cmd = None
-        self.query_mpu9250_status_cmd = None
         mcu.register_config_callback(self._build_config)
-        self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "mpu9250_data", oid)
+        self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, oid=oid)
         # Clock tracking
         chip_smooth = self.data_rate * BATCH_UPDATES * 2
         self.clock_sync = bulk_sensor.ClockSyncRegression(mcu, chip_smooth)
@@ -95,10 +94,8 @@ def _build_config(self):
                            % (self.oid,), on_restart=True)
         self.query_mpu9250_cmd = self.mcu.lookup_command(
             "query_mpu9250 oid=%c clock=%u rest_ticks=%u", cq=cmdqueue)
-        self.query_mpu9250_status_cmd = self.mcu.lookup_query_command(
-            "query_mpu9250_status oid=%c",
-            "mpu9250_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
-            " buffered=%c fifo=%u limit_count=%hu", oid=self.oid, cq=cmdqueue)
+        self.clock_updater.setup_query_command(
+            self.mcu, "query_mpu9250_status oid=%c", oid=self.oid, cq=cmdqueue)
     def read_reg(self, reg):
         params = self.i2c.i2c_read([reg], 1)
         return bytearray(params['response'])[0]
@@ -142,11 +139,6 @@ def _extract_samples(self, raw_samples):
         self.clock_sync.set_last_chip_clock(seq * SAMPLES_PER_BLOCK + i)
         del samples[count:]
         return samples
-
-    def _update_clock(self, minclock=0):
-        params = self.query_mpu9250_status_cmd.send([self.oid],
-                                                    minclock=minclock)
-        self.clock_updater.update_clock(params)
     # Start, stop, and process message batches
     def _start_measurements(self):
         # In case of miswiring, testing MPU9250 device ID prevents treating
@@ -184,8 +176,6 @@ def _start_measurements(self):
         logging.info("MPU9250 starting '%s' measurements", self.name)
         # Initialize clock tracking
         self.clock_updater.note_start(reqclock)
-        self._update_clock(minclock=reqclock)
-        self.clock_updater.clear_duration_filter()
         self.last_error_count = 0
     def _finish_measurements(self):
         # Halt bulk reading
@@ -195,15 +185,15 @@ def _finish_measurements(self):
         self.set_reg(REG_PWR_MGMT_1, SET_PWR_MGMT_1_SLEEP)
         self.set_reg(REG_PWR_MGMT_2, SET_PWR_MGMT_2_OFF)
     def _process_batch(self, eventtime):
-        self._update_clock()
+        self.clock_updater.update_clock()
         raw_samples = self.bulk_queue.pull_samples()
         if not raw_samples:
             return {}
         samples = self._extract_samples(raw_samples)
         if not samples:
             return {}
         return {'data': samples, 'errors': self.last_error_count,
-                'overflows': self.clock_updater.get_last_limit_count()}
+                'overflows': self.clock_updater.get_last_overflows()}
 
 def load_config(config):
     return MPU9250(config)

src/Kconfig

@@ -112,6 +112,10 @@ config WANT_SOFTWARE_SPI
     bool
     depends on HAVE_GPIO && HAVE_GPIO_SPI
     default y
+config NEED_SENSOR_BULK
+    bool
+    depends on WANT_SENSORS || WANT_LIS2DW
+    default y
 menu "Optional features (to reduce code size)"
     depends on HAVE_LIMITED_CODE_SIZE
 config WANT_GPIO_BITBANGING

src/Makefile

@@ -16,6 +16,7 @@ src-$(CONFIG_WANT_SOFTWARE_SPI) += spi_software.c
 src-$(CONFIG_WANT_SOFTWARE_I2C) += i2c_software.c
 sensors-src-$(CONFIG_HAVE_GPIO_SPI) := thermocouple.c sensor_adxl345.c \
     sensor_angle.c
-src-$(CONFIG_WANT_LIS2DW) += sensor_lis2dw.c
 sensors-src-$(CONFIG_HAVE_GPIO_I2C) += sensor_mpu9250.c
 src-$(CONFIG_WANT_SENSORS) += $(sensors-src-y)
+src-$(CONFIG_WANT_LIS2DW) += sensor_lis2dw.c
+src-$(CONFIG_NEED_SENSOR_BULK) += sensor_bulk.c