Add lsm6dsox MLC support

This commit is contained in:
iabdalkader 2021-10-23 22:25:07 +02:00
parent d322bcf0de
commit 63fc53be96
2 changed files with 171 additions and 52 deletions

View File

@ -0,0 +1,46 @@
# LSM6DSOX IMU MLC (Machine Learning Core) Example.
# Download the raw UCF file, copy to storage and reset.
# NOTE: The pre-trained models (UCF files) for the examples can be found here:
# https://github.com/STMicroelectronics/STMems_Machine_Learning_Core/tree/master/application_examples/lsm6dsox
import time
from lsm6dsox import LSM6DSOX
from machine import Pin, I2C
INT_MODE = True # Run in interrupt mode.
INT_FLAG = False # Set True on interrupt.
def imu_int_handler(pin):
global INT_FLAG
INT_FLAG = True
if (INT_MODE == True):
int_pin = Pin(24)
int_pin.irq(handler=imu_int_handler, trigger=Pin.IRQ_RISING)
i2c = I2C(0, scl=Pin(13), sda=Pin(12))
# Vibration detection example
UCF_FILE = "lsm6dsox_vibration_monitoring.ucf"
UCF_LABELS = {0:"no vibration", 1:"low vibration", 2:"high vibration"}
# NOTE: Selected data rate and scale must match the MLC data rate and scale.
lsm = LSM6DSOX(i2c, gyro_odr=26, accel_odr=26, gyro_scale=2000, accel_scale=4, ucf=UCF_FILE)
# Head gestures example
#UCF_FILE = "lsm6dsox_head_gestures.ucf"
#UCF_LABELS = {0:"Nod", 1:"Shake", 2:"Stationary", 3:"Swing", 4:"Walk"}
# NOTE: Selected data rate and scale must match the MLC data rate and scale.
#lsm = LSM6DSOX(i2c, gyro_odr=26, accel_odr=26, gyro_scale=250, accel_scale=2, ucf=UCF_FILE)
print("MLC configured...")
while (True):
if (INT_MODE):
if (INT_FLAG):
INT_FLAG=False
print(UCF_LABELS[lsm.read_mlc_output()[0]])
else:
buf = lsm.read_mlc_output()
if (buf != None):
print(UCF_LABELS[buf[0]])

View File

