LSM6DSV16X IMU sensor fusion not working via FIFO

HassaanSaleem-101 · ‎2025-05-12

Hi,
i'm currently working on the STMH747I-Disco board, and integrating the STEVALSMKI227AA, and within using the LSM6DSV16X IMU sensor fusion to get the game rotation vector data via FIFO. The problem is that we are ready the status of the fifo 0x00 as a result we are the getting the value of the fifo tag also as 0x00, even though we have configured the sensor fusion as well as the configurations for the fifo.

We are getting the value of Gyro and Accelerometer through their dedicated registers, however, we are not getting anything through the FIFO. I'm sharing with you my code and its output, and as you can see, we made all the configurations related to them. If anyone has managed to get this working or knows the correct steps, I'd really appreciate your guidance. I really appreciate any help you can provide.

HassaanSaleem-101 · ‎2025-05-12

The Code:
#include "main.h"

#define LSM6DSV16X_ADDR 0xD4 // 0xD4

#define WHO_AM_I_REG 0x0F

#define EXPECTED_WHO_AM_I 0x6C

#define DEVICE_ADDRESS 0x6A // Confirmed address (0xD4 >> 1)

#define REG_WHO_AM_I 0x0F

#define REG_CTRL1_XL 0x10 // accelo

#define REG_CTRL2_G 0x11 // gyro

#define REG_CTRL3_C 0x12 // BDU (block data update)

#define REG_CTRL6_C 0x15 // gyro full scale

#define REG_CTRL8_XL 0x17 // accelo full scale

#define REG_OUT_X_L_G 0x22 // gyro x axis 1

#define REG_OUT_TEMP_L 0x20 // temp snsr reg 1

#define REG_STATUS 0x1E // status reg

#define REG_OUT_TEMP_H 0x21 // temp snsr reg 2

#define REG_OUT_X_H_G 0x23 // gyro x axis 2

#define REG_OUT_Y_L_G 0x24 // gyro y axis 1

#define REG_OUT_Y_H_G 0x25 // gyro y axis 2

#define REG_OUT_Z_L_G 0x26 // gyro z axis 1

#define REG_OUT_Z_H_G 0x27 // gyro z axis 2

#define REG_OUT_X_L_A 0x28 // accelo x axis 1

#define REG_OUT_X_H_A 0x29 // accelo x axis 2

#define REG_OUT_Y_L_A 0x2A // accelo y axis 1

#define REG_OUT_Y_H_A 0x2B // accelo y axis 2

#define REG_OUT_Z_L_A 0x2C // accelo z axis 1

#define REG_OUT_Z_H_A 0x2D // accelo z axis 2

#define FIFO_STATUS1_REG 0x1B

uint8_t scale_accelo = 0x02; // ±8g (FS_XL = 10)

uint8_t scale_gyro = 0x04; // ±2000 dps, 0x0C gives 4000 dps, 0x01 gives 250 dps

uint8_t check = 1;

uint8_t data[14]; // 12 for accel/gyro + 2 for temp

int16_t temp;

uint8_t status;

uint8_t func_status;

uint8_t temp_l, temp_h;

int16_t temp_raw;

float temp_c;

uint8_t gyro_buf[6];

uint8_t accelo_buf[6];

int16_t gyro_x, gyro_y, gyro_z;

int16_t accelo_x, accelo_y, accelo_z;

float gyro_x_dps, gyro_y_dps, gyro_z_dps;

float accelo_x_dps, accelo_y_dps, accelo_z_dps;

float acc_div = 4096.0f; // ±8g: 0.244 mg/LSB -> 4096 LSB/g

float gyro_div = 14.29f; // ±2000 dps: 70 mdps/LSB -> 14.29 LSB/dps

uint8_t data[14];

int16_t gyro_raw[3], acc_raw[3];

uint8_t fifo_status[2];

uint8_t fifo_data[7];

#ifndef HSEM_ID_0

#define HSEM_ID_0 (0U) /* HW semaphore 0*/

#endif

I2C_HandleTypeDef hi2c1;

