Update LSM6DSOX driver.

This commit is contained in:
iabdalkader 2021-11-16 21:33:10 +02:00
parent df6f77bd06
commit 9c15742eb2

View File

@ -4,7 +4,8 @@ Source repo: https://github.com/hoihu/projects/tree/master/raspi-hat
The MIT License (MIT) The MIT License (MIT)
Copyright (c) 2013, 2014 Damien P. George Copyright (c) 2021 Damien P. George
Copyright (c) 2021 Ibrahim Abdelkader <iabdalkader@openmv.io>
Permission is hereby granted, free of charge, to any person obtaining a copy Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal of this software and associated documentation files (the "Software"), to deal
@ -24,7 +25,8 @@ LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE. THE SOFTWARE.
Example usage: Basic example usage:
import time import time
from lsm6dsox import LSM6DSOX from lsm6dsox import LSM6DSOX
@ -38,36 +40,47 @@ while (True):
time.sleep_ms(100) time.sleep_ms(100)
""" """
import array import array
from micropython import const
class LSM6DSOX: class LSM6DSOX:
CTRL3_C = const(0x12) _CTRL3_C = const(0x12)
CTRL1_XL = const(0x10) _CTRL1_XL = const(0x10)
CTRL8_XL = const(0x17) _CTRL8_XL = const(0x17)
CTRL9_XL = const(0x18) _CTRL9_XL = const(0x18)
CTRL2_G = const(0x11) _CTRL2_G = const(0x11)
CTRL7_G = const(0x16) _CTRL7_G = const(0x16)
OUTX_L_G = const(0x22) _OUTX_L_G = const(0x22)
OUTX_L_XL = const(0x28) _OUTX_L_XL = const(0x28)
MLC_STATUS = const(0x38) _MLC_STATUS = const(0x38)
DEFAULT_ADDR = const(0x6A) _DEFAULT_ADDR = const(0x6A)
WHO_AM_I_REG = const(0x0F) _WHO_AM_I_REG = const(0x0F)
FUNC_CFG_ACCESS = const(0x01) _FUNC_CFG_ACCESS = const(0x01)
FUNC_CFG_BANK_USER = const(0) _FUNC_CFG_BANK_USER = const(0)
FUNC_CFG_BANK_HUB = const(1) _FUNC_CFG_BANK_HUB = const(1)
FUNC_CFG_BANK_EMBED = const(2) _FUNC_CFG_BANK_EMBED = const(2)
MLC0_SRC = const(0x70) _MLC0_SRC = const(0x70)
MLC_INT1 = const(0x0D) _MLC_INT1 = const(0x0D)
TAP_CFG0 = const(0x56) _TAP_CFG0 = const(0x56)
EMB_FUNC_EN_A = const(0x04) _EMB_FUNC_EN_A = const(0x04)
EMB_FUNC_EN_B = const(0x05) _EMB_FUNC_EN_B = const(0x05)
def __init__(self, i2c, address=DEFAULT_ADDR, gyro_odr=104, accel_odr=104, gyro_scale=2000, accel_scale=4, ucf=None): def __init__(
self,
i2c,
address=_DEFAULT_ADDR,
gyro_odr=104,
accel_odr=104,
gyro_scale=2000,
accel_scale=4,
ucf=None,
):
"""Initalizes Gyro and Accelerator. """Initalizes Gyro and Accelerator.
accel_odr: (0, 1.6Hz, 3.33Hz, 6.66Hz, 12.5Hz, 26Hz, 52Hz, 104Hz, 208Hz, 416Hz, 888Hz) accel_odr: (0, 1.6Hz, 3.33Hz, 6.66Hz, 12.5Hz, 26Hz, 52Hz, 104Hz, 208Hz, 416Hz, 888Hz)
gyro_odr: (0, 1.6Hz, 3.33Hz, 6.66Hz, 12.5Hz, 26Hz, 52Hz, 104Hz, 208Hz, 416Hz, 888Hz) gyro_odr: (0, 1.6Hz, 3.33Hz, 6.66Hz, 12.5Hz, 26Hz, 52Hz, 104Hz, 208Hz, 416Hz, 888Hz)
@ -79,48 +92,65 @@ class LSM6DSOX:
self.address = address self.address = address
# check the id of the Accelerometer/Gyro # check the id of the Accelerometer/Gyro
if (self.__read_reg(WHO_AM_I_REG) != 108): if self.__read_reg(_WHO_AM_I_REG) != 108:
raise OSError("No LSM6DS device was found at address 0x%x" % (self.address)) raise OSError("No LSM6DS device was found at address 0x%x" % (self.address))
# allocate scratch buffer for efficient conversions and memread op's # allocate scratch buffer for efficient conversions and memread op's
self.scratch_int = array.array('h',[0, 0, 0]) self.scratch_int = array.array("h", [0, 0, 0])
SCALE_GYRO = {250: 0, 500: 1, 1000: 2, 2000: 3} SCALE_GYRO = {250: 0, 500: 1, 1000: 2, 2000: 3}
SCALE_ACCEL = {2: 0, 4: 2, 8: 3, 16: 1} SCALE_ACCEL = {2: 0, 4: 2, 8: 3, 16: 1}
# XL_HM_MODE = 0 by default. G_HM_MODE = 0 by default. # XL_HM_MODE = 0 by default. G_HM_MODE = 0 by default.
ODR = {0:0x00, 1.6:0x08, 3.33:0x09, 6.66:0x0A, 12.5:0x01, 26:0x02, 52:0x03, 104:0x04, 208:0x05, 416:0x06, 888:0x07} ODR = {
0: 0x00,
1.6: 0x08,
3.33: 0x09,
6.66: 0x0A,
12.5: 0x01,
26: 0x02,
52: 0x03,
104: 0x04,
208: 0x05,
416: 0x06,
888: 0x07,
}
gyro_odr = round(gyro_odr, 2) gyro_odr = round(gyro_odr, 2)
accel_odr = round(accel_odr, 2) accel_odr = round(accel_odr, 2)
assert gyro_odr in ODR, "Invalid sampling rate: %d" % accel_odr
assert gyro_scale in SCALE_GYRO, "invalid gyro scaling: %d" % gyro_scale
assert accel_odr in ODR, "Invalid sampling rate: %d" % accel_odr # Sanity checks
assert accel_scale in SCALE_ACCEL, "invalid accelerometer scaling: %d" % accel_scale if not gyro_odr in ODR:
raise ValueError("Invalid sampling rate: %d" % accel_odr)
if not gyro_scale in SCALE_GYRO:
raise ValueError("invalid gyro scaling: %d" % gyro_scale)
if not accel_odr in ODR:
raise ValueError("Invalid sampling rate: %d" % accel_odr)
if not accel_scale in SCALE_ACCEL:
raise ValueError("invalid accelerometer scaling: %d" % accel_scale)
# Soft-reset the device. # Soft-reset the device.
self.reset() self.reset()
# Load and configure MLC if UCF file is provided # Load and configure MLC if UCF file is provided
if (ucf != None): if ucf != None:
self.load_mlc(ucf) self.load_mlc(ucf)
# Set Gyroscope datarate and scale. # Set Gyroscope datarate and scale.
# Note output from LPF2 second filtering stage is selected. See Figure 18. # Note output from LPF2 second filtering stage is selected. See Figure 18.
self.__write_reg(CTRL1_XL, (ODR[accel_odr] << 4) | (SCALE_ACCEL[accel_scale] << 2) | 2); self.__write_reg(_CTRL1_XL, (ODR[accel_odr] << 4) | (SCALE_ACCEL[accel_scale] << 2) | 2)
# Enable LPF2 and HPF fast-settling mode, ODR/4 # Enable LPF2 and HPF fast-settling mode, ODR/4
self.__write_reg(CTRL8_XL, 0x09); self.__write_reg(_CTRL8_XL, 0x09)
# Set Gyroscope datarate and scale. # Set Gyroscope datarate and scale.
self.__write_reg(CTRL2_G, (ODR[gyro_odr] << 4) | (SCALE_GYRO[gyro_scale] << 2) | 0); self.__write_reg(_CTRL2_G, (ODR[gyro_odr] << 4) | (SCALE_GYRO[gyro_scale] << 2) | 0)
self.gyro_scale = 32768 / gyro_scale self.gyro_scale = 32768 / gyro_scale
self.accel_scale = 32768 / accel_scale self.accel_scale = 32768 / accel_scale
def __read_reg(self, reg, size=1): def __read_reg(self, reg, size=1):
buf = self.i2c.readfrom_mem(self.address, reg, size) buf = self.i2c.readfrom_mem(self.address, reg, size)
if (size == 1): if size == 1:
return int(buf[0]) return int(buf[0])
return [int(x) for x in buf] return [int(x) for x in buf]
@ -128,77 +158,77 @@ class LSM6DSOX:
self.i2c.writeto_mem(self.address, reg, bytes([val])) self.i2c.writeto_mem(self.address, reg, bytes([val]))
def reset(self): def reset(self):
self.__write_reg(CTRL3_C, self.__read_reg(CTRL3_C) | 0x1) self.__write_reg(_CTRL3_C, self.__read_reg(_CTRL3_C) | 0x1)
for i in range(0, 10): for i in range(0, 10):
if ((self.__read_reg(CTRL3_C) & 0x01) == 0): if (self.__read_reg(_CTRL3_C) & 0x01) == 0:
return return
time.sleep_ms(10) time.sleep_ms(10)
raise OSError("Failed to reset LSM6DS device.") raise OSError("Failed to reset LSM6DS device.")
def set_mem_bank(self, bank): def set_mem_bank(self, bank):
cfg = self.__read_reg(FUNC_CFG_ACCESS) & 0x3F cfg = self.__read_reg(_FUNC_CFG_ACCESS) & 0x3F
self.__write_reg(FUNC_CFG_ACCESS, cfg | (bank << 6)) self.__write_reg(_FUNC_CFG_ACCESS, cfg | (bank << 6))
def set_embedded_functions(self, enable, emb_ab=None): def set_embedded_functions(self, enable, emb_ab=None):
self.set_mem_bank(FUNC_CFG_BANK_EMBED) self.set_mem_bank(_FUNC_CFG_BANK_EMBED)
if (enable): if enable:
self.__write_reg(EMB_FUNC_EN_A, emb_ab[0]) self.__write_reg(_EMB_FUNC_EN_A, emb_ab[0])
self.__write_reg(EMB_FUNC_EN_B, emb_ab[1]) self.__write_reg(_EMB_FUNC_EN_B, emb_ab[1])
else: else:
emb_a = self.__read_reg(EMB_FUNC_EN_A) emb_a = self.__read_reg(_EMB_FUNC_EN_A)
emb_b = self.__read_reg(EMB_FUNC_EN_B) emb_b = self.__read_reg(_EMB_FUNC_EN_B)
self.__write_reg(EMB_FUNC_EN_A, (emb_a & 0xC7)) self.__write_reg(_EMB_FUNC_EN_A, (emb_a & 0xC7))
self.__write_reg(EMB_FUNC_EN_B, (emb_b & 0xE6)) self.__write_reg(_EMB_FUNC_EN_B, (emb_b & 0xE6))
emb_ab = (emb_a, emb_b) emb_ab = (emb_a, emb_b)
self.set_mem_bank(FUNC_CFG_BANK_USER) self.set_mem_bank(_FUNC_CFG_BANK_USER)
return emb_ab return emb_ab
def load_mlc(self, ucf): def load_mlc(self, ucf):
# Load MLC config from file # Load MLC config from file
with open(ucf, "r") as ucf_file: with open(ucf, "r") as ucf_file:
for l in ucf_file: for l in ucf_file:
if (l.startswith("Ac")): if l.startswith("Ac"):
v = [int(v, 16) for v in l.strip().split(" ")[1:3]] v = [int(v, 16) for v in l.strip().split(" ")[1:3]]
self.__write_reg(v[0], v[1]) self.__write_reg(v[0], v[1])
emb_ab = self.set_embedded_functions(False) emb_ab = self.set_embedded_functions(False)
# Disable I3C interface # Disable I3C interface
self.__write_reg(CTRL9_XL, self.__read_reg(CTRL9_XL) | 0x01) self.__write_reg(_CTRL9_XL, self.__read_reg(_CTRL9_XL) | 0x01)
# Enable Block Data Update # Enable Block Data Update
self.__write_reg(CTRL3_C, self.__read_reg(CTRL3_C) | 0x40) self.__write_reg(_CTRL3_C, self.__read_reg(_CTRL3_C) | 0x40)
# Route signals on interrupt pin 1 # Route signals on interrupt pin 1
self.set_mem_bank(FUNC_CFG_BANK_EMBED) self.set_mem_bank(_FUNC_CFG_BANK_EMBED)
self.__write_reg(MLC_INT1, self.__read_reg(MLC_INT1) & 0x01) self.__write_reg(_MLC_INT1, self.__read_reg(_MLC_INT1) & 0x01)
self.set_mem_bank(FUNC_CFG_BANK_USER) self.set_mem_bank(_FUNC_CFG_BANK_USER)
# Configure interrupt pin mode # Configure interrupt pin mode
self.__write_reg(TAP_CFG0, self.__read_reg(TAP_CFG0) | 0x41) self.__write_reg(_TAP_CFG0, self.__read_reg(_TAP_CFG0) | 0x41)
self.set_embedded_functions(True, emb_ab) self.set_embedded_functions(True, emb_ab)
def read_mlc_output(self): def read_mlc_output(self):
buf = None buf = None
if (self.__read_reg(MLC_STATUS) & 0x1): if self.__read_reg(_MLC_STATUS) & 0x1:
self.__read_reg(0x1A, size=12) self.__read_reg(0x1A, size=12)
self.set_mem_bank(FUNC_CFG_BANK_EMBED) self.set_mem_bank(_FUNC_CFG_BANK_EMBED)
buf = self.__read_reg(MLC0_SRC, 8) buf = self.__read_reg(_MLC0_SRC, 8)
self.set_mem_bank(FUNC_CFG_BANK_USER) self.set_mem_bank(_FUNC_CFG_BANK_USER)
return buf return buf
def read_gyro(self): def read_gyro(self):
"""Returns gyroscope vector in degrees/sec.""" """Returns gyroscope vector in degrees/sec."""
mv = memoryview(self.scratch_int) mv = memoryview(self.scratch_int)
f = self.gyro_scale f = self.gyro_scale
self.i2c.readfrom_mem_into(self.address, OUTX_L_G, mv) self.i2c.readfrom_mem_into(self.address, _OUTX_L_G, mv)
return (mv[0] / f, mv[1] / f, mv[2] / f) return (mv[0] / f, mv[1] / f, mv[2] / f)
def read_accel(self): def read_accel(self):
"""Returns acceleration vector in gravity units (9.81m/s^2).""" """Returns acceleration vector in gravity units (9.81m/s^2)."""
mv = memoryview(self.scratch_int) mv = memoryview(self.scratch_int)
f = self.accel_scale f = self.accel_scale
self.i2c.readfrom_mem_into(self.address, OUTX_L_XL, mv) self.i2c.readfrom_mem_into(self.address, _OUTX_L_XL, mv)
return (mv[0] / f, mv[1] / f, mv[2] / f) return (mv[0] / f, mv[1] / f, mv[2] / f)