3diy
/
klipper
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

bulk_sensor: Add new BulkDataQueue class 

Move the bulk sample queue collection to a new helper class in
bulk_sensor.py.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2023-12-16 11:30:51 -05:00
parent 978c294741
commit e67cbbe5c1
5 changed files with 41 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
klippy/extras/adxl345.py
View File

@ -3,7 +3,7 @@
# Copyright (C) 2020-2021 Kevin O'Connor <kevin@koconnor.net> # Copyright (C) 2020-2021 Kevin O'Connor <kevin@koconnor.net>
# #
# This file may be distributed under the terms of the GNU GPLv3 license. # 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 from . import bus, motion_report, bulk_sensor
# ADXL345 registers # ADXL345 registers
@ -195,9 +195,6 @@ class ADXL345:
self.data_rate = config.getint('rate', 3200) self.data_rate = config.getint('rate', 3200)
if self.data_rate not in QUERY_RATES: if self.data_rate not in QUERY_RATES:
raise config.error("Invalid rate parameter: %d" % (self.data_rate,)) 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 # Setup mcu sensor_adxl345 bulk query code
self.spi = bus.MCU_SPI_from_config(config, 3, default_speed=5000000) self.spi = bus.MCU_SPI_from_config(config, 3, default_speed=5000000)
self.mcu = mcu = self.spi.get_mcu() 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" mcu.add_config_cmd("query_adxl345 oid=%d clock=0 rest_ticks=0"
% (oid,), on_restart=True) % (oid,), on_restart=True)
mcu.register_config_callback(self._build_config) 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 # Clock tracking
self.last_sequence = self.max_query_duration = 0 self.last_sequence = self.max_query_duration = 0
self.last_limit_count = self.last_error_count = 0 self.last_limit_count = self.last_error_count = 0
@ -250,9 +247,6 @@ class ADXL345:
# Measurement collection # Measurement collection
def is_measuring(self): def is_measuring(self):
return self.query_rate > 0 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): def _extract_samples(self, raw_samples):
# Load variables to optimize inner loop below # Load variables to optimize inner loop below
(x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = self.axes_map (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_FIFO_CTL, 0x00)
self.set_reg(REG_BW_RATE, QUERY_RATES[self.data_rate]) self.set_reg(REG_BW_RATE, QUERY_RATES[self.data_rate])
self.set_reg(REG_FIFO_CTL, SET_FIFO_CTL) self.set_reg(REG_FIFO_CTL, SET_FIFO_CTL)
# Setup samples
with self.lock:
self.raw_samples = []
# Start bulk reading # Start bulk reading
self.bulk_queue.clear_samples()
systime = self.printer.get_reactor().monotonic() systime = self.printer.get_reactor().monotonic()
print_time = self.mcu.estimated_print_time(systime) + MIN_MSG_TIME print_time = self.mcu.estimated_print_time(systime) + MIN_MSG_TIME
reqclock = self.mcu.print_time_to_clock(print_time) reqclock = self.mcu.print_time_to_clock(print_time)
@ -360,15 +352,12 @@ class ADXL345:
# Halt bulk reading # Halt bulk reading
params = self.query_adxl345_end_cmd.send([self.oid, 0, 0]) params = self.query_adxl345_end_cmd.send([self.oid, 0, 0])
self.query_rate = 0 self.query_rate = 0
with self.lock: self.bulk_queue.clear_samples()
self.raw_samples = []
logging.info("ADXL345 finished '%s' measurements", self.name) logging.info("ADXL345 finished '%s' measurements", self.name)
# API interface # API interface
def _api_update(self, eventtime): def _api_update(self, eventtime):
self._update_clock() self._update_clock()
with self.lock: raw_samples = self.bulk_queue.pull_samples()
raw_samples = self.raw_samples
self.raw_samples = []
if not raw_samples: if not raw_samples:
return {} return {}
samples = self._extract_samples(raw_samples) samples = self._extract_samples(raw_samples)

23
klippy/extras/angle.py
View File

@ -3,8 +3,8 @@
# Copyright (C) 2021,2022 Kevin O'Connor <kevin@koconnor.net> # Copyright (C) 2021,2022 Kevin O'Connor <kevin@koconnor.net>
# #
# This file may be distributed under the terms of the GNU GPLv3 license. # This file may be distributed under the terms of the GNU GPLv3 license.
import logging, math, threading import logging, math
from . import bus, motion_report from . import bus, motion_report, bulk_sensor
MIN_MSG_TIME = 0.100 MIN_MSG_TIME = 0.100
TCODE_ERROR = 0xff TCODE_ERROR = 0xff
@ -417,9 +417,6 @@ class Angle:
# Measurement conversion # Measurement conversion
self.start_clock = self.time_shift = self.sample_ticks = 0 self.start_clock = self.time_shift = self.sample_ticks = 0
self.last_sequence = self.last_angle = 0 self.last_sequence = self.last_angle = 0
# Measurement storage (accessed from background thread)
self.lock = threading.Lock()
self.raw_samples = []
# Sensor type # Sensor type
sensors = { "a1333": HelperA1333, "as5047d": HelperAS5047D, sensors = { "a1333": HelperA1333, "as5047d": HelperAS5047D,
"tle5012b": HelperTLE5012B } "tle5012b": HelperTLE5012B }
@ -439,8 +436,7 @@ class Angle:
"query_spi_angle oid=%d clock=0 rest_ticks=0 time_shift=0" "query_spi_angle oid=%d clock=0 rest_ticks=0 time_shift=0"
% (oid,), on_restart=True) % (oid,), on_restart=True)
mcu.register_config_callback(self._build_config) mcu.register_config_callback(self._build_config)
mcu.register_response(self._handle_spi_angle_data, self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "spi_angle_data", oid)
"spi_angle_data", oid)
# API server endpoints # API server endpoints
self.api_dump = motion_report.APIDumpHelper( self.api_dump = motion_report.APIDumpHelper(
self.printer, self._api_update, self._api_startstop, 0.100) self.printer, self._api_update, self._api_startstop, 0.100)
@ -464,9 +460,6 @@ class Angle:
# Measurement collection # Measurement collection
def is_measuring(self): def is_measuring(self):
return self.start_clock != 0 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): def _extract_samples(self, raw_samples):
# Load variables to optimize inner loop below # Load variables to optimize inner loop below
sample_ticks = self.sample_ticks sample_ticks = self.sample_ticks
@ -524,9 +517,7 @@ class Angle:
def _api_update(self, eventtime): def _api_update(self, eventtime):
if self.sensor_helper.is_tcode_absolute: if self.sensor_helper.is_tcode_absolute:
self.sensor_helper.update_clock() self.sensor_helper.update_clock()
with self.lock: raw_samples = self.bulk_queue.pull_samples()
raw_samples = self.raw_samples
self.raw_samples = []
if not raw_samples: if not raw_samples:
return {} return {}
samples, error_count = self._extract_samples(raw_samples) samples, error_count = self._extract_samples(raw_samples)
@ -541,8 +532,7 @@ class Angle:
logging.info("Starting angle '%s' measurements", self.name) logging.info("Starting angle '%s' measurements", self.name)
self.sensor_helper.start() self.sensor_helper.start()
# Start bulk reading # Start bulk reading
with self.lock: self.bulk_queue.clear_samples()
self.raw_samples = []
self.last_sequence = 0 self.last_sequence = 0
systime = self.printer.get_reactor().monotonic() systime = self.printer.get_reactor().monotonic()
print_time = self.mcu.estimated_print_time(systime) + MIN_MSG_TIME print_time = self.mcu.estimated_print_time(systime) + MIN_MSG_TIME
@ -557,8 +547,7 @@ class Angle:
# Halt bulk reading # Halt bulk reading
params = self.query_spi_angle_end_cmd.send([self.oid, 0, 0, 0]) params = self.query_spi_angle_end_cmd.send([self.oid, 0, 0, 0])
self.start_clock = 0 self.start_clock = 0
with self.lock: self.bulk_queue.clear_samples()
self.raw_samples = []
self.sensor_helper.last_temperature = None self.sensor_helper.last_temperature = None
logging.info("Stopped angle '%s' measurements", self.name) logging.info("Stopped angle '%s' measurements", self.name)
def _api_startstop(self, is_start): def _api_startstop(self, is_start):