UART_HandleTypeDef huart1;

int16_t gyro_raw[3];

void SystemClock_Config(void);

void PeriphCommonClock_Config(void);

static void MX_GPIO_Init(void);

static void MX_USART1_UART_Init(void);

static void MX_I2C1_Init(void);

int main(void)

{

int32_t timeout;

timeout = 0xFFFF;

while((__HAL_RCC_GET_FLAG(RCC_FLAG_D2CKRDY) != RESET) && (timeout-- > 0));

if ( timeout < 0 )

{

Error_Handler();

}

HAL_Init();

SystemClock_Config();

PeriphCommonClock_Config();

__HAL_RCC_HSEM_CLK_ENABLE();

HAL_HSEM_FastTake(HSEM_ID_0);

HAL_HSEM_Release(HSEM_ID_0,0);

timeout = 0xFFFF;

while((__HAL_RCC_GET_FLAG(RCC_FLAG_D2CKRDY) == RESET) && (timeout-- > 0));

if ( timeout < 0 )

{

Error_Handler();

}

MX_GPIO_Init();

MX_USART1_UART_Init();

MX_I2C1_Init();

printf("STEVAL$KMI227AA\n\r");

HAL_StatusTypeDef ret;

HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_SET);

// Test address 0x6A

uint8_t who_am_i_reg = 0x0F;

uint8_t who_am_i_val = 0;

ret = HAL_I2C_Mem_Read(&hi2c1, (0x6A << 1), who_am_i_reg, I2C_MEMADD_SIZE_8BIT, &who_am_i_val, 1, HAL_MAX_DELAY);

if (who_am_i_val == 0x70) {

printf("device found\n\r");

} else {

printf("device not found\n\r");

while (1); // Halt if device not found

}

// Enable Block Data Update (BDU) first to prevent partial reads

uint8_t ctrl3_c = 0x44; // BDU = 1, IF_INC = 1

ret = HAL_I2C_Mem_Write(&hi2c1, 0xD4, 0x12, I2C_MEMADD_SIZE_8BIT, &ctrl3_c, 1, HAL_MAX_DELAY);

if (ret == HAL_OK) {

printf("BDU enabled successfully\n\r");

} else {

printf("Failed to enable BDU\n\r");

}

// Enable Accelerometer: 104 Hz, HP mode

uint8_t ctrl1_val = 0x76; // ODR_XL = 0110 (104 Hz), OP_MODE_XL = 000 (HP)

ret = HAL_I2C_Mem_Write(&hi2c1, 0xD4, 0x10, I2C_MEMADD_SIZE_8BIT, &ctrl1_val, 1, HAL_MAX_DELAY);

if (ret == HAL_OK) {

printf("accelo ok\n\r");

} else {

printf("failed accelo\n\r");

}

// Enable Gyroscope: Hz,

uint8_t ctrl2_val = 0x76; // ODR_G = 0110 (Hz), OP_MODE_G = 000

ret = HAL_I2C_Mem_Write(&hi2c1, 0xD4, 0x11, I2C_MEMADD_SIZE_8BIT, &ctrl2_val, 1, HAL_MAX_DELAY);

if (ret == HAL_OK) {

printf("gyro ok\n\r");

} else {

printf("failed gyro\n\r");

}

// STEP 1: Enable access to embedded function registers

uint8_t val = 0x80; // EMB_FUNC_REG_ACCESS = 1

ret = HAL_I2C_Mem_Write(&hi2c1, 0xD4, 0x01, I2C_MEMADD_SIZE_8BIT, &val, 1, HAL_MAX_DELAY);

if (ret == HAL_OK) {

printf("register enabled\n\r");

} else {

printf("failed register\n\r");

}

// STEP 2: Enable SFLP

uint8_t vall = 0x02; // SFLP_GAME_EN = 1

ret = HAL_I2C_Mem_Write(&hi2c1, 0xD4, 0x04, I2C_MEMADD_SIZE_8BIT, &vall, 1, HAL_MAX_DELAY);

