I could use some help setting and reading registers (on accel-gyro sensor) #13216

MrTinkerman · 2023-12-17T18:58:08Z

MrTinkerman
Dec 17, 2023

Hi all,

I'm migrating a project to a Seeed XIAO nRF52840 Sense.
This board has an integrated LSM6DS3TR-C accelerometer/gyro.

The last hurdle is getting the accel/gyro data from the sensor.
There seems to be no library so this is actually the first
time I'm going to have to work with registers.

I found these post: https://github.com/orgs/micropython/discussions/11363

So from that code I have made a start to read out accel and gyro values.
I think I'm on the right track but I'm stuck on configuring the
accel and gyro registers and reading the data.

The accel data is coming in but shows abrupt spikes when turning the board slowly.
And gyro data is all zero.

I hope someone can give me some helpful hints on this, thanks in advance.

Below you will find my code so far, screenshots from the datasheet,
the datasheet and a screenshot of those spikes/transitions.

lsm6ds3tr-c_datasheet.pdf

import machine
#from machine import Pin
import time

ADDRESS   = 0x6A  		# I2C address of LSM6DS3

CTRL1_XL  = 0x0			# Linear  acceleration sensor control register 1
CTRL2_G   = 0x1			# Angular rate sensor control register 2
CTRL3_C   = 0x2			# Control register 3
CTRL4_C   = 0x3			# Control register 4
CTRL5_C   = 0x4			# Control register 5
CTRL6_C   = 0x5			# Angular rate sensor control register 6
CTRL7_G   = 0x6			# Angular rate sensor control register 7
CTRL8_XL  = 0x7			# Linear  acceleration sensor control register 8
CTRL9_XL  = 0x8			# Linear  acceleration sensor control register 9

OUTX_L_G  = 0x22	        # Pitch axis (X) angular rate value (LSbyte)
'''
OUTX_H_G  = 0x23	        # Pitch axis (X) angular rate value (MSbyte)	# These are read in one go starting from 0x23
OUTY_L_G  = 0x24	        # Roll  axis (Y) angular rate value (LSbyte)	# and reading 6 registers to get values
OUTY_H_G  = 0x25	        # Roll  axis (Y) angular rate value (MSbyte)	# left in code for reference
OUTZ_L_G  = 0x26	        # Yaw   axis (Z) angular rate value (LSbyte)
OUTZ_H_G  = 0x27  		# Yaw   axis (Z) angular rate value (MSbyte)
'''


OUTX_L_XL = 0x28		# X-axis linear acceleration value (LSbyte)
'''
OUTX_H_XL = 0x29		# X-axis linear acceleration value (MSbyte)
OUTY_L_XL = 0x2A		# Y-axis linear acceleration value (LSbyte)
OUTY_H_XL = 0x2B		# Y-axis linear acceleration value (MSbyte)
OUTZ_L_XL = 0x2C		# Z-axis linear acceleration value (LSbyte)
OUTZ_H_XL = 0x2D		# Z-axis linear acceleration value (MSbyte)
'''

FSR_2G 	  = 16384.0      	# FSR for 2G mode
FSR_4G    = 8192.0     	        # FSR for 4G mode
FSR_8G    = 4096.0    	        # FSR for 8G mode
FSR_16G   = 2048.0         	# FSR for 16G mode


# Initialize I2C bus
i2c = machine.I2C(0, scl=machine.Pin(27), sda=machine.Pin(7))

# TODO: Configure CTRL1_XL and CTRL2_G and operating modes

# Configure accelerometer
#i2c.writeto_mem(ADDRESS, CTRL7_G , bytes([0x80]))		# CTRL7_G 1000 0000, page 67 9.19 in datasheet
i2c.writeto_mem(ADDRESS, CTRL1_XL, bytes([0x60]))		# 0x10 is 0001 0000 binary > 12.5 Hz, low power - G_HM_MODE bit in CTRL7_G (16h) should be set to 1

# Configure gyro
#i2c.writeto_mem(ADDRESS, CTRL6_C , bytes([0x10]))		# CTRL6_C 0001 0000, page 66 9.18 in datasheet
i2c.writeto_mem(ADDRESS, CTRL2_G , bytes([0x10]))		# 0x10 is 0001 0000- XL_HM_mode in CTRL6_C should be set to 1


def read_ag(FSR):
    
    data = i2c.readfrom_mem(ADDRESS, OUTX_L_XL, 6)	        # Get accelerometer data
    
    x = (data[1] << 8 | data[0]) / FSR				# Convert data to G units
    y = (data[3] << 8 | data[2]) / FSR
    z = (data[5] << 8 | data[4]) / FSR
    
    data_a = [x,y,z]
    
    print(data_a)   
    
    data = i2c.readfrom_mem(ADDRESS, OUTX_L_G, 6)		# Get gyro data
    
    #print(data)
    
    x = (data[1] << 8 | data[0])					
    y = (data[3] << 8 | data[2])				
    z = (data[5] << 8 | data[4])				
    
    data_g = [x,y,z]
    
    return [data_a, data_g]				        # Returns [[ax, ay, az], [gx, gy, gz]]
    

while True:
    
    read_ag(FSR_2G)
    #print(read_ag(FSR_2G)[0])					# Prints ax, ay, az
    #print(read_ag(FSR_2G)[1])					# Prints gx, gy, gz
    
    time.sleep_ms(100)

mendenm · 2023-12-17T19:58:40Z

mendenm
Dec 17, 2023

