How to set up and read SFLP Game Vector on ISM330BX

[marco_pontin]

Hello,

I am developing a custom application that uses your ISM330BX IMU. I am communicating with the IMU using I2C. I checked that I can commuicate correctly with it by requesting the ID in the WHO_AM_I register and everytihng seems to work fine.

The end goal is to get the LPSF block to work and output data to the fifo where it's going to be read by the main MCU. So far I tried following the info in the datasheet and the application note AN6109, but with little success. Every time I try to read the TAG of the data in the FIFO, the IMU always returns 0x02.

Please find below (and attached) the two functions I use to set up the IMU and read from the FIFO. I am sure I might have missed something in the configuration, but I can't seem to figure out what.

Thanks for the help.

Kind regards,

Marco

// accel/gyro control
#define CTRL1_XL               0x10  // XL ODR & mode :contentReference[oaicite:1]{index=1}
#define CTRL2_G                0x11  // G  ODR & mode :contentReference[oaicite:2]{index=2}
#define CTRL3_C                0x12  // BDU, auto-inc :contentReference[oaicite:3]{index=3}
#define CTRL6                  0x15  // G full-scale (FS_G) :contentReference[oaicite:4]{index=4}
#define CTRL8_XL               0x17  // XL full-scale (FS_XL) :contentReference[oaicite:5]{index=5}
// embedded functions (sensor fusion)
#define FUNC_CFG_ACCESS        0x01  // enable access to EMB_* :contentReference[oaicite:6]{index=6}
#define EMB_FUNC_EN_A          0x04  // SFLP_GAME_EN bit :contentReference[oaicite:7]{index=7}
#define EMB_FUNC_INIT_A        0x66  // SFLP_GAME_INIT bit :contentReference[oaicite:8]{index=8}
#define SFLP_ODR               0x5E  // SFLP low-power ODR :contentReference[oaicite:9]{index=9}
#define EMB_FUNC_FIFO_EN_A     0x44  // batch SFLP data in FIFO :contentReference[oaicite:10]{index=10}
#define PAGE_RW				   0x17
#define PAGE_SEL			   0x02
#define PAGE_ADDRESS		   0x08
#define PAGE_VALUE			   0x09
// FIFO control
#define FIFO_CTRL1			   0x07
#define FIFO_CTRL2			   0x08
#define FIFO_CTRL3			   0x09
#define FIFO_CTRL4			   0x0A
#define FIFO_STATUS1		   0x1B
#define FIFO_STATUS2		   0x1C
// FIFO-output registers
#define FIFO_DATA_OUT_TAG      0x78  // tag byte :contentReference[oaicite:11]{index=11}
#define FIFO_DATA_OUT_BYTE_0   0x79
#define FIFO_DATA_OUT_BYTE_1   0x7A
#define FIFO_DATA_OUT_BYTE_2   0x7B
#define FIFO_DATA_OUT_BYTE_3   0x7C
#define FIFO_DATA_OUT_BYTE_4   0x7D
#define FIFO_DATA_OUT_BYTE_5   0x7E
// tag for “game” rotation vector in FIFO
#define TAG_SFLP_GAME_ROT      0x13  // 0x13 = quaternion vector tag :contentReference[oaicite:12]{index=12}
// scale: raw → Q component (unit-less). ST uses Q14 → divide by 2^14
#define SFLP_QUAT_LSB          (1.0f/16384.0f)