if (ret == HAL_OK) {

printf("sflp enabled\n\r");

} else {

printf("sflp register failed\n\r");

}

// STEP 3: Set SFLP ODR

uint8_t sflp_odr = 0x67; // 60 Hz for SFLP

ret = HAL_I2C_Mem_Write(&hi2c1, 0xD4, 0x5E, I2C_MEMADD_SIZE_8BIT, &sflp_odr, 1, HAL_MAX_DELAY);

if (ret == HAL_OK) {

printf("SFLP ODR set\n\r");

} else {

printf("SFLP ODR set failed\n\r");

}

// STEP 4: Initialize SFLP

uint8_t val3 = 0x02; // SFLP_GAME_INIT = 1

ret = HAL_I2C_Mem_Write(&hi2c1, 0xD4, 0x66, I2C_MEMADD_SIZE_8BIT, &val3, 1, HAL_MAX_DELAY);

if (ret == HAL_OK) {

printf("game register enabled\n\r");

} else {

printf("game register failed\n\r");

}

// STEP 5: Enable SFLP data in FIFO

uint8_t val4 = 0x02; // FIFO_SFLP_EN = 1

ret = HAL_I2C_Mem_Write(&hi2c1, 0xD4, 0x44, I2C_MEMADD_SIZE_8BIT, &val4, 1, HAL_MAX_DELAY);

if (ret == HAL_OK) {

printf("fifo sflp enabled\n\r");

} else {

printf("fifo sflp register failed\n\r");

}

// STEP 6: Check SFLP status

uint8_t emb_func_status = 0;

ret = HAL_I2C_Mem_Read(&hi2c1, 0xD4, 0x43, I2C_MEMADD_SIZE_8BIT, &emb_func_status, 1, HAL_MAX_DELAY);

if (ret == HAL_OK) {

printf("EMB_FUNC_STATUS: 0x%02X\n\r", emb_func_status);

if (emb_func_status & 0x02) {

printf("SFLP_GAME_ON active\n\r");

} else {

printf("SFLP_GAME_ON not active\n\r");

}

} else {

printf("Failed to read EMB_FUNC_STATUS\n\r");

}

// STEP 7: Configure FIFO watermark (e.g., 10 samples)

uint8_t fifo_ctrl1 = 0x0A; // Watermark = 10 samples (low byte)

uint8_t fifo_ctrl2 = 0x00; // Watermark high byte (0 for 10 samples)

ret = HAL_I2C_Mem_Write(&hi2c1, 0xD4, 0x07, I2C_MEMADD_SIZE_8BIT, &fifo_ctrl1, 1, HAL_MAX_DELAY);

if (ret == HAL_OK) {

printf("FIFO watermark low set\n\r");

} else {

printf("FIFO watermark low set failed\n\r");

}

ret = HAL_I2C_Mem_Write(&hi2c1, 0xD4, 0x08, I2C_MEMADD_SIZE_8BIT, &fifo_ctrl2, 1, HAL_MAX_DELAY);

if (ret == HAL_OK) {

printf("FIFO watermark high set\n\r");

} else {

printf("FIFO watermark high set failed\n\r");

}

// STEP 8: Set FIFO ODR for gyro and accelerometer

uint8_t fifo_ctrl3 = 0x66; // BDR_XL = 0110 (104 Hz), BDR_GY = 0110 (104 Hz)

ret = HAL_I2C_Mem_Write(&hi2c1, 0xD4, 0x09, I2C_MEMADD_SIZE_8BIT, &fifo_ctrl3, 1, HAL_MAX_DELAY);

if (ret == HAL_OK) {

printf("Set FIFO gyro and accelo ODR successfully\n\r");

} else {

printf("Failed to set FIFO gyro and accelo ODR\n\r");

}

// STEP 9: Enable continuous mode

uint8_t val5 = 0x06; // FIFO_MODE = 110 (continuous)

ret = HAL_I2C_Mem_Write(&hi2c1, 0xD4, 0x0A, I2C_MEMADD_SIZE_8BIT, &val5, 1, HAL_MAX_DELAY);