@MrTinkerman That looks a whole lot like sign unwrapping problem on the gyro. Look closely at the documentation for the binary representation, and see if you need to do convert an unsigned number to signed. For example, if the number returned is a 16-bit value, try x = x_raw if x_raw < 32768 else x_raw-65536 on the integer register value, before scaling it to physical units

1 reply

mendenm Dec 17, 2023

I would also make a suggestion for some useful reading if you are going to be accessing device registers directly. First, somewhere deep in the documentation for most sensors, is a section describing the output number format. Read it closely. Also, look up online a discussion of binary number representations, specifically two's-complement and sign-magnitude, which are the most likely direct outputs from most sensors. Two's complement is more common, and from what your data do, I suspect is what you are looking at. Some sensors can actually have what format selected by setting bits in config registers. A third common format is straight-offset, in which (for a sensor that reads both signs data), a constant offset is applied. For example, in a 16-bit output, zero might just be 32768, so actual_value = returned_value - 32768. This is almost as common as two's complement, and fairly easily recognized by the value floating around the middle of the allowed range with no strong input to the sensor.

gassechen · 2023-12-18T12:20:53Z

gassechen
Dec 18, 2023

Hola esto es todo lo que tengo. El Xiao se me rompió y no pude conseguir otro.
espero que esto te sirva. Saludos

# RJS ***  import machine so it can be used as a module
import machine
from machine import Pin
import time
from ubluepy import Service, Characteristic, UUID, Peripheral, constants
import ble

# ble constants 
BLE_GAP_ADV_FLAG_LE_GENERAL_DISC_MODE = const(0x02)
BLE_GAP_ADV_FLAG_BR_EDR_NOT_SUPPORTED = const(0x04)

BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE = const(
    BLE_GAP_ADV_FLAG_LE_GENERAL_DISC_MODE | BLE_GAP_ADV_FLAG_BR_EDR_NOT_SUPPORTED
)

EDDYSTONE_FRAME_TYPE_URL = const(0x10)
EDDYSTONE_URL_PREFIX_HTTP_WWW = const(0x00)  # "http://www".
EDDYSTONE_URL_SUFFIX_DOT_COM = const(0x01)  # ".com"
EDDYSTONE_FRAME_TYPE_TLM = const(0x20)
# end ble constants

CTRL1_XL = 0x10  # Accelerometer control register
CTRL2_G = 0x11
CTRL3_C = 0x12
FIFO_DATA_OUT_L = 0x3E
TIMESTAMP0 = 0x49
STEP_COUNTER_L = 0x4B
STEP_COUNTER_H = 0x4C


ADDRESS = 0x6A  # I2C address of LSM6DS3

TAP_CFG1 = 0x58
TAP_THS_6D = 0x59
INT_DUR2 = 0x5A
WAKE_UP_THS = 0x5B
MD1_CFG = 0x5E
FREE_FALL = 0x5D 
WAKE_UP_DUR = 0x5C
CTRL8_XL = 0x17

CTRL10_C = 0x19
OUTX_L_XL = 0x28
OUTY_L_XL = 0x2A
OUTZ_L_XL = 0x2C
CTRL9_XL = 0x18
TILT_IA = 0x3A
FUNC_SRC1 = 0x53
TIMESTAMP2 = 0x40
TIMESTAMP1 = 0x41
TIMESTAMP0 = 0x42

FIFO_CTRL1 = 0x06
FIFO_CTRL2 = 0x07
FIFO_CTRL3 = 0x08
FIFO_CTRL4 = 0x09
FIFO_CTRL5 = 0x0A
FIFO_STATUS1 = 0x3A
FIFO_STATUS2 = 0x3B
OUT_TEMP_L = 0x20
OUT_X_L_G = 0x22
OUT_X_L_XL = 0x28
STEP_COUNTER_L = 0x4B
TIMESTAMP0 = 0x40
TIMESTAMP1 = 0x41
TIMESTAMP2 = 0x42
FIFO_SRC = 0x3F
FUNC_CFG_ACCESS = 0x01
WHO_AM_I = 0x07
# acceleromter scale
FSR_2G = 16384.0  # FSR for 2G mode
FSR_4G = 8192.0   # FSR for 4G mode
FSR_8G = 4096.0   # FSR for 8G mode
FSR_16G = 2048.0  # FSR for 16G mode


step_count = 0

# Initialize I2C bus
i2c = machine.I2C(0, scl=machine.Pin(27), sda=machine.Pin(7))

who_am_i = i2c.readfrom_mem(ADDRESS, 0x0F, 1)
print("Device detected with WHO_AM_I value: ", who_am_i[0])

# RJS *** declare pin for LED
led = Pin(26, Pin.OUT)

time.sleep_ms(50)
interruptCount = 0
prevInterruptCount = 0

def accelerometer_power_off():
    i2c.writeto_mem(ADDRESS, CTRL1_XL, bytes([0x00]))

def gyro_power_off():
    i2c.writeto_mem(ADDRESS, CTRL2_G, bytes([0x00]))

def power_off_master():
    # Power-down mode for the master and the embedded functionalities
    i2c.writeto_mem(ADDRESS, CTRL3_C, bytes([0x00]))

