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

sensor_lis2dw: add lis3dh sensor and i2c communication

Browse Source 
Signed-off-by: Luke Vuksta <wulfstawulfsta@gmail.com>
This commit is contained in:
Wulfsta
2024-10-17 02:38:59 -04:00
committed by KevinOConnor
parent a34034494e
commit 0f7887fffe
6 changed files with 246 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

125
klippy/extras/lis2dw.py
View File

@@ -12,6 +12,8 @@ REG_LIS2DW_WHO_AM_I_ADDR = 0x0F
REG_LIS2DW_CTRL_REG1_ADDR = 0x20
REG_LIS2DW_CTRL_REG2_ADDR = 0x21
REG_LIS2DW_CTRL_REG3_ADDR = 0x22
REG_LIS2DW_CTRL_REG4_ADDR = 0x23
REG_LIS2DW_CTRL_REG5_ADDR = 0x24
REG_LIS2DW_CTRL_REG6_ADDR = 0x25
REG_LIS2DW_STATUS_REG_ADDR = 0x27
REG_LIS2DW_OUT_XL_ADDR = 0x28
@@ -26,26 +28,57 @@ REG_MOD_READ = 0x80
# REG_MOD_MULTI = 0x40
LIS2DW_DEV_ID = 0x44
LIS3DH_DEV_ID = 0x33
LIS_I2C_ADDR = 0x19
# Right shift for left justified registers.
FREEFALL_ACCEL = 9.80665
SCALE = FREEFALL_ACCEL * 1.952 / 4
LIS2DW_SCALE = FREEFALL_ACCEL * 1.952 / 4
LIS3DH_SCALE = FREEFALL_ACCEL * 3.906 / 16
BATCH_UPDATES = 0.100
# "Enums" that should be compatible with all python versions
LIS2DW_TYPE = 'LIS2DW'
LIS3DH_TYPE = 'LIS3DH'
SPI_SERIAL_TYPE = 'spi'
I2C_SERIAL_TYPE = 'i2c'
# Printer class that controls LIS2DW chip
class LIS2DW:
def __init__(self, config):
def __init__(self, config, lis_type):
self.printer = config.get_printer()
adxl345.AccelCommandHelper(config, self)
self.axes_map = adxl345.read_axes_map(config, SCALE, SCALE, SCALE)
self.data_rate = 1600
self.lis_type = lis_type
if self.lis_type == LIS2DW_TYPE:
self.axes_map = adxl345.read_axes_map(config, LIS2DW_SCALE,
LIS2DW_SCALE, LIS2DW_SCALE)
self.data_rate = 1600
else:
self.axes_map = adxl345.read_axes_map(config, LIS3DH_SCALE,
LIS3DH_SCALE, LIS3DH_SCALE)
self.data_rate = 1344
# Check for spi or i2c
if config.get('cs_pin', None) is not None:
self.bus_type = SPI_SERIAL_TYPE
else:
self.bus_type = I2C_SERIAL_TYPE
# 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()
if self.bus_type == SPI_SERIAL_TYPE:
self.bus = bus.MCU_SPI_from_config(config,
3, default_speed=5000000)
else:
self.bus = bus.MCU_I2C_from_config(config,
default_addr=LIS_I2C_ADDR, default_speed=400000)
self.mcu = mcu = self.bus.get_mcu()
self.oid = oid = mcu.create_oid()
self.query_lis2dw_cmd = None
mcu.add_config_cmd("config_lis2dw oid=%d spi_oid=%d"
% (oid, self.spi.get_oid()))
mcu.add_config_cmd("config_lis2dw oid=%d bus_oid=%d bus_oid_type=%s "
"lis_chip_type=%s" % (oid, self.bus.get_oid(),
self.bus_type, self.lis_type))
mcu.add_config_cmd("query_lis2dw oid=%d rest_ticks=0"
% (oid,), on_restart=True)
mcu.register_config_callback(self._build_config)
@@ -63,17 +96,23 @@ class LIS2DW:
self.name, {'header': hdr})
def _build_config(self):
cmdqueue = self.spi.get_command_queue()
cmdqueue = self.bus.get_command_queue()
self.query_lis2dw_cmd = self.mcu.lookup_command(
"query_lis2dw oid=%c rest_ticks=%u", 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'])
return response[1]
if self.bus_type == SPI_SERIAL_TYPE:
params = self.bus.spi_transfer([reg | REG_MOD_READ, 0x00])
response = bytearray(params['response'])
return response[1]
params = self.bus.i2c_read([reg], 1)
return bytearray(params['response'])[0]
def set_reg(self, reg, val, minclock=0):
self.spi.spi_send([reg, val & 0xFF], minclock=minclock)
if self.bus_type == SPI_SERIAL_TYPE:
self.bus.spi_send([reg, val & 0xFF], minclock=minclock)
else:
self.bus.i2c_write([reg, val & 0xFF], minclock=minclock)
stored_val = self.read_reg(reg)
if stored_val != val:
raise self.printer.command_error(
@@ -102,26 +141,48 @@ class LIS2DW:
# noise or wrong signal as a correctly initialized device
dev_id = self.read_reg(REG_LIS2DW_WHO_AM_I_ADDR)
logging.info("lis2dw_dev_id: %x", dev_id)
if dev_id != LIS2DW_DEV_ID:
raise self.printer.command_error(
"Invalid lis2dw id (got %x vs %x).\n"
"This is generally indicative of connection problems\n"
"(e.g. faulty wiring) or a faulty lis2dw chip."
% (dev_id, LIS2DW_DEV_ID))
# Setup chip in requested query rate
# ODR/2, +-16g, low-pass filter, Low-noise abled
self.set_reg(REG_LIS2DW_CTRL_REG6_ADDR, 0x34)
# Continuous mode: If the FIFO is full
# the new sample overwrites the older sample.
self.set_reg(REG_LIS2DW_FIFO_CTRL, 0xC0)
# High-Performance / Low-Power mode 1600/200 Hz
# High-Performance Mode (14-bit resolution)
self.set_reg(REG_LIS2DW_CTRL_REG1_ADDR, 0x94)
if self.lis_type == LIS2DW_TYPE:
if dev_id != LIS2DW_DEV_ID:
raise self.printer.command_error(
"Invalid lis2dw id (got %x vs %x).\n"
"This is generally indicative of connection problems\n"
"(e.g. faulty wiring) or a faulty lis2dw chip."
% (dev_id, LIS2DW_DEV_ID))
# Setup chip in requested query rate
# ODR/2, +-16g, low-pass filter, Low-noise abled
self.set_reg(REG_LIS2DW_CTRL_REG6_ADDR, 0x34)
# Continuous mode: If the FIFO is full
# the new sample overwrites the older sample.
self.set_reg(REG_LIS2DW_FIFO_CTRL, 0xC0)
# High-Performance / Low-Power mode 1600/200 Hz
# High-Performance Mode (14-bit resolution)
self.set_reg(REG_LIS2DW_CTRL_REG1_ADDR, 0x94)
else:
if dev_id != LIS3DH_DEV_ID:
raise self.printer.command_error(
"Invalid lis3dh id (got %x vs %x).\n"
"This is generally indicative of connection problems\n"
"(e.g. faulty wiring) or a faulty lis3dh chip."
% (dev_id, LIS3DH_DEV_ID))
# High Resolution / Low Power mode 1344/5376 Hz
# High Resolution mode (12-bit resolution)
# Enable X Y Z axes
self.set_reg(REG_LIS2DW_CTRL_REG1_ADDR, 0x97)
# Disable all filtering
self.set_reg(REG_LIS2DW_CTRL_REG2_ADDR, 0)
# Set +-8g, High Resolution mode
self.set_reg(REG_LIS2DW_CTRL_REG4_ADDR, 0x28)
# Enable FIFO
self.set_reg(REG_LIS2DW_CTRL_REG5_ADDR, 0x40)
# Stream mode
self.set_reg(REG_LIS2DW_FIFO_CTRL, 0x80)
# Start bulk reading
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)
if self.lis_type == LIS2DW_TYPE:
self.set_reg(REG_LIS2DW_FIFO_CTRL, 0xC0)
else:
self.set_reg(REG_LIS2DW_FIFO_CTRL, 0x80)
logging.info("LIS2DW starting '%s' measurements", self.name)
# Initialize clock tracking
self.ffreader.note_start()
@@ -142,7 +203,7 @@ class LIS2DW:
'overflows': self.ffreader.get_last_overflows()}
def load_config(config):
return LIS2DW(config)
return LIS2DW(config, LIS2DW_TYPE)
def load_config_prefix(config):
return LIS2DW(config)
return LIS2DW(config, LIS2DW_TYPE)