diff --git a/klippy/extras/adxl345.py b/klippy/extras/adxl345.py index 36dc8071..1cac6143 100644 --- a/klippy/extras/adxl345.py +++ b/klippy/extras/adxl345.py @@ -3,7 +3,7 @@ # Copyright (C) 2020-2021 Kevin O'Connor # # This file may be distributed under the terms of the GNU GPLv3 license. -import logging, time, collections, threading, multiprocessing, os +import logging, time, collections, multiprocessing, os from . import bus, motion_report, bulk_sensor # ADXL345 registers @@ -195,9 +195,6 @@ class ADXL345: self.data_rate = config.getint('rate', 3200) if self.data_rate not in QUERY_RATES: raise config.error("Invalid rate parameter: %d" % (self.data_rate,)) - # Measurement storage (accessed from background thread) - self.lock = threading.Lock() - self.raw_samples = [] # Setup mcu sensor_adxl345 bulk query code self.spi = bus.MCU_SPI_from_config(config, 3, default_speed=5000000) self.mcu = mcu = self.spi.get_mcu() @@ -209,7 +206,7 @@ class ADXL345: mcu.add_config_cmd("query_adxl345 oid=%d clock=0 rest_ticks=0" % (oid,), on_restart=True) mcu.register_config_callback(self._build_config) - mcu.register_response(self._handle_adxl345_data, "adxl345_data", oid) + self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "adxl345_data", oid) # Clock tracking self.last_sequence = self.max_query_duration = 0 self.last_limit_count = self.last_error_count = 0 @@ -250,9 +247,6 @@ class ADXL345: # Measurement collection def is_measuring(self): return self.query_rate > 0 - def _handle_adxl345_data(self, params): - with self.lock: - self.raw_samples.append(params) def _extract_samples(self, raw_samples): # Load variables to optimize inner loop below (x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = self.axes_map @@ -335,10 +329,8 @@ class ADXL345: self.set_reg(REG_FIFO_CTL, 0x00) self.set_reg(REG_BW_RATE, QUERY_RATES[self.data_rate]) self.set_reg(REG_FIFO_CTL, SET_FIFO_CTL) - # Setup samples - with self.lock: - self.raw_samples = [] # Start bulk reading + self.bulk_queue.clear_samples() systime = self.printer.get_reactor().monotonic() print_time = self.mcu.estimated_print_time(systime) + MIN_MSG_TIME reqclock = self.mcu.print_time_to_clock(print_time) @@ -360,15 +352,12 @@ class ADXL345: # Halt bulk reading params = self.query_adxl345_end_cmd.send([self.oid, 0, 0]) self.query_rate = 0 - with self.lock: - self.raw_samples = [] + self.bulk_queue.clear_samples() logging.info("ADXL345 finished '%s' measurements", self.name) # API interface def _api_update(self, eventtime): self._update_clock() - with self.lock: - raw_samples = self.raw_samples - self.raw_samples = [] + raw_samples = self.bulk_queue.pull_samples() if not raw_samples: return {} samples = self._extract_samples(raw_samples) diff --git a/klippy/extras/angle.py b/klippy/extras/angle.py index 30a4447a..26b9c6f0 100644 --- a/klippy/extras/angle.py +++ b/klippy/extras/angle.py @@ -3,8 +3,8 @@ # Copyright (C) 2021,2022 Kevin O'Connor # # This file may be distributed under the terms of the GNU GPLv3 license. -import logging, math, threading -from . import bus, motion_report +import logging, math +from . import bus, motion_report, bulk_sensor MIN_MSG_TIME = 0.100 TCODE_ERROR = 0xff @@ -417,9 +417,6 @@ class Angle: # Measurement conversion self.start_clock = self.time_shift = self.sample_ticks = 0 self.last_sequence = self.last_angle = 0 - # Measurement storage (accessed from background thread) - self.lock = threading.Lock() - self.raw_samples = [] # Sensor type sensors = { "a1333": HelperA1333, "as5047d": HelperAS5047D, "tle5012b": HelperTLE5012B } @@ -439,8 +436,7 @@ class Angle: "query_spi_angle oid=%d clock=0 rest_ticks=0 time_shift=0" % (oid,), on_restart=True) mcu.register_config_callback(self._build_config) - mcu.register_response(self._handle_spi_angle_data, - "spi_angle_data", oid) + self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "spi_angle_data", oid) # API server endpoints self.api_dump = motion_report.APIDumpHelper( self.printer, self._api_update, self._api_startstop, 0.100) @@ -464,9 +460,6 @@ class Angle: # Measurement collection def is_measuring(self): return self.start_clock != 0 - def _handle_spi_angle_data(self, params): - with self.lock: - self.raw_samples.append(params) def _extract_samples(self, raw_samples): # Load variables to optimize inner loop below sample_ticks = self.sample_ticks @@ -524,9 +517,7 @@ class Angle: def _api_update(self, eventtime): if self.sensor_helper.is_tcode_absolute: self.sensor_helper.update_clock() - with self.lock: - raw_samples = self.raw_samples - self.raw_samples = [] + raw_samples = self.bulk_queue.pull_samples() if not raw_samples: return {} samples, error_count = self._extract_samples(raw_samples) @@ -541,8 +532,7 @@ class Angle: logging.info("Starting angle '%s' measurements", self.name) self.sensor_helper.start() # Start bulk reading - with self.lock: - self.raw_samples = [] + self.bulk_queue.clear_samples() self.last_sequence = 0 systime = self.printer.get_reactor().monotonic() print_time = self.mcu.estimated_print_time(systime) + MIN_MSG_TIME @@ -557,8 +547,7 @@ class Angle: # Halt bulk reading params = self.query_spi_angle_end_cmd.send([self.oid, 0, 0, 0]) self.start_clock = 0 - with self.lock: - self.raw_samples = [] + self.bulk_queue.clear_samples() self.sensor_helper.last_temperature = None logging.info("Stopped angle '%s' measurements", self.name) def _api_startstop(self, is_start): diff --git a/klippy/extras/bulk_sensor.py b/klippy/extras/bulk_sensor.py index 3d76eb6f..15749051 100644 --- a/klippy/extras/bulk_sensor.py +++ b/klippy/extras/bulk_sensor.py @@ -3,6 +3,26 @@ # Copyright (C) 2020-2023 Kevin O'Connor # # This file may be distributed under the terms of the GNU GPLv3 license. +import threading + +# Helper class to store incoming messages in a queue +class BulkDataQueue: + def __init__(self, mcu, msg_name, oid): + # Measurement storage (accessed from background thread) + self.lock = threading.Lock() + self.raw_samples = [] + # Register callback with mcu + mcu.register_response(self._handle_data, msg_name, oid) + def _handle_data(self, params): + with self.lock: + self.raw_samples.append(params) + def pull_samples(self): + with self.lock: + raw_samples = self.raw_samples + self.raw_samples = [] + return raw_samples + def clear_samples(self): + self.pull_samples() # Helper class for chip clock synchronization via linear regression class ClockSyncRegression: diff --git a/klippy/extras/lis2dw.py b/klippy/extras/lis2dw.py index 174e071a..af9faba5 100644 --- a/klippy/extras/lis2dw.py +++ b/klippy/extras/lis2dw.py @@ -4,7 +4,7 @@ # Copyright (C) 2020-2021 Kevin O'Connor # # This file may be distributed under the terms of the GNU GPLv3 license. -import logging, time, threading, multiprocessing, os +import logging from . import bus, motion_report, adxl345, bulk_sensor # LIS2DW registers @@ -50,9 +50,6 @@ class LIS2DW: raise config.error("Invalid lis2dw axes_map parameter") self.axes_map = [am[a.strip()] for a in axes_map] self.data_rate = 1600 - # Measurement storage (accessed from background thread) - self.lock = threading.Lock() - self.raw_samples = [] # Setup mcu sensor_lis2dw bulk query code self.spi = bus.MCU_SPI_from_config(config, 3, default_speed=5000000) self.mcu = mcu = self.spi.get_mcu() @@ -64,7 +61,7 @@ class LIS2DW: mcu.add_config_cmd("query_lis2dw oid=%d clock=0 rest_ticks=0" % (oid,), on_restart=True) mcu.register_config_callback(self._build_config) - mcu.register_response(self._handle_lis2dw_data, "lis2dw_data", oid) + self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "lis2dw_data", oid) # Clock tracking self.last_sequence = self.max_query_duration = 0 self.last_limit_count = self.last_error_count = 0 @@ -106,9 +103,6 @@ class LIS2DW: # Measurement collection def is_measuring(self): return self.query_rate > 0 - def _handle_lis2dw_data(self, params): - with self.lock: - self.raw_samples.append(params) def _extract_samples(self, raw_samples): # Load variables to optimize inner loop below (x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = self.axes_map @@ -198,10 +192,8 @@ class LIS2DW: # High-Performance Mode (14-bit resolution) self.set_reg(REG_LIS2DW_CTRL_REG1_ADDR, 0x94) - # Setup samples - with self.lock: - self.raw_samples = [] # Start bulk reading + self.bulk_queue.clear_samples() systime = self.printer.get_reactor().monotonic() print_time = self.mcu.estimated_print_time(systime) + MIN_MSG_TIME reqclock = self.mcu.print_time_to_clock(print_time) @@ -223,16 +215,13 @@ class LIS2DW: # Halt bulk reading params = self.query_lis2dw_end_cmd.send([self.oid, 0, 0]) self.query_rate = 0 - with self.lock: - self.raw_samples = [] + self.bulk_queue.clear_samples() logging.info("LIS2DW finished '%s' measurements", self.name) self.set_reg(REG_LIS2DW_FIFO_CTRL, 0x00) # API interface def _api_update(self, eventtime): self._update_clock() - with self.lock: - raw_samples = self.raw_samples - self.raw_samples = [] + raw_samples = self.bulk_queue.pull_samples() if not raw_samples: return {} samples = self._extract_samples(raw_samples) diff --git a/klippy/extras/mpu9250.py b/klippy/extras/mpu9250.py index 204ca870..cb5e7b28 100644 --- a/klippy/extras/mpu9250.py +++ b/klippy/extras/mpu9250.py @@ -4,7 +4,7 @@ # Copyright (C) 2020-2021 Kevin O'Connor # # This file may be distributed under the terms of the GNU GPLv3 license. -import logging, time, threading, multiprocessing, os +import logging, time from . import bus, motion_report, adxl345, bulk_sensor MPU9250_ADDR = 0x68 @@ -69,9 +69,6 @@ class MPU9250: self.data_rate = config.getint('rate', 4000) if self.data_rate not in SAMPLE_RATE_DIVS: raise config.error("Invalid rate parameter: %d" % (self.data_rate,)) - # Measurement storage (accessed from background thread) - self.lock = threading.Lock() - self.raw_samples = [] # Setup mcu sensor_mpu9250 bulk query code self.i2c = bus.MCU_I2C_from_config(config, default_addr=MPU9250_ADDR, @@ -81,7 +78,7 @@ class MPU9250: self.query_mpu9250_cmd = self.query_mpu9250_end_cmd = None self.query_mpu9250_status_cmd = None mcu.register_config_callback(self._build_config) - mcu.register_response(self._handle_mpu9250_data, "mpu9250_data", oid) + self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "mpu9250_data", oid) # Clock tracking self.last_sequence = self.max_query_duration = 0 self.last_limit_count = self.last_error_count = 0 @@ -120,9 +117,6 @@ class MPU9250: # Measurement collection def is_measuring(self): return self.query_rate > 0 - def _handle_mpu9250_data(self, params): - with self.lock: - self.raw_samples.append(params) def _extract_samples(self, raw_samples): # Load variables to optimize inner loop below (x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = self.axes_map @@ -210,10 +204,8 @@ class MPU9250: self.set_reg(REG_ACCEL_CONFIG, SET_ACCEL_CONFIG) self.set_reg(REG_ACCEL_CONFIG2, SET_ACCEL_CONFIG2) - # Setup samples - with self.lock: - self.raw_samples = [] # Start bulk reading + self.bulk_queue.clear_samples() systime = self.printer.get_reactor().monotonic() print_time = self.mcu.estimated_print_time(systime) + MIN_MSG_TIME reqclock = self.mcu.print_time_to_clock(print_time) @@ -235,8 +227,7 @@ class MPU9250: # Halt bulk reading params = self.query_mpu9250_end_cmd.send([self.oid, 0, 0]) self.query_rate = 0 - with self.lock: - self.raw_samples = [] + self.bulk_queue.clear_samples() 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) @@ -244,9 +235,7 @@ class MPU9250: # API interface def _api_update(self, eventtime): self._update_clock() - with self.lock: - raw_samples = self.raw_samples - self.raw_samples = [] + raw_samples = self.bulk_queue.pull_samples() if not raw_samples: return {} samples = self._extract_samples(raw_samples)