# Function to set the power mode of the LSM6DS3TR-C
def set_power_mode(mode):
    if mode == 'Power-Down':
        # CTRL3_C register: set all sensors in Power-Down mode
        i2c.writeto_mem(ADDRESS, CTRL3_C, bytes([0x00])) 
    elif mode == 'Low-Power':
        # CTRL3_C register: enable accelerometer/gyroscope Low-Power mode
        i2c.writeto_mem(ADDRESS, CTRL3_C, bytes([0x01]))
        i2c.writeto_mem(ADDRESS, CTRL1_XL, bytes([0x40]))
    elif mode == 'Normal':
        # CTRL3_C register: enable accelerometer/gyroscope Normal mode
        i2c.writeto_mem(ADDRESS, CTRL3_C, bytes([0x02]))
        # CTRL1_XL register: ODR_XL = 104 Hz, FS_XL = +/-2g, BW_XL = 50 Hz
        i2c.writeto_mem(ADDRESS, CTRL1_XL, bytes([0x60]))
        # CTRL2_G register: ODR_G = 104 Hz, FS_G = +/-250 dps
        i2c.writeto_mem(ADDRESS, CTRL2_G, bytes([0x68])) 


    elif mode == 'High-Performance':
        # CTRL3_C register: enable accelerometer/gyroscope High-Performance mode
        i2c.writeto_mem(ADDRESS, CTRL3_C, bytes([0x03])) 


def read_accelerometer_data(FSR):
    # Get accelerometer data
    data = i2c.readfrom_mem(ADDRESS, OUTX_L_XL, 6)
    # Convert data to G units
    x = (data[1] << 8 | data[0]) / FSR
    y = (data[3] << 8 | data[2]) / FSR
    z = (data[5] << 8 | data[4]) / FSR
    # Output values
    print('X: {} G, Y: {} G, Z: {} G'.format(x, y, z))


# Configure single tap detection
def configure_single_tap_detection():
    i2c.writeto_mem(ADDRESS, CTRL1_XL, bytes([0x60]))
    i2c.writeto_mem(ADDRESS, TAP_CFG1, bytes([0x8E])) 
    i2c.writeto_mem(ADDRESS, TAP_THS_6D, bytes([0x09]))
    i2c.writeto_mem(ADDRESS, INT_DUR2, bytes([0x06]))
    i2c.writeto_mem(ADDRESS, WAKE_UP_THS, bytes([0x00]))  # single and double tap
    i2c.writeto_mem(ADDRESS, MD1_CFG, bytes([0x40]))  # double tap to INT1



# Configure double tap detection
def configure_double_tap_detection():
    i2c.writeto_mem(ADDRESS, CTRL1_XL, bytes([0x60]))
    i2c.writeto_mem(
        ADDRESS, TAP_CFG1, bytes([0x8E])
    )  # enable interrupt(0x80), tap xyz (0x0E)
    i2c.writeto_mem(ADDRESS, TAP_THS_6D, bytes([0x8C]))
    i2c.writeto_mem(ADDRESS, INT_DUR2, bytes([0x7F]))
    i2c.writeto_mem(ADDRESS, WAKE_UP_THS, bytes([0x80]))  # single and double tap
    i2c.writeto_mem(ADDRESS, MD1_CFG, bytes([0x08]))  # double tap to INT1


# Configure free fall detection
def configure_free_fall_detection():
    i2c.writeto_mem(ADDRESS, CTRL1_XL, bytes([0x60]))
    i2c.writeto_mem(ADDRESS, TAP_CFG1, bytes([0x81])) 
    i2c.writeto_mem(ADDRESS, FREE_FALL, bytes([0x7F]))
    i2c.writeto_mem(ADDRESS, WAKE_UP_THS, bytes([0x33])) 
    i2c.writeto_mem(ADDRESS, MD1_CFG, bytes([0x10]))  # double tap to INT1


# Configure wake-up detection
def configure_wake_up_detection():
    #no duration
    i2c.writeto_mem(ADDRESS, WAKE_UP_DUR, bytes([0x00])) 
    #Set wake-up threshold
    i2c.writeto_mem(ADDRESS, WAKE_UP_THS, bytes([0x02]))
    #Enable interrupts and apply slope filter; latch mode disabled
    i2c.writeto_mem(ADDRESS, TAP_CFG1, bytes([0x80]))
    #Turn on the accelerometer  ODR_XL = 833 Hz, FS_XL = ±2 g
    i2c.writeto_mem(ADDRESS, CTRL1_XL, bytes([0x70]))
    
    time.sleep_ms(4) #Insert (reduced) wait time
    #ODR_XL = 12.5 Hz
    i2c.writeto_mem(ADDRESS, CTRL1_XL, bytes([0x10]))
    
    #Wake-up interrupt driven to INT1 pin
    i2c.writeto_mem(ADDRESS, MD1_CFG, bytes([0x20]))
    

# Configure 6d orientation detection
def configure_6D_orientation_detection():
    #Turn on the accelerometer  ODR_XL = 416 Hz, FS_XL = ±2 g
    i2c.writeto_mem(ADDRESS, CTRL1_XL, bytes([0x60]))
    #Enable interrupts and apply slope filter; latch mode disabled
    i2c.writeto_mem(ADDRESS, TAP_CFG1, bytes([0x80]))
    
    i2c.writeto_mem(ADDRESS, TAP_THS_6D, bytes([0x40]))
    # Enable LPF2 filter to 6D functionality
    i2c.writeto_mem(ADDRESS, CTRL8_XL, bytes([0x01])) 
    
    #6D interrupt driven to INT1 pin
    i2c.writeto_mem(ADDRESS, MD1_CFG, bytes([0x04])) 