void imu_init(void){
	uint8_t t;


	MXC_GPIO_OutClr(gpio_ch_sel_out.port, gpio_ch_sel_out.mask);
	MXC_Delay(MXC_DELAY_USEC(20));

	// Fully reeset device through SW_POR
	writeRegister(I2C_MCU_ADDR, FUNC_CFG_ACCESS, (uint8_t)(0x01<<2), 1); //parameters are IMU_ADDR, REG2WRITE, DATA2WRITE, DATA LEN IN BYTES
	MXC_Delay(MXC_DELAY_MSEC(50));

	// From Application Note AN6109 page 57
	writeRegister(I2C_MCU_ADDR, FUNC_CFG_ACCESS, 0x80, 1);
	writeRegister(I2C_MCU_ADDR, PAGE_RW, 0x40, 1);
	writeRegister(I2C_MCU_ADDR, PAGE_SEL, 0x01, 1);
	writeRegister(I2C_MCU_ADDR, PAGE_ADDRESS, 0xD2, 1);
	writeRegister(I2C_MCU_ADDR, PAGE_VALUE, 0x50, 1);
	writeRegister(I2C_MCU_ADDR, PAGE_RW, 0x00, 1);
	writeRegister(I2C_MCU_ADDR, FUNC_CFG_ACCESS, 0x00, 1);

	//CTRL3_C: block-data-update, auto-increment
	t = (1<<6) /* BDU=1 */ | (1<<2) /* IF_INC=1 */;
	writeRegister(I2C_MCU_ADDR, CTRL3_C, t, 1);

	writeRegister(I2C_MCU_ADDR, FIFO_CTRL3, 0x00, 1); //Prevent Accel and Gyro data from being stored to FIFO
	writeRegister(I2C_MCU_ADDR, FIFO_CTRL2, 0x00, 1);
	writeRegister(I2C_MCU_ADDR, FIFO_CTRL1, 0x01, 1); //Set 6 samples as max FIFO depth (2 samples for each quaternion are needed)
	writeRegister(I2C_MCU_ADDR, FIFO_CTRL4, 0x02, 1); //Set FIFO with WTM

	// accelerometer:
	writeRegister(I2C_MCU_ADDR, CTRL8_XL, 0x00, 1); //FS_XL=00→±2g
	writeRegister(I2C_MCU_ADDR, CTRL1_XL, 0b0111, 1); // ODR_XL=0111→240Hz, high-perf;
	// gyroscope:
	writeRegister(I2C_MCU_ADDR, CTRL6, 0x00, 1); // set FS_G=0000→±125dps
	writeRegister(I2C_MCU_ADDR, CTRL2_G, 0b0111, 1); // ODR_G =0111→240Hz, high-perf;

	// access embedded-function page
	writeRegister(I2C_MCU_ADDR, FUNC_CFG_ACCESS, 0x80, 1);
	// enable sensor-fusion LP block
	writeRegister(I2C_MCU_ADDR, EMB_FUNC_EN_A, 0b0100, 1);  // SFLP_GAME_EN = bit2
	// set SFLP ODR to 120Hz: bit6=1, bits5-3=011, bit2=0, bit1-0=11
	t = (1<<6) | (0b011<<3) | 0x03;
	writeRegister(I2C_MCU_ADDR, SFLP_ODR, t, 1);
	// batch quaternions into FIFO
	writeRegister(I2C_MCU_ADDR, EMB_FUNC_FIFO_EN_A, 0b0010, 1); // SFLP_GAME_FIFO_EN = bit1
	// reinit SFLP state machine
	writeRegister(I2C_MCU_ADDR, EMB_FUNC_INIT_A, 0b0010, 1); // SFLP_GAME_INIT = bit1
	// leave embedded-function page
	writeRegister(I2C_MCU_ADDR, FUNC_CFG_ACCESS, 0x00, 1);

	return;
}

int imu_read_quaternions(void){

	MXC_GPIO_OutClr(gpio_ch_sel_out.port, gpio_ch_sel_out.mask);
	MXC_Delay(MXC_DELAY_USEC(20));

	if(readRegister(I2C_MCU_ADDR, FIFO_DATA_OUT_TAG, 1) != 0){ //parameters are IMU_ADDR, REG2READ, N BYTES TO READ
		return 1;
	}
    //The rxdata buffer contains the data received from the I2C slave
	if ((rxdata[0] & 0x1F) != TAG_SFLP_GAME_ROT) {
		readRegister(I2C_MCU_ADDR, FIFO_DATA_OUT_BYTE_0, 6);
		return 2;  // not a quaternion sample
	}
	if (readRegister(I2C_MCU_ADDR, FIFO_DATA_OUT_BYTE_0, 6) != 0) {
		return 3;
	}
	Stxdata[0] = rxdata[1];
	Stxdata[1] = rxdata[0];
	Stxdata[2] = rxdata[3];
	Stxdata[3] = rxdata[2];
	Stxdata[4] = rxdata[5];
	Stxdata[5] = rxdata[4];
	Stxdata[6] = CRC8(Stxdata, 6);
	return 0;
}