lsm6dsox: Refactor read/write into select page_function.

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

Author: sebromero

micropython/drivers/imu/lsm6dsox/lsm6dsox.py

@@ -222,55 +222,41 @@ def _read_reg_into(self, reg, buf):
             finally:
                 self.cs(1)
 
-    def _read_page(self, address):
-        msb = (address >> 8) & 0x0F
-        lsb = address & 0xFF
+    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)
 
-        # 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)
+        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)
 
-        # read value
-        val = self._read_reg(_PAGE_VALUE)
+        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 read and page_sel
-        self._clear_bits(_PAGE_RW, 0x20)
+        # 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 _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):
@@ -408,12 +394,12 @@ def pedometer_steps(self):
     @property
     def pedometer_debounce_steps(self):
         """Get the pedometer debounce steps."""
-        return self._read_page(_PEDO_DEB_STEPS_CONF)
+        return self._select_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)
+        self._select_page(_PEDO_DEB_STEPS_CONF, steps)
 
     def gyro(self):
         """Returns gyroscope vector in degrees/sec."""