LSM6DSV16BX SFLP performance

[NicolasGoualard]

Hi, I am using the LSM6DSV16BX IMU on a project where I need the Game Rotation data.

I used the Github examples as a base, and reading the Game Rotation values works well and I am able to save the quaternion data in a file. I wrote a little script that takes the Game Rotation quaternion values and applies the orientation to an object, to mimic what is possible in Unico GUI with STEVAL-MKI234, however I do not get the same performance with my custom board. For instance, I sometimes get some weird rotations around two axis even though I rotate the device around a single axis. See video:

I was wondering if any special setup was required to achieve this kind of performance ? Is there any calibration that can be made ?

I configure the Accelerometer and gyroscope to 960Hz, but batch samples of Accelerometer, Gyrometer and Game Rotation data at 120Hz in FIFO. I realize this is not much information to try and help me, I am more than happy to provide more detailed information if needed, any advice is appreciated.

 

Nicolas Goualard

[NicolasGoualard]

To recap my issue (as after reading my first post it did not seem super clear...), here is a video of the movement I apply to the device:

Here is a plot of the quaternion data I read from the device corresponding to this movement:

I have a script that displays a 3D model of a surfboard and orients it according to the received data, which looks like what is in my original post. You can see rotations around two axis, where I only rotate the device around one axis.

I would have expected the graphs to look like this:

That is, the quaternion data stabilizes after the move. It is what I observe when using Unico and the STEVAL MKI234AA.

I did some more tests to try to understand what I could have done wrong, in particular I exported the configuration from Unico and applied it to my device. The configuration I used is the following:

/**
 ******************************************************************************
 * @attention
 *
 * Copyright (c) 2022 STMicroelectronics.
 * All rights reserved.
 *
 * This software is licensed under terms that can be found in the LICENSE file
 * in the root directory of this software component.
 * If no LICENSE file comes with this software, it is provided AS-IS.
 *
 ******************************************************************************
 */

#ifndef SFLP_CORRECT_CONFIG_H
#define SFLP_CORRECT_CONFIG_H

#ifdef __cplusplus
extern "C" {
#endif

#include <stdint.h>

#ifndef MEMS_UCF_SHARED_TYPES
#define MEMS_UCF_SHARED_TYPES

/** Common data block definition **/
typedef struct {
 uint8_t address;
 uint8_t data;
} ucf_line_t;

#endif /* MEMS_UCF_SHARED_TYPES */

/** Configuration array generated from Unico Tool **/
const ucf_line_t sflp_correct_config[] = {
 {.address = 0x01, .data = 0x00,},
 {.address = 0x02, .data = 0x23,},
 {.address = 0x03, .data = 0x00,},
 {.address = 0x07, .data = 0x19,},
 {.address = 0x08, .data = 0x00,},
 {.address = 0x09, .data = 0x00,},
 {.address = 0x0A, .data = 0x06,},
 {.address = 0x0B, .data = 0x00,},
 {.address = 0x0C, .data = 0x00,},
 {.address = 0x0D, .data = 0x08,},
 {.address = 0x0E, .data = 0x00,},
 {.address = 0x10, .data = 0x06,},
 {.address = 0x11, .data = 0x06,},
 {.address = 0x12, .data = 0x44,},
 {.address = 0x13, .data = 0x08,},
 {.address = 0x14, .data = 0x00,},
 {.address = 0x15, .data = 0x04,},
 {.address = 0x16, .data = 0x00,},
 {.address = 0x17, .data = 0x02,},
 {.address = 0x18, .data = 0x00,},
 {.address = 0x19, .data = 0x00,},
 {.address = 0x50, .data = 0x00,},
 {.address = 0x54, .data = 0x04,},
 {.address = 0x55, .data = 0x00,},
 {.address = 0x56, .data = 0x00,},
 {.address = 0x57, .data = 0x00,},
 {.address = 0x58, .data = 0x00,},
 {.address = 0x59, .data = 0x00,},
 {.address = 0x5A, .data = 0x00,},
 {.address = 0x5B, .data = 0x00,},
 {.address = 0x5C, .data = 0x00,},
 {.address = 0x5D, .data = 0x00,},
 {.address = 0x5E, .data = 0x00,},
 {.address = 0x5F, .data = 0x00,},
 {.address = 0x63, .data = 0x30,},
 {.address = 0x6C, .data = 0x03,},
 {.address = 0x6D, .data = 0xE0,},
 {.address = 0x6E, .data = 0x01,},
 {.address = 0x73, .data = 0x00,},
 {.address = 0x74, .data = 0x00,},
 {.address = 0x75, .data = 0x00,},
 {.address = 0x01, .data = 0x80,},
 {.address = 0x04, .data = 0x02,},
 {.address = 0x44, .data = 0x32,},
 {.address = 0x01, .data = 0x00,}
};

#ifdef __cplusplus
}
#endif