20
klippy/extras/bulk_sensor.py
View File

@ -3,6 +3,26 @@
# Copyright (C) 2020-2023 Kevin O'Connor <kevin@koconnor.net> # Copyright (C) 2020-2023 Kevin O'Connor <kevin@koconnor.net>
# #
# This file may be distributed under the terms of the GNU GPLv3 license. # 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 # Helper class for chip clock synchronization via linear regression
class ClockSyncRegression: class ClockSyncRegression:

21
klippy/extras/lis2dw.py
View File

@ -4,7 +4,7 @@
# Copyright (C) 2020-2021 Kevin O'Connor <kevin@koconnor.net> # Copyright (C) 2020-2021 Kevin O'Connor <kevin@koconnor.net>
# #
# This file may be distributed under the terms of the GNU GPLv3 license. # 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 from . import bus, motion_report, adxl345, bulk_sensor
# LIS2DW registers # LIS2DW registers
@ -50,9 +50,6 @@ class LIS2DW:
raise config.error("Invalid lis2dw axes_map parameter") raise config.error("Invalid lis2dw axes_map parameter")
self.axes_map = [am[a.strip()] for a in axes_map] self.axes_map = [am[a.strip()] for a in axes_map]
self.data_rate = 1600 self.data_rate = 1600
# Measurement storage (accessed from background thread)
self.lock = threading.Lock()
self.raw_samples = []
# Setup mcu sensor_lis2dw bulk query code # Setup mcu sensor_lis2dw bulk query code
self.spi = bus.MCU_SPI_from_config(config, 3, default_speed=5000000) self.spi = bus.MCU_SPI_from_config(config, 3, default_speed=5000000)
self.mcu = mcu = self.spi.get_mcu() 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" mcu.add_config_cmd("query_lis2dw oid=%d clock=0 rest_ticks=0"
% (oid,), on_restart=True) % (oid,), on_restart=True)
mcu.register_config_callback(self._build_config) 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 # Clock tracking
self.last_sequence = self.max_query_duration = 0 self.last_sequence = self.max_query_duration = 0
self.last_limit_count = self.last_error_count = 0 self.last_limit_count = self.last_error_count = 0
@ -106,9 +103,6 @@ class LIS2DW:
# Measurement collection # Measurement collection
def is_measuring(self): def is_measuring(self):
return self.query_rate > 0 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): def _extract_samples(self, raw_samples):
# Load variables to optimize inner loop below # Load variables to optimize inner loop below
(x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = self.axes_map (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) # High-Performance Mode (14-bit resolution)
self.set_reg(REG_LIS2DW_CTRL_REG1_ADDR, 0x94) self.set_reg(REG_LIS2DW_CTRL_REG1_ADDR, 0x94)
# Setup samples
with self.lock:
self.raw_samples = []
# Start bulk reading # Start bulk reading
self.bulk_queue.clear_samples()
systime = self.printer.get_reactor().monotonic() systime = self.printer.get_reactor().monotonic()
print_time = self.mcu.estimated_print_time(systime) + MIN_MSG_TIME print_time = self.mcu.estimated_print_time(systime) + MIN_MSG_TIME
reqclock = self.mcu.print_time_to_clock(print_time) reqclock = self.mcu.print_time_to_clock(print_time)
@ -223,16 +215,13 @@ class LIS2DW:
# Halt bulk reading # Halt bulk reading
params = self.query_lis2dw_end_cmd.send([self.oid, 0, 0]) params = self.query_lis2dw_end_cmd.send([self.oid, 0, 0])
self.query_rate = 0 self.query_rate = 0
with self.lock: self.bulk_queue.clear_samples()
self.raw_samples = []
logging.info("LIS2DW finished '%s' measurements", self.name) logging.info("LIS2DW finished '%s' measurements", self.name)
self.set_reg(REG_LIS2DW_FIFO_CTRL, 0x00) self.set_reg(REG_LIS2DW_FIFO_CTRL, 0x00)
# API interface # API interface
def _api_update(self, eventtime): def _api_update(self, eventtime):
self._update_clock() self._update_clock()
with self.lock: raw_samples = self.bulk_queue.pull_samples()
raw_samples = self.raw_samples
self.raw_samples = []
if not raw_samples: if not raw_samples:
return {} return {}
samples = self._extract_samples(raw_samples) samples = self._extract_samples(raw_samples)

21
klippy/extras/mpu9250.py
View File

@ -4,7 +4,7 @@
# Copyright (C) 2020-2021 Kevin O'Connor <kevin@koconnor.net> # Copyright (C) 2020-2021 Kevin O'Connor <kevin@koconnor.net>
# #
# This file may be distributed under the terms of the GNU GPLv3 license. # 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 from . import bus, motion_report, adxl345, bulk_sensor
MPU9250_ADDR = 0x68 MPU9250_ADDR = 0x68
@ -69,9 +69,6 @@ class MPU9250:
self.data_rate = config.getint('rate', 4000) self.data_rate = config.getint('rate', 4000)
if self.data_rate not in SAMPLE_RATE_DIVS: if self.data_rate not in SAMPLE_RATE_DIVS:
raise config.error("Invalid rate parameter: %d" % (self.data_rate,)) 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 # Setup mcu sensor_mpu9250 bulk query code
self.i2c = bus.MCU_I2C_from_config(config, self.i2c = bus.MCU_I2C_from_config(config,
default_addr=MPU9250_ADDR, default_addr=MPU9250_ADDR,
@ -81,7 +78,7 @@ class MPU9250:
self.query_mpu9250_cmd = self.query_mpu9250_end_cmd = None self.query_mpu9250_cmd = self.query_mpu9250_end_cmd = None
self.query_mpu9250_status_cmd = None self.query_mpu9250_status_cmd = None
mcu.register_config_callback(self._build_config) 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 # Clock tracking
self.last_sequence = self.max_query_duration = 0 self.last_sequence = self.max_query_duration = 0
self.last_limit_count = self.last_error_count = 0 self.last_limit_count = self.last_error_count = 0
@ -120,9 +117,6 @@ class MPU9250:
# Measurement collection # Measurement collection
def is_measuring(self): def is_measuring(self):
return self.query_rate > 0 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): def _extract_samples(self, raw_samples):
# Load variables to optimize inner loop below # Load variables to optimize inner loop below
(x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = self.axes_map (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_CONFIG, SET_ACCEL_CONFIG)
self.set_reg(REG_ACCEL_CONFIG2, SET_ACCEL_CONFIG2) self.set_reg(REG_ACCEL_CONFIG2, SET_ACCEL_CONFIG2)
# Setup samples
with self.lock:
self.raw_samples = []
# Start bulk reading # Start bulk reading
self.bulk_queue.clear_samples()
systime = self.printer.get_reactor().monotonic() systime = self.printer.get_reactor().monotonic()
print_time = self.mcu.estimated_print_time(systime) + MIN_MSG_TIME print_time = self.mcu.estimated_print_time(systime) + MIN_MSG_TIME
reqclock = self.mcu.print_time_to_clock(print_time) reqclock = self.mcu.print_time_to_clock(print_time)
@ -235,8 +227,7 @@ class MPU9250:
# Halt bulk reading # Halt bulk reading
params = self.query_mpu9250_end_cmd.send([self.oid, 0, 0]) params = self.query_mpu9250_end_cmd.send([self.oid, 0, 0])
self.query_rate = 0 self.query_rate = 0
with self.lock: self.bulk_queue.clear_samples()
self.raw_samples = []
logging.info("MPU9250 finished '%s' measurements", self.name) 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_1, SET_PWR_MGMT_1_SLEEP)
self.set_reg(REG_PWR_MGMT_2, SET_PWR_MGMT_2_OFF) self.set_reg(REG_PWR_MGMT_2, SET_PWR_MGMT_2_OFF)
@ -244,9 +235,7 @@ class MPU9250:
# API interface # API interface
def _api_update(self, eventtime): def _api_update(self, eventtime):
self._update_clock() self._update_clock()
with self.lock: raw_samples = self.bulk_queue.pull_samples()
raw_samples = self.raw_samples
self.raw_samples = []
if not raw_samples: if not raw_samples:
return {} return {}
samples = self._extract_samples(raw_samples) samples = self._extract_samples(raw_samples)