# Configure 4d orientation detection
def configure_4D_orientation_detection():
    #Turn on the accelerometer  ODR_XL = 416 Hz, FS_XL = ±2 g
    i2c.writeto_mem(ADDRESS, CTRL1_XL, bytes([0x60]))
    #Enable interrupts and apply slope filter; latch mode disabled
    i2c.writeto_mem(ADDRESS, TAP_CFG1, bytes([0x80]))
    
    i2c.writeto_mem(ADDRESS, TAP_THS_6D, bytes([0xC0]))
    # Enable LPF2 filter to 6D functionality
    i2c.writeto_mem(ADDRESS, CTRL8_XL, bytes([0x01])) 
    
    #6D interrupt driven to INT1 pin
    i2c.writeto_mem(ADDRESS, MD1_CFG, bytes([0x04])) 

def configure_Activity_Inactivity_detection():
    #turn on thr accelerometer
    i2c.writeto_mem(ADDRESS, CTRL1_XL, bytes([0x50]))
    #turn on the gyro
    i2c.writeto_mem(ADDRESS, CTRL2_G, bytes([0x40]))
    #Set duration for Inactivity detection
    i2c.writeto_mem(ADDRESS, WAKE_UP_DUR, bytes([0x02]))
    #Set Activity/Inactivity threshold
    i2c.writeto_mem(ADDRESS, WAKE_UP_THS, bytes([0x02]))
    #Enable interrupts//nactivity configuration: acc to 12.5 LP, gyro to Power-Down
    i2c.writeto_mem(ADDRESS, TAP_CFG1, bytes([0xE0]))
    #Activity/Inactivity interrupt driven to INT1
    i2c.writeto_mem(ADDRESS, MD1_CFG, bytes([0x80])) 

    
def get_acceleration():
    # Read X-axis acceleration from accelerometer
    x_lsb = i2c.readfrom_mem(ADDRESS, OUTX_L_XL, 1)[0]
    x_accel = (int(x_lsb) * 9.81) / (256 * 4)
    y_lsb = i2c.readfrom_mem(ADDRESS, OUTY_L_XL, 1)[0]
    y_accel = (int(y_lsb) * 9.81) / (256 * 4)
    z_lsb = i2c.readfrom_mem(ADDRESS, OUTZ_L_XL, 1)[0]
    z_accel = (int(z_lsb) * 9.81) / (256 * 4)
    
    return x_accel,y_accel,z_accel

def count_steps(threshold):
    # Initialize variables
    step_count = 0
    step_detected = False
    last_step_time = 0
    
    # Main loop
    while True:
        x_accel = get_acceleration()
        
        # Check if step is detected
        if x_accel > threshold and not step_detected and time.ticks_ms() - last_step_time > 100:
            step_detected = True
            last_step_time = time.ticks_ms()
        
        # Check if step is completed
        if step_detected and x_accel < threshold:
            step_detected = False
            step_count += 1
        
        # Print step count
        print("Step count: {}".format(step_count))
        
        # Delay for 10ms
        time.sleep_ms(10)
        
def configure_tilt_detection():
        
        i2c.writeto_mem(ADDRESS, CTRL1_XL, bytes([0x60]))
        # Enable the tilt detection function
        i2c.writeto_mem(ADDRESS, CTRL10_C, bytes([0x0C]))
        i2c.writeto_mem(ADDRESS, TAP_CFG1, bytes([0x02]))
        
        


def tilt_detected():
    # Check if the tilt interrupt signal is set
    tilt_interrupt = i2c.readfrom_mem(ADDRESS, FUNC_SRC1, 1)
    if tilt_interrupt[0] & 0x20:
        print("Tilt detected!")
        # Clear the tilt interrupt request
        i2c.readfrom_mem(ADDRESS, TILT_IA, 1)
        time.sleep(0.1) # Wait for a short period to avoid repeated interrupts
        
def configure_timestamp():
        # LSM6DS3TRC accelerometer initialisation
        # Set accelerometer ODR to 208Hz and FS to ±2g
        i2c.writeto_mem(ADDRESS, CTRL1_XL, bytes([0x50]))
        # Set timestamp resolution to 25 µs
        i2c.writeto_mem(ADDRESS, WAKE_UP_DUR, bytes([0x10]))
        # Enable timestamp count
        i2c.writeto_mem(ADDRESS, CTRL10_C, bytes([0x20]))
        # Enable end counter interrupt driven to INT1
        i2c.writeto_mem(ADDRESS, MD1_CFG, bytes([0x01]))  

def reset_timestamp():
    # Reset timer counter
    i2c.writeto_mem(ADDRESS, TIMESTAMP_REG2, bytes([0xAA]))
    
def read_timestamp():
    timestamp2 = i2c.readfrom_mem(ADDRESS, TIMESTAMP2, 1)[0] << 16
    timestamp1 = i2c.readfrom_mem(ADDRESS, TIMESTAMP1, 1)[0] << 8
    timestamp0 = i2c.readfrom_mem(ADDRESS, TIMESTAMP0, 1)[0]
    timestamp = timestamp2 | timestamp1 | timestamp0
    print("Timestamp: {}".format(timestamp2))
    

def set_fifo_mode(mode):
    # Read current FIFO_CTRL5 register value
    reg_val = i2c.readfrom_mem(ADDRESS, 0x0A, 1)[0]
    
    # Clear FIFO_MODE_[2:0] bits
    reg_val &= ~0x07
    
    # Set FIFO mode bits according to the input mode
    if mode == "Bypass":
        reg_val |= 0x00
    elif mode == "FIFO":
        reg_val |= 0x01
    elif mode == "Stream":
        reg_val |= 0x02
    elif mode == "Stream-to-FIFO":
        reg_val |= 0x03
    elif mode == "Bypass-to-Stream":
        reg_val |= 0x04
    
    # Write modified value back to register
    i2c.writeto_mem(ADDRESS, 0x0A, bytearray([reg_val]))