#endif /* SFLP_CORRECT_CONFIG_H */

When using this configuration I see no changes to the behaviour of my device.

 

In my application I use the lsm6dsv16bx_sflp_game_gbias_set function to manually set the GBias, however from my understanding, the LSM6DSV16BX calibrates the GBias automatically when it is enabled, which should be the case when SFLP is enabled. Is that correct ? Could setting the gbias manually cause problems ?

I was thinking maybe the various filters that can be tuned in the LSM6DSV16BX could make an impact ? However they don't seem to be used in the configuration I exported from Unico.

 

If that's any help, here is how I setup the device for game rotation data acquisition:

int imu_start_acquisition(imu_recording_state_t wanted_state)
{
	lsm6dsv16bx_pin_int_route_t pin1_int = {0};
	lsm6dsv16bx_fifo_sflp_raw_t fifo_sflp = {0};
	lsm6dsv16bx_filt_settling_mask_t filt_settling_mask = {0};
	int ret;

	sensor.state.recording_state = wanted_state;

	/* Enable Block Data Update */
	ret = lsm6dsv16bx_block_data_update_set(&sensor.dev_ctx, PROPERTY_ENABLE);
	if (ret) {
		LOG_ERR("lsm6dsv16bx_block_data_update_set (%i)", ret);
		return ret;
	}
	/* Set full scale */
	ret = lsm6dsv16bx_xl_full_scale_set(&sensor.dev_ctx, LSM6DSV16BX_8g);
	if (ret) {
		LOG_ERR("lsm6dsv16bx_xl_full_scale_set (%i)", ret);
		return ret;
	}
	ret = lsm6dsv16bx_gy_full_scale_set(&sensor.dev_ctx, LSM6DSV16BX_2000dps);
	if (ret) {
		LOG_ERR("lsm6dsv16bx_gy_full_scale_set (%i)", ret);
		return ret;
	}

	/*
	* Set FIFO watermark (number of unread sensor data TAG + 6 bytes
	* stored in FIFO) to CONFIG_LSM6DSV16BX_FIFO_WATERMARK samples
	*/
	ret = lsm6dsv16bx_fifo_watermark_set(&sensor.dev_ctx, 25);
	if (ret) {
		LOG_ERR("lsm6dsv16bx_fifo_watermark_set (%i)", ret);
		return ret;
	}

	/* Set FIFO batch XL/Gyro ODR to specified frequency */
	ret = lsm6dsv16bx_fifo_xl_batch_set(&sensor.dev_ctx, LSM6DSV16BX_XL_BATCHED_AT_120Hz);
	if (ret) {
		LOG_ERR("lsm6dsv16bx_fifo_xl_batch_set (%i)", ret);
		return ret;
	}
	if (wanted_state.gy_enabled)
	{
		ret = lsm6dsv16bx_fifo_gy_batch_set(&sensor.dev_ctx, LSM6DSV16BX_GY_BATCHED_AT_120Hz);
		if (ret) {
			LOG_ERR("lsm6dsv16bx_fifo_gy_batch_set (%i)", ret);
			return ret;
		}
	}

	/* Set FIFO batch of sflp data */
	fifo_sflp.game_rotation = wanted_state.sflp_state.game_rot_enabled; // true
	fifo_sflp.gravity = wanted_state.sflp_state.gravity_enabled; // false
	fifo_sflp.gbias = wanted_state.sflp_state.gbias_enabled; // false
	
	ret = lsm6dsv16bx_fifo_sflp_batch_set(&sensor.dev_ctx, fifo_sflp);
	if (ret) {
		LOG_ERR("lsm6dsv16bx_fifo_sflp_batch_set (%i)", ret);
		return ret;
	}

	/* Set FIFO mode to Stream mode (aka Continuous Mode) */
	ret = lsm6dsv16bx_fifo_mode_set(&sensor.dev_ctx, LSM6DSV16BX_STREAM_MODE);
	if (ret) {
		LOG_ERR("lsm6dsv16bx_fifo_mode_set (%i)", ret);
		return ret;
	}

	pin1_int.fifo_th = PROPERTY_ENABLE;
	ret = lsm6dsv16bx_pin_int1_route_set(&sensor.dev_ctx, pin1_int);
	if (ret) {
		LOG_ERR("lsm6dsv16bx_pin_int1_route_set (%i)", ret);
		return ret;
	}

	/* Set High Performance mode */
	ret = lsm6dsv16bx_xl_mode_set(&sensor.dev_ctx, LSM6DSV16BX_XL_HIGH_PERFORMANCE_MD);
	if (ret)
	{
		LOG_ERR("lsm6dsv16bx_xl_mode_set %i", ret);
		return ret;
	}
	/* Set Output Data Rate */
	ret = lsm6dsv16bx_xl_data_rate_set(&sensor.dev_ctx, LSM6DSV16BX_XL_ODR_AT_960Hz);
	if (ret) {
		LOG_ERR("lsm6dsv16bx_xl_data_rate_set (%i)", ret);
		return ret;
	}

	/* Set High Performance mode */
	ret = lsm6dsv16bx_gy_mode_set(&sensor.dev_ctx, LSM6DSV16BX_GY_HIGH_PERFORMANCE_MD);
	if (ret)
	{
		LOG_ERR("lsm6dsv16bx_xl_mode_set %i", ret);
		return ret;
	}
	/* Set Output Data Rate */
	ret = lsm6dsv16bx_gy_data_rate_set(&sensor.dev_ctx, LSM6DSV16BX_GY_ODR_AT_960Hz);
	if (ret) {
		LOG_ERR("lsm6dsv16bx_gy_data_rate_set (%i)", ret);
		return ret;
	}

	if (wanted_state.sflp_state.gbias_enabled || wanted_state.sflp_state.game_rot_enabled || wanted_state.sflp_state.gravity_enabled)
	{
		ret = lsm6dsv16bx_sflp_data_rate_set(&sensor.dev_ctx, LSM6DSV16BX_SFLP_120Hz);
		if (ret) {
			LOG_ERR("lsm6dsv16bx_sflp_data_rate_set (%i)", ret);
			return ret;
		}
	}
	ret = lsm6dsv16bx_fifo_timestamp_batch_set(&sensor.dev_ctx, LSM6DSV16BX_TMSTMP_DEC_1);
	if (ret) {
		LOG_ERR("lsm6dsv16bx_fifo_timestamp_batch_set (%i)", ret);
		return ret;
	}
	ret = lsm6dsv16bx_timestamp_set(&sensor.dev_ctx, PROPERTY_ENABLE);
	if (ret) {
		LOG_ERR("lsm6dsv16bx_timestamp_set (%i)", ret);
		return ret;
	}

	if (wanted_state.sflp_state.game_rot_enabled || wanted_state.sflp_state.gravity_enabled || wanted_state.sflp_state.gbias_enabled)
	{
		ret = lsm6dsv16bx_sflp_game_rotation_set(&sensor.dev_ctx, PROPERTY_ENABLE);
		if (ret) {
			LOG_ERR("lsm6dsv16bx_sflp_game_rotation_set (%i)", ret);
			return ret;
		}

		ret = lsm6dsv16bx_sflp_game_gbias_set(&sensor.dev_ctx, &gbias);
		if (ret) {
			LOG_ERR("lsm6dsv16bx_sflp_game_gbias_set (%i)", ret);
			return ret;
		}
	}

	/* Mask accelerometer and gyroscope data until the settling of the sensors filter is completed */
 	filt_settling_mask.drdy = PROPERTY_ENABLE;
 	filt_settling_mask.irq_xl = PROPERTY_ENABLE;
 	filt_settling_mask.irq_g = PROPERTY_ENABLE;
 	ret = lsm6dsv16bx_filt_settling_mask_set(&sensor.dev_ctx, filt_settling_mask);
	if (ret) {
		LOG_ERR("lsm6dsv16bx_filt_settling_mask_set (%i)", ret);
		return ret;
	}

	return 0;
}

 