@ -40,92 +40,165 @@ while (True):
import array import array
class LSM6DSOX: class LSM6DSOX:
DEFAULT_ADDR = const(0x6A) CTRL3_C = const(0x12)
WHO_AM_I_REG = const(0x0F) CTRL1_XL = const(0x10)
CTRL8_XL = const(0x17)
CTRL9_XL = const(0x18)
CTRL1_XL = const(0x10) CTRL2_G = const(0x11)
CTRL8_XL = const(0x17) CTRL7_G = const(0x16)
CTRL2_G = const(0x11) OUTX_L_G = const(0x22)
CTRL7_G = const(0x16) OUTX_L_XL = const(0x28)
MLC_STATUS = const(0x38)
OUTX_L_G = const(0x22) DEFAULT_ADDR = const(0x6A)
OUTX_L_XL = const(0x28) WHO_AM_I_REG = const(0x0F)
def __init__(self, i2c, address=DEFAULT_ADDR): FUNC_CFG_ACCESS = const(0x01)
FUNC_CFG_BANK_USER = const(0)
FUNC_CFG_BANK_HUB = const(1)
FUNC_CFG_BANK_EMBED = const(2)
MLC0_SRC = const(0x70)
MLC_INT1 = const(0x0D)
TAP_CFG0 = const(0x56)
EMB_FUNC_EN_A = const(0x04)
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):
""" Initalizes Gyro and Accelerator.
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_scale: (245dps, 500dps, 1000dps, 2000dps)
accel_scale: (+/-2g, +/-4g, +/-8g, +-16g)
ucf: MLC program to load.
"""
self.i2c = i2c self.i2c = i2c
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, 1) != b'l'): 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])
self.init_gyro_accel()
def __read_reg(self, reg, size):
return self.i2c.readfrom_mem(self.address, reg, size)
def __write_reg(self, reg, val):
self.i2c.writeto_mem(self.address, reg, bytes([val]))
def init_gyro_accel(self, fs_gyro=104, fs_accel=104, scale_gyro=2000, scale_accel=4):
""" Initalizes Gyro and Accelerator.
fs_accel: (0, 1.6Hz, 3.33Hz, 6.66Hz, 12.5Hz, 26Hz, 52Hz, 104Hz, 208Hz, 416Hz, 888Hz)
fs_gyro: (0, 1.6Hz, 3.33Hz, 6.66Hz, 12.5Hz, 26Hz, 52Hz, 104Hz, 208Hz, 416Hz, 888Hz)
scale_gyro: (245dps, 500dps, 1000dps, 2000dps)
scale_accel: (+/-2g, +/-4g, +/-8g, +-16g)
"""
# 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
}
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.
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}
fs_gyro = round(fs_gyro, 2) gyro_odr = round(gyro_odr, 2)
fs_accel = round(fs_accel, 2) accel_odr = round(accel_odr, 2)
assert fs_gyro in ODR, "Invalid sampling rate: %d" % fs_accel assert gyro_odr in ODR, "Invalid sampling rate: %d" % accel_odr
assert scale_gyro in SCALE_GYRO, "invalid gyro scaling: %d" % scale_gyro assert gyro_scale in SCALE_GYRO, "invalid gyro scaling: %d" % gyro_scale
assert fs_accel in ODR, "Invalid sampling rate: %d" % fs_accel assert accel_odr in ODR, "Invalid sampling rate: %d" % accel_odr
assert scale_accel in SCALE_ACCEL, "invalid accelerometer scaling: %d" % scale_accel assert accel_scale in SCALE_ACCEL, "invalid accelerometer scaling: %d" % accel_scale
# Soft-reset the device.
self.reset()
# Load and configure MLC if UCF file is provided
if (ucf != None):
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[fs_accel] << 4) | (SCALE_ACCEL[scale_accel] << 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[fs_gyro] << 4) | (SCALE_GYRO[scale_gyro] << 2) | 0); self.__write_reg(CTRL2_G, (ODR[gyro_odr] << 4) | (SCALE_GYRO[gyro_scale] << 2) | 0);
self.scale_gyro = 32768 / scale_gyro self.gyro_scale = 32768 / gyro_scale
self.scale_accel = 32768 / scale_accel self.accel_scale = 32768 / accel_scale
def __read_reg(self, reg, size=1):
buf = self.i2c.readfrom_mem(self.address, reg, size)
if (size == 1):
return int(buf[0])
return [int(x) for x in buf]
def __write_reg(self, reg, val):
self.i2c.writeto_mem(self.address, reg, bytes([val]))
def reset(self):
self.__write_reg(CTRL3_C, self.__read_reg(CTRL3_C) | 0x1)
for i in range(0, 10):
if ((self.__read_reg(CTRL3_C) & 0x01) == 0):
return
time.sleep_ms(10)
raise OSError("Failed to reset LSM6DS device.")
def set_mem_bank(self, bank):
cfg = self.__read_reg(FUNC_CFG_ACCESS) & 0x3F
self.__write_reg(FUNC_CFG_ACCESS, cfg | (bank << 6))
def set_embedded_functions(self, enable, emb_ab=None):
self.set_mem_bank(FUNC_CFG_BANK_EMBED)
if (enable):
self.__write_reg(EMB_FUNC_EN_A, emb_ab[0])
self.__write_reg(EMB_FUNC_EN_B, emb_ab[1])
else:
emb_a = self.__read_reg(EMB_FUNC_EN_A)
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_B, (emb_b & 0xE6))
emb_ab = (emb_a, emb_b)
self.set_mem_bank(FUNC_CFG_BANK_USER)
return emb_ab
def load_mlc(self, ucf):
# Load MLC config from file
with open(ucf, "r") as ucf_file:
for l in ucf_file:
if (l.startswith("Ac")):
v = [int(v, 16) for v in l.strip().split(" ")[1:3]]
self.__write_reg(v[0], v[1])
emb_ab = self.set_embedded_functions(False)
# Disable I3C interface
self.__write_reg(CTRL9_XL, self.__read_reg(CTRL9_XL) | 0x01)
# Enable Block Data Update
self.__write_reg(CTRL3_C, self.__read_reg(CTRL3_C) | 0x40)
# Route signals on interrupt pin 1
self.set_mem_bank(FUNC_CFG_BANK_EMBED)
self.__write_reg(MLC_INT1, self.__read_reg(MLC_INT1) & 0x01)
self.set_mem_bank(FUNC_CFG_BANK_USER)
# Configure interrupt pin mode
self.__write_reg(TAP_CFG0, self.__read_reg(TAP_CFG0) | 0x41)
self.set_embedded_functions(True, emb_ab)
def read_mlc_output(self):
buf = None
if (self.__read_reg(MLC_STATUS) & 0x1):
self.__read_reg(0x1A, size=12)
self.set_mem_bank(FUNC_CFG_BANK_EMBED)
buf = self.__read_reg(MLC0_SRC, 8)
self.set_mem_bank(FUNC_CFG_BANK_USER)
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.scale_gyro 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.scale_accel 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)