def get_fifo_mode():
    # Read FIFO_CTRL5 register
    fifo_ctrl5 = i2c.readfrom_mem(ADDRESS, 0x3E, 1)[0]

    # Parse FIFO mode information
    fifo_mode = fifo_ctrl5 & 0b00000111

    # Return FIFO mode as a string
    if fifo_mode == 0:
        return 'Bypass'
    elif fifo_mode == 1:
        return 'FIFO'
    elif fifo_mode == 3:
        return 'Stream'
    elif fifo_mode == 4:
        return 'Stream-to-FIFO'
    elif fifo_mode == 6:
        return 'Bypass-to-Stream'
    else:
        return 'Unknown'


def enable_fifo():
    # Enable the accelerometer
    i2c.writeto_mem(ADDRESS, CTRL1_XL, bytes([0x50]))
    #FIFO enable
    i2c.writeto_mem(ADDRESS, CTRL3_C, bytes([0x04]))
    # Enable the timestamp and step counter
    i2c.writeto_mem(ADDRESS, CTRL10_C, bytes([0x20]))
    i2c.writeto_mem(ADDRESS, FUNC_CFG_ACCESS, bytes([0x04]))
    i2c.writeto_mem(ADDRESS, FIFO_CTRL5, bytes([0x06]))


def clear_fifo():
    # Write 0b00000100 to FIFO_CTRL5 register to set FIFO_MODE = Bypass
    i2c.writeto_mem(ADDRESS, WHO_AM_I, bytes([0x04]))

    # Write 0b00010000 to FIFO_CTRL5 register to set FIFO_MODE = FIFO
    i2c.writeto_mem(ADDRESS, WHO_AM_I, bytes([0x10]))



def get_fifo_status():
    # Read FIFO_STATUS1 and FIFO_STATUS2 registers
    fifo_status1 = i2c.readfrom_mem(ADDRESS, 0x3A, 1)[0]
    fifo_status2 = i2c.readfrom_mem(ADDRESS, 0x3B, 1)[0]

    # Parse FIFO status information
    fifo_overflow = (fifo_status1 & 0b10000000) >> 7
    fifo_full = (fifo_status1 & 0b01000000) >> 6
    fifo_empty = (fifo_status1 & 0b00100000) >> 5
    fifo_pattern = fifo_status2 & 0b00111111

    # Return FIFO status as a dictionary
    return {'overflow': fifo_overflow, 'full': fifo_full, 'empty': fifo_empty, 'pattern': fifo_pattern}

def set_fifo_watermark(watermark):
    # Write the FIFO watermark value to the FIFO_CTRL1 register
    fifo_ctrl1 = bytearray([0])
    i2c.readfrom_mem_into(ADDRESS, FIFO_CTRL1, fifo_ctrl1)
    fifo_ctrl1[0] &= ~0b00111111  # Clear the FIFO watermark bits
    fifo_ctrl1[0] |= (watermark & 0b00111111)  # Set the FIFO watermark bits
    i2c.writeto_mem(ADDRESS, FIFO_CTRL1, fifo_ctrl1)

def get_fifo_watermark():
    # Read FIFO_CTRL1 register
    fifo_ctrl1 = i2c.readfrom_mem(ADDRESS, 0x06, 1)[0]

    # Parse FIFO watermark information
    fifo_watermark = fifo_ctrl1 & 0b00111111

    # Return FIFO watermark as an integer
    return fifo_watermark

def read_fifo_data():
    # Read the FIFO status to check if it has data
    fifo_status = get_fifo_status()
    if fifo_status['empty']:
        return None
    
    # Read the number of bytes in the FIFO
    fifo_length = i2c.readfrom_mem(0x6A, 0x3C, 2)
    fifo_length = (fifo_length[1] << 8) | fifo_length[0]
    print(fifo_length)
    # Read the FIFO data
    fifo_data = bytearray(i2c.readfrom_mem(0x6A, 0x3E, 16))

    # Parse the FIFO data to extract the pedometer and timestamp data
    pedometer_data = []
    timestamp_data = []
    i = 0
    while i < len(fifo_data):
        if fifo_data[i] == 0b11110000:
            # Pedometer data
            pedometer_data.append({
                'step_count': (fifo_data[i+2] << 16) | (fifo_data[i+1] << 8) | fifo_data[i+0],
                'walk_time': (fifo_data[i+5] << 16) | (fifo_data[i+4] << 8) | fifo_data[i+3]
            })
            i += 6
        elif fifo_data[i] == 0b11100000:
            # Timestamp data
            timestamp_data.append({
                'timestamp': (fifo_data[i+3] << 16) | (fifo_data[i+2] << 8) | fifo_data[i+1],
                'timestamp_overflow': fifo_data[i+4]
            })
            i += 5
        else:
            # Unknown data
            break
    
    print(pedometer_data)
    # Clear the FIFO
    

    # Return the pedometer and timestamp data
    return {'pedometer': pedometer_data, 'timestamp': timestamp_data}