Any help/pointers is appreciated :)

[NicolasGoualard]

Any help on this would be greatly appreciated :)

[Federica Bossi]

Hi @NicolasGoualard ,

 

While reviewing the code you shared, we noticed that in the section you indicated, a fundamental step was missing. This step is described in the Application Manual AN5845.

int imu_start_acquisition(imu_recording_state_t wanted_state)
{
 lsm6dsv16bx_pin_int_route_t pin1_int = {0};
 lsm6dsv16bx_fifo_sflp_raw_t fifo_sflp = {0};
 lsm6dsv16bx_filt_settling_mask_t filt_settling_mask = {0};
 int ret;

 sensor.state.recording_state = wanted_state;

 /* Enable Block Data Update */
 ret = lsm6dsv16bx_block_data_update_set(&sensor.dev_ctx, PROPERTY_ENABLE);
 if (ret) {
 LOG_ERR("lsm6dsv16bx_block_data_update_set (%i)", ret);
 return ret;
 }

 /* 2. Configuration Init*/
 ret = lsm6dsv16bx_sflp_configure(&sensor.dev_ctx);
 if (ret) {
 LOG_ERR("lsm6dsv16bx_sflp_configure (%i)", ret);
 return ret;
 }

 /* Set full scale */
 ret = lsm6dsv16bx_xl_full_scale_set(&sensor.dev_ctx, LSM6DSV16BX_8g);
 if (ret) {
 LOG_ERR("lsm6dsv16bx_xl_full_scale_set (%i)", ret);
 return ret;
 }
 ret = lsm6dsv16bx_gy_full_scale_set(&sensor.dev_ctx, LSM6DSV16BX_2000dps);
 if (ret) {
 LOG_ERR("lsm6dsv16bx_gy_full_scale_set (%i)", ret);
 return ret;
 }

 /*
 * Set FIFO watermark (number of unread sensor data TAG + 6 bytes
 * stored in FIFO) to CONFIG_LSM6DSV16BX_FIFO_WATERMARK samples
 */
 ret = lsm6dsv16bx_fifo_watermark_set(&sensor.dev_ctx, 25);
 if (ret) {
 LOG_ERR("lsm6dsv16bx_fifo_watermark_set (%i)", ret);
 return ret;
 }


 /* Set FIFO batch of sflp data */
 fifo_sflp.game_rotation = wanted_state.sflp_state.game_rot_enabled; // true
 fifo_sflp.gravity = wanted_state.sflp_state.gravity_enabled; // false
 fifo_sflp.gbias = wanted_state.sflp_state.gbias_enabled; // false

 ret = lsm6dsv16bx_fifo_sflp_batch_set(&sensor.dev_ctx, fifo_sflp);
 if (ret) {
 LOG_ERR("lsm6dsv16bx_fifo_sflp_batch_set (%i)", ret);
 return ret;
 }

 /* Set FIFO mode to Stream mode (aka Continuous Mode) */
 ret = lsm6dsv16bx_fifo_mode_set(&sensor.dev_ctx, LSM6DSV16BX_STREAM_MODE);
 if (ret) {
 LOG_ERR("lsm6dsv16bx_fifo_mode_set (%i)", ret);
 return ret;
 }

 pin1_int.fifo_th = PROPERTY_ENABLE;
 ret = lsm6dsv16bx_pin_int1_route_set(&sensor.dev_ctx, pin1_int);
 if (ret) {
 LOG_ERR("lsm6dsv16bx_pin_int1_route_set (%i)", ret);
 return ret;
 }

 /* Set High Performance mode */
 ret = lsm6dsv16bx_xl_mode_set(&sensor.dev_ctx, LSM6DSV16BX_XL_HIGH_PERFORMANCE_MD);
 if (ret)
 {
 LOG_ERR("lsm6dsv16bx_xl_mode_set %i", ret);
 return ret;
 }
 /* Set Output Data Rate */
 ret = lsm6dsv16bx_xl_data_rate_set(&sensor.dev_ctx, LSM6DSV16BX_XL_ODR_AT_960Hz);
 if (ret) {
 LOG_ERR("lsm6dsv16bx_xl_data_rate_set (%i)", ret);
 return ret;
 }

 /* Set High Performance mode */
 ret = lsm6dsv16bx_gy_mode_set(&sensor.dev_ctx, LSM6DSV16BX_GY_HIGH_PERFORMANCE_MD);
 if (ret)
 {
 LOG_ERR("lsm6dsv16bx_xl_mode_set %i", ret);
 return ret;
 }
 /* Set Output Data Rate */
 ret = lsm6dsv16bx_gy_data_rate_set(&sensor.dev_ctx, LSM6DSV16BX_GY_ODR_AT_960Hz);
 if (ret) {
 LOG_ERR("lsm6dsv16bx_gy_data_rate_set (%i)", ret);
 return ret;
 }

 if (wanted_state.sflp_state.gbias_enabled || wanted_state.sflp_state.game_rot_enabled || wanted_state.sflp_state.gravity_enabled)
 {
 ret = lsm6dsv16bx_sflp_data_rate_set(&sensor.dev_ctx, LSM6DSV16BX_SFLP_120Hz);
 if (ret) {
 LOG_ERR("lsm6dsv16bx_sflp_data_rate_set (%i)", ret);
 return ret;
 }
 }

 if (wanted_state.sflp_state.game_rot_enabled || wanted_state.sflp_state.gravity_enabled || wanted_state.sflp_state.gbias_enabled)
 {
 ret = lsm6dsv16bx_sflp_game_rotation_set(&sensor.dev_ctx, PROPERTY_ENABLE);
 if (ret) {
 LOG_ERR("lsm6dsv16bx_sflp_game_rotation_set (%i)", ret);
 return ret;
 }
 lsm6dsv16bx_sflp_gbias_t gbias;
 gbias.gbias_x = 0.0f;
 gbias.gbias_y = 0.0f;
 gbias.gbias_z = 0.0f;
 ret = lsm6dsv16bx_sflp_game_gbias_set(&sensor.dev_ctx, &gbias);
 if (ret) {
 LOG_ERR("lsm6dsv16bx_sflp_game_gbias_set (%i)", ret);
 return ret;
 }
 }

 /* Mask accelerometer and gyroscope data until the settling of the sensors filter is completed */
 filt_settling_mask.drdy = PROPERTY_ENABLE;
 filt_settling_mask.irq_xl = PROPERTY_ENABLE;
 filt_settling_mask.irq_g = PROPERTY_ENABLE;
 ret = lsm6dsv16bx_filt_settling_mask_set(&sensor.dev_ctx, filt_settling_mask);
 if (ret) {
 LOG_ERR("lsm6dsv16bx_filt_settling_mask_set (%i)", ret);
 return ret;
 }

 return 0;
}

 

