diff options
Diffstat (limited to 'klippy/extras/lis2dw.py')
| -rw-r--r-- | klippy/extras/lis2dw.py | 30 |
1 files changed, 10 insertions, 20 deletions
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) |