# def read_fifo_data(fifo_mode):
#     fifo_data = []
#     
#     # Check if FIFO is empty
#     fifo_status = get_fifo_status()
#     print(fifo_status)
#     if fifo_status['empty']:
#         return {'fifo_data': fifo_data, 'timestamp': None, 'step_counter': None}
# 
#     # Read FIFO data
#     fifo_size = 0
#     if fifo_mode == 'bypass':
#         fifo_size = 6
#     elif fifo_mode == 'fifo':
#         fifo_size = 18
#     fifo_bytes = i2c.readfrom_mem(0x6A, 0x3E, fifo_size)
# 
#     # Parse FIFO data and extract timestamp and step counter
#     for i in range(0, fifo_size, 6):
#         x_l = fifo_bytes[i]
#         x_h = fifo_bytes[i+1]
#         y_l = fifo_bytes[i+2]
#         y_h = fifo_bytes[i+3]
#         z_l = fifo_bytes[i+4]
#         z_h = fifo_bytes[i+5]
# 
#         x = (x_h << 8) | x_l
#         y = (y_h << 8) | y_l
#         z = (z_h << 8) | z_l
# 
#         fifo_data.append({'x': x, 'y': y, 'z': z})
#     
#     timestamp2 = i2c.readfrom_mem(0x6A, 0x40, 1)[0] << 16
#     timestamp1 = i2c.readfrom_mem(0x6A, 0x41, 1)[0] << 8
#     timestamp0 = i2c.readfrom_mem(0x6A, 0x42, 1)[0]
#     timestamp = (timestamp2 | timestamp1 | timestamp0) / 1000.0
#     
#     step_counter_l = i2c.readfrom_mem(0x6A, 0x4B, 1)[0]
#     step_counter_h = i2c.readfrom_mem(0x6A, 0x4C, 1)[0]
#     step_counter = (step_counter_h << 16) | step_counter_l
# 
#     return {'fifo_data': fifo_data, 'timestamp': timestamp, 'step_counter': step_counter}
# 


def configure_step_counter_and_timestamp():
    # Enable step counter and timestamp functions
    #i2c.writeto_mem(0x6A, 0x10, bytes([0xAC]))
    # Configure LSM6DS3TR-C for pedometer mode
    # set accelerometer full scale range to ±2G
    i2c.writeto_mem(ADDRESS, CTRL1_XL, bytes([0x50]))
    # enable step detection, set threshold to 6LSBs,
    # set minimum time between steps to 100ms
    i2c.writeto_mem(ADDRESS, CTRL9_XL, bytes([0x40]))
    #i2c.writeto_mem(ADDRESS, FIFO_CTRL1, b'\x3F')
    # store counter step and timestamp data in FIFO
    i2c.writeto_mem(ADDRESS, FIFO_CTRL2, bytes([0x01]))
    #no store gyro and accelerometer data in FIFO
    i2c.writeto_mem(ADDRESS, FIFO_CTRL3, bytes([0x00]))
    # decimation, only store step counter, four register decimation none
    i2c.writeto_mem(ADDRESS, FIFO_CTRL4, bytes([0x38]))
    # Set FIFO mode to Stream mode
    set_fifo_mode('fifo')

    # Set FIFO watermark to 256
    set_fifo_watermark(32)

    # Enable FIFO
    enable_fifo()

def configure_pedometer_step():
    # Configure LSM6DS3TR-C for pedometer mode
    # set accelerometer full scale range to ±2G
    i2c.writeto_mem(ADDRESS, CTRL1_XL, bytes([0x50]))
    # enable step detection, set threshold to 6LSBs,
    # set minimum time between steps to 100ms
    i2c.writeto_mem(ADDRESS, CTRL9_XL, bytes([0x38]))
    
    # store counter step and timestamp data in FIFO
    i2c.writeto_mem(ADDRESS, FIFO_CTRL2, bytes([0x80]))
    #no store gyro and accelerometer data in FIFO
    i2c.writeto_mem(ADDRESS, FIFO_CTRL3, bytes([0x00]))
    # decimation, only store step counter, four register decimation none
    i2c.writeto_mem(ADDRESS, FIFO_CTRL4, bytes([0x38]))

# Define tap_interrupt function to handle tap events
# RJS *** pin interrupt handler routine needs one argument- the pin
#         your original had a pin argument
def event_interrupt(pin):
    global interruptCount
    interruptCount = interruptCount + 100
    print("Tap detected")
    print(interruptCount)
    # RJS *** off and on are functions (see uPy documentation)
    #         and XIAO has inverted logic on board LED (high is off)
    #         again your original had the right usage .
    led.off()  # Turn on the LED... on/off are functions
    # led wiring on XIAO is inverted
    # same as led.value(0)
    time.sleep_ms(50)  # Wait for a short time
    led.on()  # Turn off the LED
    # sane as led.value(1)
    #a=read_fifo1()
    #print(a)
    update_adv(interruptCount)
    
    

    #print(read_fifo_data())
    

def enable_fifo1():
        # Enable FIFO
        i2c.writeto_mem(ADDRESS, 0x18, bytes([0x40]))

def set_fifo_mode1(mode):
        if mode == "bypass":
            i2c.writeto_mem(ADDRESS, FIFO_CTRL5, bytes([0x00]))
        elif mode == "fifo":
            i2c.writeto_mem(ADDRESS, FIFO_CTRL5, bytes([0x40]))
        elif mode == "stream":
            i2c.writeto_mem(ADDRESS, FIFO_CTRL5, bytes([0x80]))
        elif mode == "trigger":
            i2c.writeto_mem(ADDRESS, FIFO_CTRL5, bytes([0xC0]))
        else:
            raise ValueError("Invalid FIFO mode")