I took the liberty to modify some parts.
I noticed that you were saving accelerometer and gyroscope values as well as the timestamp into the FIFO. For an initial test, I removed these to simplify the results and avoid the risk of incorrect data.

Please try running the test again with these modifications, and the expected results should be obtained.

[NicolasGoualard]

Hi Federica,

Thanks for your reply!

I have a call to the sflp_configure function in my imu_reset function, but that did not prevent the issue.

int imu_reset()
{
	k_timer_stop(&sensor.imu_timer.timer);

	lsm6dsv16bx_reset_t rst;
	/* Restore default configuration */
 	lsm6dsv16bx_reset_set(&sensor.dev_ctx, LSM6DSV16BX_GLOBAL_RST);
	do {
		lsm6dsv16bx_reset_get(&sensor.dev_ctx, &rst);
	} while (rst != LSM6DSV16BX_READY);

	imu_state_t tmp_state = {
		.recording_state = {
			.xl_enabled = false,
			.gy_enabled = false,
			.qvar_enabled = false,
			.sflp_state = {
				.gbias_enabled = false,
				.game_rot_enabled = false,
				.gravity_enabled = false,
			},
			.sampling_frequency = 0,
			.scale = {
				.xl_scale = CONFIG_LSM6DSV16BX_DEFAULT_XL_SCALE,
				.gy_scale = CONFIG_LSM6DSV16BX_DEFAULT_GY_SCALE,
			}
		},
		.sigmot_enabled = false,
		.wakeup_enabled = false,
		.fsm_enabled = false,
		.calib = IMU_CALIBRATION_NOT_CALIBRATING,
		.int2_on_int1 = false,
		.xl_conversion_function = NULL,
		.gy_conversion_function = NULL,
	};
	memcpy(&sensor.state, &tmp_state, sizeof(tmp_state));

	lsm6dsv16bx_sflp_configure(&sensor.dev_ctx);

	return 0;
}

 

However, when I tried to call the sflp_configure function just before starting the acquisition as you suggested, the problem went away and the device seems to behave as expected. Something in between the IMU reset and the acquisition start must mess up the SFLP configuration. I'll do some more digging to see what was causing the issue, but that solves my problem.

Thanks, and happy holidays!

Nicolas Goualard