bulk_sensor: Rework ChipClockUpdater class into FixedFreqReader

Move the sensor_bulk_data message queuing into the class, and then
rename that class.  This simplifies the users of the code.

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

5 files changed, 66 insertions, 93 deletions

diff --git a/klippy/extras/adxl345.py b/klippy/extras/adxl345.py
index 07973894..5323be79 100644
--- a/klippy/extras/adxl345.py
+++ b/klippy/extras/adxl345.py
@@ -205,11 +205,9 @@ class ADXL345:
         mcu.add_config_cmd("query_adxl345 oid=%d rest_ticks=0"
                            % (oid,), on_restart=True)
         mcu.register_config_callback(self._build_config)
-        self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, oid=oid)
-        # Clock tracking
+        # Bulk sample message reading
         chip_smooth = self.data_rate * BATCH_UPDATES * 2
-        self.clock_updater = bulk_sensor.ChipClockUpdater(mcu, chip_smooth,
-                                                          "BBBBB")
+        self.ffreader = bulk_sensor.FixedFreqReader(mcu, chip_smooth, "BBBBB")
         self.last_error_count = 0
         # Process messages in batches
         self.batch_bulk = bulk_sensor.BatchBulkHelper(
@@ -223,8 +221,8 @@ class ADXL345:
         cmdqueue = self.spi.get_command_queue()
         self.query_adxl345_cmd = self.mcu.lookup_command(
             "query_adxl345 oid=%c rest_ticks=%u", cq=cmdqueue)
-        self.clock_updater.setup_query_command("query_adxl345_status oid=%c",
-                                               oid=self.oid, cq=cmdqueue)
+        self.ffreader.setup_query_command("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'])
@@ -243,10 +241,7 @@ class ADXL345:
         self.batch_bulk.add_client(aqh.handle_batch)
         return aqh
     # Measurement decoding
-    def _extract_samples(self, raw_samples):
-        # Convert messages to samples
-        samples = self.clock_updater.extract_samples(raw_samples)
-        # Convert samples
+    def _convert_samples(self, samples):
         (x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = self.axes_map
         count = 0
         for ptime, xlow, ylow, zlow, xzhigh, yzhigh in samples:
@@ -264,7 +259,6 @@ class ADXL345:
             samples[count] = (round(ptime, 6), x, y, z)
             count += 1
         del samples[count:]
-        return samples
     # Start, stop, and process message batches
     def _start_measurements(self):
         # In case of miswiring, testing ADXL345 device ID prevents treating
@@ -283,30 +277,26 @@ class ADXL345:
         self.set_reg(REG_BW_RATE, QUERY_RATES[self.data_rate])
         self.set_reg(REG_FIFO_CTL, SET_FIFO_CTL)
         # Start bulk reading
-        self.bulk_queue.clear_samples()
         rest_ticks = self.mcu.seconds_to_clock(4. / self.data_rate)
         self.query_adxl345_cmd.send([self.oid, rest_ticks])
         self.set_reg(REG_POWER_CTL, 0x08)
         logging.info("ADXL345 starting '%s' measurements", self.name)
         # Initialize clock tracking
-        self.clock_updater.note_start()
+        self.ffreader.note_start()
         self.last_error_count = 0
     def _finish_measurements(self):
         # Halt bulk reading
         self.set_reg(REG_POWER_CTL, 0x00)
         self.query_adxl345_cmd.send_wait_ack([self.oid, 0])
-        self.bulk_queue.clear_samples()
+        self.ffreader.note_end()
         logging.info("ADXL345 finished '%s' measurements", self.name)
     def _process_batch(self, eventtime):
-        self.clock_updater.update_clock()
-        raw_samples = self.bulk_queue.pull_samples()
-        if not raw_samples:
-            return {}
-        samples = self._extract_samples(raw_samples)
+        samples = self.ffreader.pull_samples()
+        self._convert_samples(samples)
         if not samples:
             return {}
         return {'data': samples, 'errors': self.last_error_count,
-                'overflows': self.clock_updater.get_last_overflows()}
+                'overflows': self.ffreader.get_last_overflows()}
 
 def load_config(config):
     return ADXL345(config)
diff --git a/klippy/extras/bulk_sensor.py b/klippy/extras/bulk_sensor.py
index d514bc91..6a63447b 100644
--- a/klippy/extras/bulk_sensor.py
+++ b/klippy/extras/bulk_sensor.py
@@ -200,8 +200,9 @@ class ClockSyncRegression:
 
 MAX_BULK_MSG_SIZE = 52
 
-# Handle common periodic chip status query responses
-class ChipClockUpdater:
+# Read sensor_bulk_data and calculate timestamps for devices that take
+# samples at a fixed frequency (and produce fixed data size samples).
+class FixedFreqReader:
     def __init__(self, mcu, chip_clock_smooth, unpack_fmt):
         self.mcu = mcu
         self.clock_sync = ClockSyncRegression(mcu, chip_clock_smooth)
@@ -211,27 +212,33 @@ class ChipClockUpdater:
         self.samples_per_block = MAX_BULK_MSG_SIZE // self.bytes_per_sample
         self.last_sequence = self.max_query_duration = 0
         self.last_overflows = 0
-        self.oid = self.query_status_cmd = None
+        self.bulk_queue = self.oid = self.query_status_cmd = None
     def setup_query_command(self, msgformat, oid, cq):
+        # Lookup sensor query command (that responds with sensor_bulk_status)
         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
+        # Read sensor_bulk_data messages and store in a queue
+        self.bulk_queue = BulkDataQueue(self.mcu, oid=oid)
     def get_last_overflows(self):
         return self.last_overflows
-    def clear_duration_filter(self):
+    def _clear_duration_filter(self):
         self.max_query_duration = 1 << 31
     def note_start(self):
         self.last_sequence = 0
         self.last_overflows = 0
+        # Clear local queue (clear any stale samples from previous session)
+        self.bulk_queue.clear_samples()
         # Set initial clock
-        self.clear_duration_filter()
-        self.update_clock(is_reset=True)
-        self.clear_duration_filter()
-    def update_clock(self, is_reset=False):
+        self._clear_duration_filter()
+        self._update_clock(is_reset=True)
+        self._clear_duration_filter()
+    def note_end(self):
+        # Clear local queue (free no longer needed memory)
+        self.bulk_queue.clear_samples()
+    def _update_clock(self, is_reset=False):
         params = self.query_status_cmd.send([self.oid])
         mcu_clock = self.mcu.clock32_to_clock64(params['clock'])
         seq_diff = (params['next_sequence'] - self.last_sequence) & 0xffff
@@ -257,10 +264,16 @@ class ChipClockUpdater:
             self.clock_sync.reset(avg_mcu_clock, chip_clock)
         else:
             self.clock_sync.update(avg_mcu_clock, chip_clock)
-    # Convert a list of sensor_bulk_data responses into list of samples
-    def extract_samples(self, raw_samples):
+    # Convert sensor_bulk_data responses into list of samples
+    def pull_samples(self):
+        # Query MCU for sample timing and update clock synchronization
+        self._update_clock()
+        # Pull sensor_bulk_data messages from local queue
+        raw_samples = self.bulk_queue.pull_samples()
+        if not raw_samples:
+            return []
         # Load variables to optimize inner loop below
-        last_sequence = self.get_last_sequence()
+        last_sequence = self.last_sequence
         time_base, chip_base, inv_freq = self.clock_sync.get_time_translation()
         unpack_from = self.unpack_from
         bytes_per_sample = self.bytes_per_sample
diff --git a/klippy/extras/ldc1612.py b/klippy/extras/ldc1612.py
index 4b8a3f72..2ae4dd7d 100644
--- a/klippy/extras/ldc1612.py
+++ b/klippy/extras/ldc1612.py
@@ -92,11 +92,9 @@ class LDC1612:
         mcu.add_config_cmd("query_ldc1612 oid=%d rest_ticks=0"
                            % (oid,), on_restart=True)
         mcu.register_config_callback(self._build_config)
-        self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, oid=oid)
-        # Clock tracking
+        # Bulk sample message reading
         chip_smooth = self.data_rate * BATCH_UPDATES * 2
-        self.clock_updater = bulk_sensor.ChipClockUpdater(mcu, chip_smooth,
-                                                          ">I")
+        self.ffreader = bulk_sensor.FixedFreqReader(mcu, chip_smooth, ">I")
         self.last_error_count = 0
         # Process messages in batches
         self.batch_bulk = bulk_sensor.BatchBulkHelper(
@@ -110,8 +108,8 @@ class LDC1612:
         cmdqueue = self.i2c.get_command_queue()
         self.query_ldc1612_cmd = self.mcu.lookup_command(
             "query_ldc1612 oid=%c rest_ticks=%u", cq=cmdqueue)
-        self.clock_updater.setup_query_command("query_ldc1612_status oid=%c",
-                                               oid=self.oid, cq=cmdqueue)
+        self.ffreader.setup_query_command("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)
@@ -144,10 +142,7 @@ class LDC1612:
         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):
-        # Convert messages to samples
-        samples = self.clock_updater.extract_samples(raw_samples)
-        # Convert samples
+    def _convert_samples(self, samples):
         freq_conv = float(LDC1612_FREQ) / (1<<28)
         count = 0
         for ptime, val in samples:
@@ -156,7 +151,6 @@ class LDC1612:
                 self.last_error_count += 1
             samples[count] = (round(ptime, 6), round(freq_conv * mv, 3), 999.9)
             count += 1
-        return samples
     # Start, stop, and process message batches
     def _start_measurements(self):
         # In case of miswiring, testing LDC1612 device ID prevents treating
@@ -180,27 +174,23 @@ class LDC1612:
         self.set_reg(REG_CONFIG, 0x001 | (1<<12) | (1<<10) | (1<<9))
         self.set_reg(REG_DRIVE_CURRENT0, self.dccal.get_drive_current() << 11)
         # Start bulk reading
-        self.bulk_queue.clear_samples()
         rest_ticks = self.mcu.seconds_to_clock(0.5 / self.data_rate)
         self.query_ldc1612_cmd.send([self.oid, rest_ticks])
         logging.info("LDC1612 starting '%s' measurements", self.name)
         # Initialize clock tracking
-        self.clock_updater.note_start()
+        self.ffreader.note_start()
         self.last_error_count = 0
     def _finish_measurements(self):
         # Halt bulk reading
         self.query_ldc1612_cmd.send_wait_ack([self.oid, 0])
-        self.bulk_queue.clear_samples()
+        self.ffreader.note_end()
         logging.info("LDC1612 finished '%s' measurements", self.name)
     def _process_batch(self, eventtime):
-        self.clock_updater.update_clock()
-        raw_samples = self.bulk_queue.pull_samples()
-        if not raw_samples:
-            return {}
-        samples = self._extract_samples(raw_samples)
+        samples = self.ffreader.pull_samples()
+        self._convert_samples(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()}
+                'overflows': self.ffreader.get_last_overflows()}
diff --git a/klippy/extras/lis2dw.py b/klippy/extras/lis2dw.py
index 96b7db76..3f17c1f4 100644
--- a/klippy/extras/lis2dw.py
+++ b/klippy/extras/lis2dw.py
@@ -49,11 +49,9 @@ class LIS2DW:
         mcu.add_config_cmd("query_lis2dw oid=%d rest_ticks=0"
                            % (oid,), on_restart=True)
         mcu.register_config_callback(self._build_config)
-        self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, oid=oid)
-        # Clock tracking
+        # Bulk sample message reading
         chip_smooth = self.data_rate * BATCH_UPDATES * 2