def clear_fifo1():
        i2c.writeto_mem(ADDRESS, FIFO_CTRL5, bytes([0x01]))


def set_fifo_watermark1(value):
        value= value & 0xFF 
        i2c.writeto_mem(ADDRESS, FIFO_CTRL2, bytes([value]))
        
def get_fifo_status1():
        status1=i2c.readfrom_mem(ADDRESS, FIFO_STATUS1, 1)[0]
        status2=i2c.readfrom_mem(ADDRESS, FIFO_STATUS2, 1)[0]
        return (status1, status2)

def read_fifo1():
        fifo_status = get_fifo_status1()
        fifo_length = (fifo_status[0] & 0x3F) | (fifo_status[1] << 6)
        data = bytearray(i2c.readfrom_mem(ADDRESS, FIFO_DATA_OUT_L,fifo_length * 2 ))
        
        #print(data)
        #self._read_bytes(self.fifo_data_out_l, fifo_length * 2)
        samples = []
        for i in range(0, len(data), 2):
            x = data[i] | (data[i+1] << 8)
            samples.append(x)
        return samples


# ble functions


def string_to_binarray(text):
    b = bytearray([])
    for c in text:
        b.append(ord(c))
    return b


def gen_ad_type_content(ad_type, data):
    b = bytearray(1)
    b.append(ad_type)
    b.extend(data)
    b[0] = len(b) - 1
    return b
def string_to_binarray(text):
    b = bytearray([])
    for c in text:
        b.append(ord(c))
    return b


def gen_ad_type_content(ad_type, data):
    b = bytearray(1)
    b.append(ad_type)
    b.extend(data)
    b[0] = len(b) - 1
    return b



def generate_eddystone_adv_packet(data):
    # flags
    disc_mode = bytearray([BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE])
    packet_flags = gen_ad_type_content(constants.ad_types.AD_TYPE_FLAGS, disc_mode)

    # 16-bit uuid
    uuid = bytearray([0xAA, 0xFE])
    packet_uuid16 = gen_ad_type_content(
        constants.ad_types.AD_TYPE_16BIT_SERVICE_UUID_COMPLETE, uuid
    )

    # eddystone data
    rssi = 0xEE  # -18 dB, approx signal strength at 0m.
    eddystone_data = bytearray([])
    eddystone_data.append(EDDYSTONE_FRAME_TYPE_TLM)  # use TLM frame type
    eddystone_data.extend(data)  # add custom data payload

    # service data
    service_data = uuid + eddystone_data
    packet_service_data = gen_ad_type_content(
        constants.ad_types.AD_TYPE_SERVICE_DATA, service_data
    )

    # generate advertisement packet
    packet = bytearray([])
    packet.extend(packet_flags)
    packet.extend(packet_uuid16)
    packet.extend(packet_service_data)

    return packet

def update_adv(data):
    #adv_packet = generate_eddystone_adv_packet("0123456789")
    ble.disable()
    time.sleep_ms(10)
    ble.enable()
    adv_packet = generate_eddystone_adv_packet(str(data))
    p.advertise(data=adv_packet, connectable=False)
# end ble fucntions



machine.enable_irq()

#select interrupt mode 
#configure_double_tap_detection()
#configure_free_fall_detection()
configure_single_tap_detection()
#configure_wake_up_detection()
#configure_6D_orientation_detection()
#configure_4D_orientation_detection()
#configure_pedometer_step()
#configure_tilt_detection()
#configure_timestamp()
#configure_Activity_Inactivity_detection()
#configure_step_counter_and_timestamp()


tap_pin = Pin(11, Pin.IN, Pin.PULL_UP)
tap_pin.irq(trigger=Pin.IRQ_RISING, handler=event_interrupt)

#clear_fifo()

devicename="MTK_SENSE"
p = Peripheral()
p.advertise(device_name=devicename)
#update_adv(0) #inicialize in zero


# while True:
#     #read_accelerometer_data(FSR_2G)
#     #read_timestamp()
#     #clear_fifo1()
#     #ble.disable()
#     
#     
#     print(get_fifo_status1())
#     set_fifo_watermark1(16)
#     set_fifo_mode1("stream")
#     enable_fifo1()
#     
#     if interruptCount > prevInterruptCount:
#         print("Interrupt received!")
# 
#     prevInterruptCount = interruptCount
#     print("Interrupt Counter: " + str(prevInterruptCount))
#     #ble.disable()
#     time.sleep_ms(50)
#     
#     interruptCount=interruptCount+1
#     set_power_mode("Normal")

1 reply

gassechen Dec 18, 2023

