lsm6dsox: Add pedometer support.

Signed-off-by: Sebastian Romero <s.romero@arduino.cc>

Author: sebromero

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,7 +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:
     def __init__(
@@ -108,8 +126,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 +204,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 +221,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 +332,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)