lsm6dsox: Add pedometer support

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,41 @@ def _read_reg_into(self, reg, buf):
             finally:
                 self.cs(1)
 
+    def _select_page(self, address, value=None):
+        """
+        Selects the embedded function page and reads/writes the value at the given address.
+        If value is None, it reads the value at the address. Otherwise, it writes the value to the address.
+        """
+        msb = (address >> 8) & 0x0F  # MSB is the page number
+        lsb = address & 0xFF  # LSB is the register address within the page
+
+        self.set_mem_bank(_FUNC_CFG_BANK_EMBED)
+
+        rw_bit = 0x20 if value is None else 0x40
+        # Clear both read and write bits first, then set read (bit 5) or write (bit 6).
+        self._write_reg(_PAGE_RW, (self._read_reg(_PAGE_RW) & 0x9F) | rw_bit)
+
+        # select page
+        self._write_reg(_PAGE_SEL, (msb << 4) | 0x01)
+
+        # set page addr
+        self._write_reg(_PAGE_ADDRESS, lsb)
+
+        val = None
+        if value is None:
+            # read value
+            val = self._read_reg(_PAGE_VALUE)
+        else:
+            # write value
+            self._write_reg(_PAGE_VALUE, value)
+
+        # unset page write/read and page_sel
+        self._write_reg(_PAGE_SEL, 0x01)
+        self._clear_bits(_PAGE_RW, rw_bit)
+
+        self.set_mem_bank(_FUNC_CFG_BANK_USER)
+        return val
+
     def reset(self):
         self._write_reg(_CTRL3_C, self._read_reg(_CTRL3_C) | 0x1)
         for i in range(10):
@@ -258,6 +319,47 @@ def mlc_output(self):
             self.set_mem_bank(_FUNC_CFG_BANK_USER)
         return buf
 
+    def pedometer_config(self, enable=True, debounce=10, int1_enable=False, int2_enable=False):
+        """Configure the pedometer features."""
+        self._select_page(_PEDO_DEB_STEPS_CONF, debounce)
+
+        if int1_enable:
+            self._set_bits(_MD1_CFG, _INT_EMB_FUNC_MASK)
+        if int2_enable:
+            self._set_bits(_MD2_CFG, _INT_EMB_FUNC_MASK)
+
+        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)
+
+        if int1_enable:
+            self._set_bits(_EMB_FUNC_INT1, _PEDO_INT_MASK)
+        else:
+            self._clear_bits(_EMB_FUNC_INT1, _PEDO_INT_MASK)
+
+        if int2_enable:
+            self._set_bits(_EMB_FUNC_INT2, _PEDO_INT_MASK)
+        else:
+            self._clear_bits(_EMB_FUNC_INT2, _PEDO_INT_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)
+
+    def 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)
+
     def gyro(self):
         """Returns gyroscope vector in degrees/sec."""
         mv = memoryview(self.scratch_int)

micropython/drivers/imu/lsm6dsox/lsm6dsox_pedometer.py

@@ -0,0 +1,55 @@
+"""
+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, set debounce steps to 5, and enable interrupts on both INT1 and INT2.
+# Default debounce steps 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.
+# If you just want to enable the pedometer, simply call lsm.pedometer_config(enable=True).
+lsm.pedometer_config(debounce=5, int1_enable=True, int2_enable=True)
+
+# Register interrupt handler on a Pin. e.g. D8
+# The interrupt pins are push-pull outputs by default that go low when a step is detected.
+# You can connect either INT1 or INT2 to the interrupt pin.
+interrupt_pin = Pin("D8", Pin.IN)  # Change this to your desired interrupt pin.
+interrupt_fired = False  # Flag to indicate if the interrupt has been fired.
+
+
+def on_step_detected(pin):
+    global interrupt_fired
+    interrupt_fired = True
+
+
+# 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 = None  # Keep track of the last step count to detect changes.
+
+while True:
+    if interrupt_fired:
+        print("Step detected!")
+        interrupt_fired = False  # Reset the flag after handling the interrupt.
+
+    steps = lsm.steps()
+
+    if steps != last_steps:
+        print(f"Steps: {steps}")
+        last_steps = steps
+
+    time.sleep_ms(100)