mirror of
https://github.com/openmv/openmv.git
synced 2025-11-04 14:49:50 +08:00
Update LSM6DSOX driver.
This commit is contained in:
parent
df6f77bd06
commit
9c15742eb2
@ -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)
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user