if (ret == HAL_OK) {

printf("fifo continuous enabled\n\r");

} else {

printf("fifo continuous failed\n\r");

}

// STEP 10: Disable embedded register access

val = 0x00; // Clear EMB_FUNC_REG_ACCESS

ret = HAL_I2C_Mem_Write(&hi2c1, 0xD4, 0x01, I2C_MEMADD_SIZE_8BIT, &val, 1, HAL_MAX_DELAY);

if (ret == HAL_OK) {

printf("exited embedded register access\n\r");

} else {

printf("failed to exit embedded register access\n\r");

}

// Wait for SFLP to stabilize (increased to 1000 ms)

HAL_Delay(1000);

// Verify accelerometer and gyroscope data

uint8_t acc_data[6];

ret = HAL_I2C_Mem_Read(&hi2c1, 0xD4, 0x28, I2C_MEMADD_SIZE_8BIT, acc_data, 6, HAL_MAX_DELAY);

if (ret == HAL_OK) {

int16_t ax = (int16_t)(acc_data[1] << 8 | acc_data[0]);

int16_t ay = (int16_t)(acc_data[3] << 8 | acc_data[2]);

int16_t az = (int16_t)(acc_data[5] << 8 | acc_data[4]);

printf("Accelerometer X: %d, Y: %d, Z: %d\n\r", ax, ay, az);

} else {

printf("Failed to read accelerometer data\n\r");

}

uint8_t gyro_data[6];

ret = HAL_I2C_Mem_Read(&hi2c1, 0xD4, 0x22, I2C_MEMADD_SIZE_8BIT, gyro_data, 6, HAL_MAX_DELAY);

if (ret == HAL_OK) {

int16_t gx = (int16_t)(gyro_data[1] << 8 | gyro_data[0]);

int16_t gy = (int16_t)(gyro_data[3] << 8 | gyro_data[2]);

int16_t gz = (int16_t)(gyro_data[5] << 8 | gyro_data[4]);

printf("Gyroscope X: %d, Y: %d, Z: %d\n\r", gx, gy, gz);

} else {

printf("Failed to read gyroscope data\n\r");

}

while (1) {

// Read FIFO status

uint8_t fifo_status[2];

ret = HAL_I2C_Mem_Read(&hi2c1, 0xD4, 0x1B, I2C_MEMADD_SIZE_8BIT, fifo_status, 2, HAL_MAX_DELAY);

uint16_t fifo_word_count = ((fifo_status[1] & 0x03) << 8) | fifo_status[0];

uint8_t fifo_empty = (fifo_status[1] & 0x80) >> 7;

uint8_t fifo_full = (fifo_status[1] & 0x20) >> 5;

uint8_t fifo_overrun = (fifo_status[1] & 0x40) >> 6;

printf("FIFO status 1: 0x%02X, FIFO status 2: 0x%02X, Word count: %d, Empty: %d, Full: %d, Overrun: %d\r\n",

fifo_status[0], fifo_status[1], fifo_word_count, fifo_empty, fifo_full, fifo_overrun);

// Read FIFO data (7 bytes: 1 tag + 6 data bytes)

ret = HAL_I2C_Mem_Read(&hi2c1, 0xD4, 0x78, I2C_MEMADD_SIZE_8BIT, fifo_data, 7, HAL_MAX_DELAY);

printf("FIFO raw Tag out: 0x%02X \r\n", fifo_data[0]);

HAL_Delay(100);

}

}

Its output:

STEVAL$KMI227AA

device found

BDU enabled successfully

accelo ok

gyro ok

register enabled

sflp enabled

SFLP ODR set

game register enabled

fifo sflp enabled

EMB_FUNC_STATUS: 0xC3

SFLP_GAME_ON active

FIFO watermark low set

FIFO watermark high set

Set FIFO gyro and accelo ODR successfully

fifo continuous enabled

exited embedded register access

Accelerometer X: 0, Y: 73, Z: 16447

Gyroscope X: -39, Y: 52, Z: -72