-        self.clock_updater = bulk_sensor.ChipClockUpdater(mcu, chip_smooth,
-                                                          "<hhh")
+        self.ffreader = bulk_sensor.FixedFreqReader(mcu, chip_smooth, "<hhh")
         self.last_error_count = 0
         # Process messages in batches
         self.batch_bulk = bulk_sensor.BatchBulkHelper(
@@ -68,8 +66,8 @@ class LIS2DW:
         cmdqueue = self.spi.get_command_queue()
         self.query_lis2dw_cmd = self.mcu.lookup_command(
             "query_lis2dw oid=%c rest_ticks=%u", cq=cmdqueue)
-        self.clock_updater.setup_query_command("query_lis2dw_status oid=%c",
-                                               oid=self.oid, cq=cmdqueue)
+        self.ffreader.setup_query_command("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'])
@@ -88,10 +86,7 @@ class LIS2DW:
         self.batch_bulk.add_client(aqh.handle_batch)
         return aqh
     # Measurement decoding
-    def _extract_samples(self, raw_samples):
-        # Convert messages to samples
-        samples = self.clock_updater.extract_samples(raw_samples)
-        # Convert samples
+    def _convert_samples(self, samples):
         (x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = self.axes_map
         count = 0
         for ptime, rx, ry, rz in samples:
@@ -101,7 +96,6 @@ class LIS2DW:
             z = round(raw_xyz[z_pos] * z_scale, 6)
             samples[count] = (round(ptime, 6), x, y, z)
             count += 1
-        return samples
     # Start, stop, and process message batches
     def _start_measurements(self):
         # In case of miswiring, testing LIS2DW device ID prevents treating
@@ -125,31 +119,27 @@ class LIS2DW:
         self.set_reg(REG_LIS2DW_CTRL_REG1_ADDR, 0x94)
 
         # Start bulk reading
-        self.bulk_queue.clear_samples()
         rest_ticks = self.mcu.seconds_to_clock(4. / self.data_rate)
         self.query_lis2dw_cmd.send([self.oid, rest_ticks])
         self.set_reg(REG_LIS2DW_FIFO_CTRL, 0xC0)
         logging.info("LIS2DW starting '%s' measurements", self.name)
         # Initialize clock tracking
-        self.clock_updater.note_start()
+        self.ffreader.note_start()
         self.last_error_count = 0
     def _finish_measurements(self):
         # Halt bulk reading
         self.set_reg(REG_LIS2DW_FIFO_CTRL, 0x00)
         self.query_lis2dw_cmd.send_wait_ack([self.oid, 0])
-        self.bulk_queue.clear_samples()
+        self.ffreader.note_end()
         logging.info("LIS2DW finished '%s' measurements", self.name)
         self.set_reg(REG_LIS2DW_FIFO_CTRL, 0x00)
     def _process_batch(self, eventtime):
-        self.clock_updater.update_clock()
-        raw_samples = self.bulk_queue.pull_samples()
-        if not raw_samples:
-            return {}
-        samples = self._extract_samples(raw_samples)
+        samples = self.ffreader.pull_samples()
+        self._convert_samples(samples)
         if not samples:
             return {}
         return {'data': samples, 'errors': self.last_error_count,
-                'overflows': self.clock_updater.get_last_overflows()}
+                'overflows': self.ffreader.get_last_overflows()}
 
 def load_config(config):
     return LIS2DW(config)
diff --git a/klippy/extras/mpu9250.py b/klippy/extras/mpu9250.py
index abc7911a..9a07b705 100644
--- a/klippy/extras/mpu9250.py
+++ b/klippy/extras/mpu9250.py
@@ -71,11 +71,9 @@ class MPU9250:
         self.oid = oid = mcu.create_oid()
         self.query_mpu9250_cmd = None
         mcu.register_config_callback(self._build_config)
-        self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, oid=oid)
-        # Clock tracking
+        # Bulk sample message reading
         chip_smooth = self.data_rate * BATCH_UPDATES * 2
-        self.clock_updater = bulk_sensor.ChipClockUpdater(mcu, chip_smooth,
-                                                          ">hhh")
+        self.ffreader = bulk_sensor.FixedFreqReader(mcu, chip_smooth, ">hhh")
         self.last_error_count = 0
         # Process messages in batches
         self.batch_bulk = bulk_sensor.BatchBulkHelper(
@@ -93,8 +91,8 @@ class MPU9250:
                            % (self.oid,), on_restart=True)
         self.query_mpu9250_cmd = self.mcu.lookup_command(
             "query_mpu9250 oid=%c rest_ticks=%u", cq=cmdqueue)
-        self.clock_updater.setup_query_command("query_mpu9250_status oid=%c",
-                                               oid=self.oid, cq=cmdqueue)
+        self.ffreader.setup_query_command("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]
@@ -105,10 +103,7 @@ class MPU9250:
         self.batch_bulk.add_client(aqh.handle_batch)
         return aqh
     # Measurement decoding
-    def _extract_samples(self, raw_samples):
-        # Convert messages to samples
-        samples = self.clock_updater.extract_samples(raw_samples)
-        # Convert samples
+    def _convert_samples(self, samples):
         (x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = self.axes_map
         count = 0
         for ptime, rx, ry, rz in samples:
@@ -118,7 +113,6 @@ class MPU9250:
             z = round(raw_xyz[z_pos] * z_scale, 6)
             samples[count] = (round(ptime, 6), x, y, z)
             count += 1
-        return samples
     # Start, stop, and process message batches
     def _start_measurements(self):
         # In case of miswiring, testing MPU9250 device ID prevents treating
@@ -151,32 +145,28 @@ class MPU9250:
         self.read_reg(REG_INT_STATUS) # clear FIFO overflow flag
 
         # Start bulk reading
-        self.bulk_queue.clear_samples()
         rest_ticks = self.mcu.seconds_to_clock(4. / self.data_rate)
         self.query_mpu9250_cmd.send([self.oid, rest_ticks])
         self.set_reg(REG_FIFO_EN, SET_ENABLE_FIFO)
         logging.info("MPU9250 starting '%s' measurements", self.name)
         # Initialize clock tracking
-        self.clock_updater.note_start()
+        self.ffreader.note_start()
         self.last_error_count = 0
     def _finish_measurements(self):
         # Halt bulk reading
         self.set_reg(REG_FIFO_EN, SET_DISABLE_FIFO)
         self.query_mpu9250_cmd.send_wait_ack([self.oid, 0])
-        self.bulk_queue.clear_samples()
+        self.ffreader.note_end()
         logging.info("MPU9250 finished '%s' measurements", self.name)
         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.clock_updater.update_clock()
-        raw_samples = self.bulk_queue.pull_samples()
-        if not raw_samples:
-            return {}
-        samples = self._extract_samples(raw_samples)
+        samples = self.ffreader.pull_samples()
+        self._convert_samples(samples)
         if not samples:
             return {}
         return {'data': samples, 'errors': self.last_error_count,
-                'overflows': self.clock_updater.get_last_overflows()}
+                'overflows': self.ffreader.get_last_overflows()}
 
 def load_config(config):
     return MPU9250(config)