accelerometer_power_off(): Powers off the accelerometer by writing a specific value to the control register.
gyro_power_off(): Powers off the gyroscope by writing a specific value to the control register.
power_off_master(): Powers down the master and embedded functionalities by writing a specific value to the control register.
set_power_mode(mode): Sets the power mode of the LSM6DS3TR-C based on the given mode parameter, which can be 'Power-Down', 'Low-Power', 'Normal', or 'High-Performance'.
read_accelerometer_data(FSR): Reads and prints the accelerometer data in G units based on the given full-scale range (FSR).
configure_single_tap_detection(): Configures the accelerometer for single tap detection.
configure_double_tap_detection(): Configures the accelerometer for double tap detection.
configure_free_fall_detection(): Configures the accelerometer for free fall detection.
configure_wake_up_detection(): Configures the accelerometer for wake-up detection.
configure_6D_orientation_detection(): Configures the accelerometer for 6D orientation detection.
configure_4D_orientation_detection(): Configures the accelerometer for 4D orientation detection.
configure_Activity_Inactivity_detection(): Configures the accelerometer and gyro for activity/inactivity detection.
get_acceleration(): Reads and returns the X, Y, and Z-axis acceleration values from the accelerometer.
count_steps(threshold): Counts the number of steps based on the accelerometer data and a given threshold.
configure_tilt_detection(): Configures the accelerometer for tilt detection.
tilt_detected(): Checks if a tilt is detected and prints a message.
configure_timestamp(): Configures the accelerometer for timestamp functionality.
reset_timestamp(): Resets the timestamp counter.
read_timestamp(): Reads and prints the timestamp value.
set_fifo_mode(mode): Sets the FIFO mode of the accelerometer based on the given mode parameter.
get_fifo_mode(): Reads and returns the current FIFO mode.
enable_fifo(): Enables the FIFO for the accelerometer.
clear_fifo(): Clears the FIFO for the accelerometer.
get_fifo_status(): Reads and returns the status of the FIFO (overflow, full, empty, pattern).
set_fifo_watermark(watermark): Sets the FIFO watermark level.
get_fifo_watermark(): Reads and returns the current FIFO watermark level.
read_fifo_data(): Reads and prints the data from the FIFO.
configure_step_counter_and_timestamp(): Configures the accelerometer for step counter and timestamp functionality.
configure_pedometer_step(): Configures the accelerometer for pedometer mode.
configure_tap_detection(): Configures the accelerometer for tap detection.
event_interrupt(pin): Interrupt handler function for tap events.
enable_fifo1(): Enables the FIFO for the accelerometer (alternative implementation).
set_fifo_mode1(mode): Sets the FIFO mode of the accelerometer (alternative implementation).
clear_fifo1(): Clears the FIFO for the accelerometer (alternative implementation).
set_fifo_watermark1(value): Sets the FIFO watermark level (alternative implementation).
get_fifo_status1(): Reads and returns the status of the FIFO (alternative implementation).
read_fifo1(): Reads and returns the data from the FIFO (alternative implementation).
generate_eddystone_adv_packet(data): Generates an Eddystone advertisement packet with custom data.
update_adv(data): Disables and re-enables BLE, then updates the BLE advertisement data with the given data.

These functions handle various configurations, data reading, and BLE advertisement for the LSM6DS3TR-C accelerometer and gyro sensor.

MrTinkerman · 2023-12-18T19:54:14Z

MrTinkerman
Dec 18, 2023
Author

@mendenm :
Thanks for giving some pointers on how to get into this,
it does seem to be more complicated then I would have thought though.

@gassechen :
Thanks so much for posting that code. I hope to learn from it when going through it.

0 replies

MrTinkerman · 2023-12-21T14:02:58Z

MrTinkerman
Dec 21, 2023
Author

I ended up adapting: https://github.com/wrh2/LSM6DS3/blob/master/src/lsm6ds3.py for use with uPy.

And mendenm, you were right on the 'spikes/jumps'.

This does the trick for me now:

            gyro_x = self.__read_reg(self.regs['OUTX_L_G'], b=2)
            gyro_y = self.__read_reg(self.regs['OUTY_L_G'], b=2)
            gyro_z = self.__read_reg(self.regs['OUTZ_L_G'], b=2)
            
            g = [(gyro_x[1] << 8) | gyro_x[0],
                 (gyro_y[1] << 8) | gyro_y[0],
                 (gyro_z[1] << 8) | gyro_z[0]]
            
            for i, val in enumerate(g):
                if val>32_768: g[i] = 65536 - val
            
            return g

Thanks to both of you for your help

2 replies

mendenm Dec 21, 2023

I think you want if val>32_768: g[i] = val - 65536 instead of if val>32_768: g[i] = 65536 - val, since the 'large' numbers should be negative. In the conversion from unsigned to signed twos-complement, the value 65535 (unsigned) really means -1. So 65535 - 65536 = -1. It is possible your numbers are encoded strangely, but the plots you show make what I suggest appears likely.

MrTinkerman Dec 21, 2023
Author

Thanks for pointing it out, i still have to read up on the topic though.

In my current code both 'val - 65536' and '65536 - val' work fine.
I'm working with absolute values and running averages though.

A classic case of:
"It's annoying if you don't know why your code doesn't work,
but it's kind of concerning if you don't know why your code does work..."

MicroPython

I could use some help setting and reading registers (on accel-gyro sensor) #13216

Uh oh!

Uh oh!

MrTinkerman Dec 17, 2023

Replies: 4 comments · 4 replies

Uh oh!

Uh oh!

mendenm Dec 17, 2023

Uh oh!

Uh oh!

mendenm Dec 17, 2023

Uh oh!

gassechen Dec 18, 2023

Uh oh!

gassechen Dec 18, 2023

Uh oh!

MrTinkerman Dec 18, 2023 Author

Uh oh!

MrTinkerman Dec 21, 2023 Author

Uh oh!

mendenm Dec 21, 2023

Uh oh!

MrTinkerman Dec 21, 2023 Author

MrTinkerman
Dec 17, 2023

Replies: 4 comments 4 replies

mendenm
Dec 17, 2023

gassechen
Dec 18, 2023

MrTinkerman
Dec 18, 2023
Author

MrTinkerman
Dec 21, 2023
Author

MrTinkerman Dec 21, 2023
Author