Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
159 changes: 158 additions & 1 deletion micropython/drivers/imu/lsm6dsox/lsm6dsox.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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:
Expand Down Expand Up @@ -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}
Expand Down Expand Up @@ -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)
Expand All @@ -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):
Expand Down Expand Up @@ -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)
Expand Down
52 changes: 52 additions & 0 deletions micropython/drivers/imu/lsm6dsox/lsm6dsox_pedometer.py
Original file line number Diff line number Diff line change
@@ -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)
Loading