diff --git a/micropython/drivers/imu/lsm6dsox/lsm6dsox.py b/micropython/drivers/imu/lsm6dsox/lsm6dsox.py index ca7b77673..683fd329c 100644 --- a/micropython/drivers/imu/lsm6dsox/lsm6dsox.py +++ b/micropython/drivers/imu/lsm6dsox/lsm6dsox.py @@ -60,6 +60,16 @@ _OUTX_L_G = const(0x22) _OUTX_L_XL = const(0x28) _MLC_STATUS = const(0x38) +_STEP_COUNTER_L = const(0x62) +_EMB_FUNC_SRC = const(0x64) + +_PAGE_SEL = const(0x02) +_PAGE_ADDRESS = const(0x08) +_PAGE_VALUE = const(0x09) +_PAGE_RW = const(0x17) + +_MD1_CFG = const(0x5E) +_MD2_CFG = const(0x5F) _DEFAULT_ADDR = const(0x6A) _WHO_AM_I_REG = const(0x0F) @@ -75,6 +85,15 @@ _EMB_FUNC_EN_A = const(0x04) _EMB_FUNC_EN_B = const(0x05) +_EMB_FUNC_INT1 = const(0x0A) +_EMB_FUNC_INT2 = const(0x0E) + +_PEDO_DEB_STEPS_CONF = const(0x0184) + +_PEDO_EN_MASK = const(0x08) +_PEDO_RST_STEP_MASK = const(0x80) +_PEDO_INT_MASK = const(0x08) +_INT_EMB_FUNC_MASK = const(0x02) class LSM6DSOX: @@ -108,8 +127,9 @@ def __init__( 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 + # allocate scratch buffers for efficient conversions and memread op's self.scratch_int = array.array("h", [0, 0, 0]) + self.scratch_2b = bytearray(2) SCALE_GYRO = {250: 0, 500: 1, 1000: 2, 2000: 3} SCALE_ACCEL = {2: 0, 4: 2, 8: 3, 16: 1} @@ -185,6 +205,12 @@ def _write_reg(self, reg, val): finally: self.cs(1) + def _set_bits(self, reg, mask): + self._write_reg(reg, self._read_reg(reg) | mask) + + def _clear_bits(self, reg, mask): + self._write_reg(reg, self._read_reg(reg) & ~mask) + def _read_reg_into(self, reg, buf): if self._use_i2c: self.bus.readfrom_mem_into(self.address, reg, buf) @@ -196,6 +222,55 @@ def _read_reg_into(self, reg, buf): finally: self.cs(1) + def _read_page(self, address): + msb = (address >> 8) & 0x0F + lsb = address & 0xFF + + self.set_mem_bank(_FUNC_CFG_BANK_EMBED) + + # Clear both read and write bits first, then set read bit (bit 5). + self._write_reg(_PAGE_RW, (self._read_reg(_PAGE_RW) & 0x9F) | 0x20) + + # select page + self._write_reg(_PAGE_SEL, (msb << 4) | 0x01) + + # set page addr + self._write_reg(_PAGE_ADDRESS, lsb) + + # read value + val = self._read_reg(_PAGE_VALUE) + + # unset page read and page_sel + self._clear_bits(_PAGE_RW, 0x20) + self._write_reg(_PAGE_SEL, 0x01) + + self.set_mem_bank(_FUNC_CFG_BANK_USER) + return val + + def _write_page(self, address, val): + msb = (address >> 8) & 0x0F + lsb = address & 0xFF + + self.set_mem_bank(_FUNC_CFG_BANK_EMBED) + + # Clear both read and write bits first, then set write bit (bit 6). + self._write_reg(_PAGE_RW, (self._read_reg(_PAGE_RW) & 0x9F) | 0x40) + + # select page + self._write_reg(_PAGE_SEL, (msb << 4) | 0x01) + + # set page addr + self._write_reg(_PAGE_ADDRESS, lsb) + + # write value + self._write_reg(_PAGE_VALUE, val) + + # unset page write and page_sel + self._write_reg(_PAGE_SEL, 0x01) + self._clear_bits(_PAGE_RW, 0x40) + + self.set_mem_bank(_FUNC_CFG_BANK_USER) + def reset(self): self._write_reg(_CTRL3_C, self._read_reg(_CTRL3_C) | 0x1) for i in range(10): @@ -258,6 +333,88 @@ def mlc_output(self): self.set_mem_bank(_FUNC_CFG_BANK_USER) return buf + @property + def pedometer_enabled(self): + """Whether the pedometer feature is enabled.""" + self.set_mem_bank(_FUNC_CFG_BANK_EMBED) + data = self._read_reg(_EMB_FUNC_EN_A) + self.set_mem_bank(_FUNC_CFG_BANK_USER) + return bool(data & _PEDO_EN_MASK) + + @pedometer_enabled.setter + def pedometer_enabled(self, enable): + """Enable or disable the pedometer feature.""" + self.set_mem_bank(_FUNC_CFG_BANK_EMBED) + if enable: + self._set_bits(_EMB_FUNC_EN_A, _PEDO_EN_MASK) + else: + self._clear_bits(_EMB_FUNC_EN_A, _PEDO_EN_MASK) + self.set_mem_bank(_FUNC_CFG_BANK_USER) + + def pedometer_reset(self): + """Reset the step counter.""" + self.set_mem_bank(_FUNC_CFG_BANK_EMBED) + self._set_bits(_EMB_FUNC_SRC, _PEDO_RST_STEP_MASK) + self.set_mem_bank(_FUNC_CFG_BANK_USER) + + @property + def pedometer_int1_enabled(self): + """Whether step detection interrupt is routed to INT1 pin.""" + self.set_mem_bank(_FUNC_CFG_BANK_EMBED) + data = self._read_reg(_EMB_FUNC_INT1) + self.set_mem_bank(_FUNC_CFG_BANK_USER) + return bool(data & _PEDO_INT_MASK) + + @pedometer_int1_enabled.setter + def pedometer_int1_enabled(self, enable): + """Route step detection interrupt to INT1 pin.""" + if enable: + self._set_bits(_MD1_CFG, _INT_EMB_FUNC_MASK) + self.set_mem_bank(_FUNC_CFG_BANK_EMBED) + self._set_bits(_EMB_FUNC_INT1, _PEDO_INT_MASK) + else: + self.set_mem_bank(_FUNC_CFG_BANK_EMBED) + self._clear_bits(_EMB_FUNC_INT1, _PEDO_INT_MASK) + self.set_mem_bank(_FUNC_CFG_BANK_USER) + + @property + def pedometer_int2_enabled(self): + """Whether step detection interrupt is routed to INT2 pin.""" + self.set_mem_bank(_FUNC_CFG_BANK_EMBED) + data = self._read_reg(_EMB_FUNC_INT2) + self.set_mem_bank(_FUNC_CFG_BANK_USER) + return bool(data & _PEDO_INT_MASK) + + @pedometer_int2_enabled.setter + def pedometer_int2_enabled(self, enable): + """Route step detection interrupt to INT2 pin.""" + if enable: + self._set_bits(_MD2_CFG, _INT_EMB_FUNC_MASK) + self.set_mem_bank(_FUNC_CFG_BANK_EMBED) + self._set_bits(_EMB_FUNC_INT2, _PEDO_INT_MASK) + else: + self.set_mem_bank(_FUNC_CFG_BANK_EMBED) + self._clear_bits(_EMB_FUNC_INT2, _PEDO_INT_MASK) + self.set_mem_bank(_FUNC_CFG_BANK_USER) + + @property + def pedometer_steps(self): + """Return the number of detected steps.""" + self.set_mem_bank(_FUNC_CFG_BANK_EMBED) + self._read_reg_into(_STEP_COUNTER_L, self.scratch_2b) + self.set_mem_bank(_FUNC_CFG_BANK_USER) + return self.scratch_2b[0] | (self.scratch_2b[1] << 8) + + @property + def pedometer_debounce_steps(self): + """Get the pedometer debounce steps.""" + return self._read_page(_PEDO_DEB_STEPS_CONF) + + @pedometer_debounce_steps.setter + def pedometer_debounce_steps(self, steps): + """Set the pedometer debounce steps. Default is 10.""" + self._write_page(_PEDO_DEB_STEPS_CONF, steps) + def gyro(self): """Returns gyroscope vector in degrees/sec.""" mv = memoryview(self.scratch_int) diff --git a/micropython/drivers/imu/lsm6dsox/lsm6dsox_pedometer.py b/micropython/drivers/imu/lsm6dsox/lsm6dsox_pedometer.py new file mode 100644 index 000000000..357acea06 --- /dev/null +++ b/micropython/drivers/imu/lsm6dsox/lsm6dsox_pedometer.py @@ -0,0 +1,52 @@ +""" +LSM6DSOX IMU Pedometer Example. + +This example demonstrates how to use the built-in pedometer feature of the LSM6DSOX IMU. +The pedometer counts the number of steps taken based on the accelerometer data +and can generate interrupts when a step is detected. + +Copyright (C) Arduino s.r.l. and/or its affiliated companies +""" + +import time +from lsm6dsox import LSM6DSOX + +from machine import Pin, I2C + +lsm = LSM6DSOX(I2C(0)) +# Or init in SPI mode. +# lsm = LSM6DSOX(SPI(5), cs=Pin(10)) + +# Enable the pedometer feature. +lsm.pedometer_enabled = True + +# Set debounce steps to 5 (default is 10). +# This means that after a step is detected, the pedometer will ignore any new steps for the next 5 step detections. +# This can help to filter out false positives and improve step counting accuracy. +lsm.pedometer_debounce_steps = 5 + +# Enable interrupts for step detection on both INT1 and INT2 pins. +lsm.pedometer_int1_enabled = True +lsm.pedometer_int2_enabled = True + +# Register interrupt handler on a Pin. e.g. D8 +interrupt_pin = Pin("D8", Pin.IN, Pin.PULL_UP) + + +def on_step_detected(pin): + print("Step detected!") + + +# Configure the interrupt pin to trigger on falling edge (active low) when a step is detected. +interrupt_pin.irq(trigger=Pin.IRQ_FALLING, handler=on_step_detected) + +last_steps = 0 # Keep track of the last step count to detect changes. + +while True: + steps = lsm.pedometer_steps + + if steps != last_steps: + print(f"Steps: {steps}") + last_steps = steps + + time.sleep_ms(100)