From 63fc53be96f4d98cadb9e485dc6f91cbec2acdea Mon Sep 17 00:00:00 2001 From: iabdalkader Date: Sat, 23 Oct 2021 22:25:07 +0200 Subject: [PATCH] Add lsm6dsox MLC support --- .../Nano-RP2040/01-Sensors/lsm6dsox_mlc.py | 46 +++++ scripts/libraries/lsm6dsox.py | 177 +++++++++++++----- 2 files changed, 171 insertions(+), 52 deletions(-) create mode 100644 scripts/examples/Arduino/Nano-RP2040/01-Sensors/lsm6dsox_mlc.py diff --git a/scripts/examples/Arduino/Nano-RP2040/01-Sensors/lsm6dsox_mlc.py b/scripts/examples/Arduino/Nano-RP2040/01-Sensors/lsm6dsox_mlc.py new file mode 100644 index 000000000..a5443fa5f --- /dev/null +++ b/scripts/examples/Arduino/Nano-RP2040/01-Sensors/lsm6dsox_mlc.py @@ -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]]) diff --git a/scripts/libraries/lsm6dsox.py b/scripts/libraries/lsm6dsox.py index d6167526b..ec16c95a1 100644 --- a/scripts/libraries/lsm6dsox.py +++ b/scripts/libraries/lsm6dsox.py @@ -40,92 +40,165 @@ while (True): import array class LSM6DSOX: - DEFAULT_ADDR = const(0x6A) - WHO_AM_I_REG = const(0x0F) + CTRL3_C = const(0x12) + CTRL1_XL = const(0x10) + CTRL8_XL = const(0x17) + CTRL9_XL = const(0x18) - CTRL1_XL = const(0x10) - CTRL8_XL = const(0x17) + CTRL2_G = const(0x11) + CTRL7_G = const(0x16) - CTRL2_G = const(0x11) - CTRL7_G = const(0x16) + OUTX_L_G = const(0x22) + OUTX_L_XL = const(0x28) + MLC_STATUS = const(0x38) - OUTX_L_G = const(0x22) - OUTX_L_XL = const(0x28) + DEFAULT_ADDR = const(0x6A) + 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.address = address # 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)) # allocate scratch buffer for efficient conversions and memread op's 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_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) - fs_accel = round(fs_accel, 2) - assert fs_gyro in ODR, "Invalid sampling rate: %d" % fs_accel - assert scale_gyro in SCALE_GYRO, "invalid gyro scaling: %d" % scale_gyro + gyro_odr = round(gyro_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 fs_accel in ODR, "Invalid sampling rate: %d" % fs_accel - assert scale_accel in SCALE_ACCEL, "invalid accelerometer scaling: %d" % scale_accel + assert accel_odr in ODR, "Invalid sampling rate: %d" % accel_odr + 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. # 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 self.__write_reg(CTRL8_XL, 0x09); # 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.scale_accel = 32768 / scale_accel + self.gyro_scale = 32768 / gyro_scale + 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): """Returns gyroscope vector in degrees/sec.""" mv = memoryview(self.scratch_int) - f = self.scale_gyro + f = self.gyro_scale self.i2c.readfrom_mem_into(self.address, OUTX_L_G, mv) return (mv[0]/f, mv[1]/f, mv[2]/f) def read_accel(self): """Returns acceleration vector in gravity units (9.81m/s^2).""" mv = memoryview(self.scratch_int) - f = self.scale_accel + f = self.accel_scale self.i2c.readfrom_mem_into(self.address, OUTX_L_XL, mv) return (mv[0]/f, mv[1]/f, mv[2]/f)