on - edited on by
Laurids_PETERSE
1. How to compute data fusion based on an IMU?
Rotation matrices can be obtained from these three examples using matrix multiplication. For example, the product:
Represents a rotation whose yaw, pitch, and roll angles are α, β and γ, respectively. More formally, it is an intrinsic rotation whose Tait–Bryan angles are α, β, γ, about axes z, y, x, respectively. Similarly, the product:
2. What is the SFLP algorithm in the LSM6DSV16X?
A sensor fusion low-power (SFLP) block is available in the LSM6DSV16X for generating the following data based on the accelerometer and gyroscope data processing:
- Game rotation vector, which provides a quaternion representing the attitude of the device.
- Gravity vector, which provides a three-dimensional vector representing the direction of gravity.
- Gyroscope bias, which provides a three-dimensional vector representing the gyroscope bias.
Sensor fusion performance and time required to reach steady state
3. SFLP demo and tools
The hardware setup is STEVAL-MKI109V3 and DIL24 adapter board STEVAL-MKI227KA.
STEVAL-MKI109V3
DIL24 adapter
The software used is Unico-GUI.
When you shake the demo board, the image follows the actual position displayed. SFLP has integrated quaternion, so customers can directly get quaternion and convert it into euler angles.
4. Yaw angle automatic calibration flow
Due to the long-term operation of the gyroscope, the integration error caused by zero deviation becomes increasingly large. The yaw angle may experience significant drift due to lack of calibration of the magnetometer. To solve this problem, the SFLP algorithm adopts a dynamic self-calibration method to ensure yaw angle stability.
5. Automatic calibration flow
6. How to configure the SFLP?
Coordinates:
Pitch rotation around the X-axisΩP
Roll rotation around the Y-axis ΩR
Heading/Yaw rotation around the Z-axisΩY
- Game rotation vector, which provides a quaternion representing the attitude of the device.
- Gravity vector, which provides a three-dimensional vector representing the direction of gravity.
- Gyroscope bias, which provides a three-dimensional vector representing the gyroscope bias.
SFLP Register description:
|
Register bit config |
Bit description |
|
SFLP_GAME_EN |
Enables SFLP |
|
SFLP_GBIAS_FIFO_EN |
Enables Gbias in FIFO mode |
|
SFLP_GRAVITY_FIFO_EN |
Enables gravity vector in FIFO mode |
|
SFLP_GAME_FIFO_EN |
Enables game rotation vector in FIFO mode |
|
SFLP_GAME_ODR_[2:0] |
Configures SFLP ODR |
SFLP data format:
|
TAG |
X_L |
X_H |
Y_L |
Y_H |
Z_L |
Z_H |
Axis format |
|
|
Game rotation vector |
13h |
X |
Y |
Z |
Half precision floating-point |
|||
|
Gyroscope bias |
16h |
X |
Y |
Z |
int16_t (raw, 125 dps sensitivity) |
|||
|
Gravity vector |
17h |
X |
Y |
Z |
int16_t (raw, 2 g sensitivity) |
|||
Code configuration:
/* Check device ID */
lsm6dsv16x_device_id_get(&dev_ctx, &whoamI);
if (whoamI != LSM6DSV16X_ID)
while (1);
/* Restore default configuration */
lsm6dsv16x_reset_set(&dev_ctx, LSM6DSV16X_RESTORE_CTRL_REGS);
do {
lsm6dsv16x_reset_get(&dev_ctx, &rst);
} while (rst != LSM6DSV16X_READY);
/* Enable Block Data Update */
lsm6dsv16x_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
/* Set full scale */
lsm6dsv16x_xl_full_scale_set(&dev_ctx, LSM6DSV16X_4g);
lsm6dsv16x_gy_full_scale_set(&dev_ctx, LSM6DSV16X_2000dps);
/*
* Set FIFO watermark (number of unread sensor data TAG + 6 bytes
* stored in FIFO) to FIFO_WATERMARK samples
*/
lsm6dsv16x_fifo_watermark_set(&dev_ctx, FIFO_WATERMARK);
/* Set FIFO batch of sflp data */
fifo_sflp.game_rotation = 1;
fifo_sflp.gravity = 1;
fifo_sflp.gbias = 1;
lsm6dsv16x_fifo_sflp_batch_set(&dev_ctx, fifo_sflp);
/* Set FIFO mode to Stream mode (aka Continuous Mode) */
lsm6dsv16x_fifo_mode_set(&dev_ctx, LSM6DSV16X_STREAM_MODE);
/* Set Output Data Rate */
lsm6dsv16x_xl_data_rate_set(&dev_ctx, LSM6DSV16X_ODR_AT_30Hz);
lsm6dsv16x_gy_data_rate_set(&dev_ctx, LSM6DSV16X_ODR_AT_30Hz);
lsm6dsv16x_sflp_data_rate_set(&dev_ctx, LSM6DSV16X_SFLP_30Hz);
lsm6dsv16x_sflp_game_rotation_set(&dev_ctx, PROPERTY_ENABLE);More details about SFLP software are available on the GitHub repo sensor-fusion-code.
Conclusion
In this article, we described the principles of sensor fusion in the context of an inertial measurement unit and how to use sensor fusion in the LSM6DSV16X device. The LSM6DSV16X integrates a complete set of acceleration and gyroscope algorithms in the sensor. This allows customers to obtain high-precision fusion algorithm results without specifying specific algorithm implementations.
Customers can use STEVAL-MKI109V3 and LSM6DSV16X to evaluate the SFLP performance and how to configure SFLP on the demo board. Users can quickly deploy algorithms, test, and evaluate performance.
You may also be interested in reading the following knowledge article: How to save power with LSM6DSV16X by leveraging on its adaptive self-configuration with MLC and FSM
@Denise SANFILIPPO I managed to visualize the quaternions through the output terminal and saved the data.
The commans I used are:
"
fifo_sflp.game_rotation = 1;
fifo_sflp.gravity = 1;
fifo_sflp.gbias = 1;
lsm6dsv16x_fifo_sflp_batch_set(&dev_ctx, fifo_sflp);
lsm6dsv16x_fifo_mode_set(&dev_ctx, LSM6DSV16X_STREAM_MODE);
lsm6dsv16x_xl_data_rate_set(&dev_ctx, LSM6DSV16X_ODR_AT_30Hz); // ODR dell'accelerometro
lsm6dsv16x_gy_data_rate_set(&dev_ctx, LSM6DSV16X_ODR_AT_30Hz); // ODR del giroscopio
lsm6dsv16x_sflp_data_rate_set(&dev_ctx, LSM6DSV16X_SFLP_30Hz);
lsm6dsv16x_sflp_game_rotation_set(&dev_ctx, PROPERTY_ENABLE);
lsm6dsv16x_sflp_gbias_t gbias;
gbias.gbias_x = 0.0f;
gbias.gbias_y = 0.0f;
gbias.gbias_z = 0.0f;
lsm6dsv16x_sflp_game_gbias_set(&dev_ctx, &gbias);
while (1)
{
// Controlla lo stato del FIFO
lsm6dsv16x_fifo_status_get(&dev_ctx, &fifo_status);
if (fifo_status.fifo_th == 1) {
// Leggi i dati dal FIFO
uint16_t num_samples = fifo_status.fifo_level;
while (num_samples--) {
lsm6dsv16x_fifo_out_raw_t raw_data;
lsm6dsv16x_fifo_out_raw_get(&dev_ctx, &raw_data);
int16_t *axis;
float gravity_mg[3];
float gbias_mdps[3];
// Se i dati sono del Game Rotation Vector
if (raw_data.tag == LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG) {
// Converti i dati grezzi in quaternioni
sflp2q(quaternion, (uint16_t*)&raw_data.data[0]);
// Invia i dati via UART
snprintf((char*)tx_buffer, sizeof(tx_buffer), "Game Rotation \tX: %2.3f\tY: %2.3f\tZ: %2.3f\tW: %2.3f\r\n", quaternion[0], quaternion[1], quaternion[2], quaternion[3]);
tx_com(tx_buffer, strlen((char const*)tx_buffer));
}
else if (raw_data.tag == LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG)
{
axis = (int16_t *)&raw_data.data[0];
gbias_mdps[0] = lsm6dsv16x_from_fs125_to_mdps(axis[0]);
gbias_mdps[1] = lsm6dsv16x_from_fs125_to_mdps(axis[1]);
gbias_mdps[2] = lsm6dsv16x_from_fs125_to_mdps(axis[2]);
snprintf((char *)tx_buffer, sizeof(tx_buffer), "GBIAS [mdps]:%4.2f\t%4.2f\t%4.2f\r\n",
(double_t)gbias_mdps[0], (double_t)gbias_mdps[1], (double_t)gbias_mdps[2]);
tx_com(tx_buffer, strlen((char const *)tx_buffer));
}
else if (raw_data.tag == LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG)
{
axis = (int16_t *)&raw_data.data[0];
gravity_mg[0] = lsm6dsv16x_from_sflp_to_mg(axis[0]);
gravity_mg[1] = lsm6dsv16x_from_sflp_to_mg(axis[1]);
gravity_mg[2] = lsm6dsv16x_from_sflp_to_mg(axis[2]);
snprintf((char *)tx_buffer, sizeof(tx_buffer), "Gravity [mg]:%4.2f\t%4.2f\t%4.2f\r\n",
(double_t)gravity_mg[0], (double_t)gravity_mg[1], (double_t)gravity_mg[2]);
tx_com(tx_buffer, strlen((char const *)tx_buffer));
}
}
}
MX_MEMS_Process();
}"
The commands I used to get Euler angles from quaternions in Matlab are the following:
"q=quaternion(W_values,X_values,Y_values,Z_values);
angles=eulerd(q,'ZYX','frame');
roll_eulerd=angles(:,3);
pitch_eulerd=angles(:,2);
yaw_eulerd=angles(:,1);
yaw_eulerd = yaw_eulerd - yaw_eulerd(1);"
In the first acquisition I performed a roll:+90, pitch:+90, yaw:+90 rotation and in the second acquisition I performed a roll:-90, pitch:-90, yaw:-90 rotation and I get the attached graphs.
The pitch angle corresponds to the rotations made but the Roll angle varies even when it varies pitch angle and the Yaw angle instead always has a strange trend.
I also encountered this problem when implementing the Motion FX 6x and Motion FX 9x libraries and getting quaternions as output.
Since for my project I need to obtain very precise angles that do not vary when there is rotation around a single axis, what can I do?
Hi Denis
I have set the all the registers
It looks like it works.
I can read the FIFO values
The values change but not yet properly. I just read the the six FIFO_data_out byte
I have not yet understood what this table means,
Hi Denis
I have a very nice signal from the Accellero and from Gyroscope.
From the SFLP I get some problems.
Hi Denis
I read with an Interrupt 30Hz the six register and display the read value 1000 times in Chart. I do not move the sensor. For the first test I read only Gyro data, for the second only Acceloero data and for the third test only the fusion data.
If I move the sensor all values change, that would be ok so far
Hi @Stefan3 ,
Are you converting the SFLP output data from half-precision floating-point format (float16) to float format?
You should interpret SFLP data as follows:
Please note that the SFLP output data is quaternions, not angles.
Hi Denis
With the attention of the TAG I can now read SFLP.
But in the data I regularly had wrong values.
For example 20,21,24. in all the 3Channel
Every time I got these values, I read the data again and then they were ok again.
Now I am trying to convert the values into roll, pitch, yaw
Hi Denis, Hi Eliane
I read the game rotation vectors from the LSM9
But unfortunately no fourth value. This is called the scalar part. float sw = quat[3];
How do I calculate this scalar part or how do I read it?
float sx = quat[1];
float sy = quat[2];
float sz = quat[0];
float sw = quat[3];
Hi Denis
It now works from configuring the LSM6, reading from FIFO, sending to USB and receiving in the App.
Also the conversion from FP16 to FB32 and the calculation of pitch, yaw, roll and each in grad degres is perfect.
A small problem is still, regular spikes in the data.
In decimal the values are about 20-40 und if I convert to FP32 it's looks like a very small number or somtimes very high number
I suppose I read a counter or something similar instead of the quaternion.
Hi Denis
Ok before I had only read 1 byte and if that was correct then I have read the 6 others.
With your suggestion to read 7 at once and then when the tag is right I use it, it works very well and also faster.
I can read and transmit at 15Hz, 30Hz, 60Hz, 120Hz, 240Hz, 480Hz without any problems.
The old chart shows Pitch, Yaw, Roll in Grad degrees.
Here a new chart with both, the convertet Float32 value (top) and the chart with the Pitch, Yaw, Roll (bottom). The x (pitch) is 358 grad. It changes the value when I turn, but I suspect not in the right range
Hi Denis
In your code, the axes are assigned as follows in the picture.
I have now done the same for me.
And I use this:
yaw=euler[0]
pitch=euler[1]
roll=euler[2]
Then the results look different. I will do further tests and see how it goes.
One more question: Does the sensitivity of accelero and gyro have an influence?
I tried the Continuous Mode and the FIFO Mode. In both cases I acquire data from two sensors performing the same movement simultaneously.
With the "Continuous Mode", for each occurrence (i.e. ACC1, ACC2, GYR1, GYR2, etc) the number of values I get is different. Also, Sensor 2 data is lagging behind sensor 1 data. There is a delay in acquiring data from sensor 1 and sensor 2:
Also, from the quaternion I obtain the following angles whose trend is very strange and does not appear continuous:
I think the main problem is in the data management in FIFO. I don't understand why the data is not printed in order, i.e. "Game Rotation 1, Game Rotation 2, GYR1,GYR2, ACC1,ACC2" but the order changes over time (see this post Issue with Misaligned Accelerometer and Gyroscope ... - STMicroelectronics Community)
For the "FIFO Mode" the code is:
/* Includes ------------------------------------------------------------------*/
#include "main.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "lsm6dsv16x_reg.h" // Include driver del sensore
#include <stdio.h>
#include <string.h>
/* USER CODE END Includes */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define FIFO_WATERMARK1 200
#define FIFO_WATERMARK2 200
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
I2C_HandleTypeDef hi2c1;
UART_HandleTypeDef huart2;
/* USER CODE BEGIN PV */
static stmdev_ctx_t dev_ctx1; // Sensore 1
static stmdev_ctx_t dev_ctx2; // Sensore 2
static uint8_t tx_buffer1[1000]; // Buffer per la trasmissione UART sensore 1
static uint8_t tx_buffer2[1000]; // Buffer per la trasmissione UART sensore 2
static lsm6dsv16x_fifo_sflp_raw_t fifo_sflp1; // Istanza della struttura per le impostazioni del FIFO
static lsm6dsv16x_fifo_sflp_raw_t fifo_sflp2; // Istanza della struttura per le impostazioni del FIFO
static lsm6dsv16x_fifo_status_t fifo_status1;
static lsm6dsv16x_fifo_status_t fifo_status2;
float quaternion1[4]; // Array 1 per i dati del quaternione: w, x, y, z
float quaternion2[4]; // Array 2 per i dati del quaternione: w, x, y, z
float acc1[3];
float acc2[3];
float gyr1[3];
float gyr2[3];
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_USART2_UART_Init(void);
static void MX_I2C1_Init(void);
/* USER CODE BEGIN PFP */
int32_t platform_write(void* handle, uint8_t reg, uint8_t* bufp, uint16_t len);
int32_t platform_read(void* handle, uint8_t reg, uint8_t* bufp, uint16_t len);
void platform_delay(uint32_t ms);
static void tx_com(uint8_t* tx_buffer, uint16_t len);
void initialize_sensors();
void set_sensors();
void reset_fifos();
void get_data();
void read_sensors_data();
static void sflp2q(float quat[4], uint16_t sflp[3]);
static float_t npy_half_to_float(uint16_t h);
static uint32_t npy_halfbits_to_floatbits(uint16_t h);
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_USART2_UART_Init();
MX_I2C1_Init();
/* USER CODE BEGIN 2 */
initialize_sensors();
set_sensors();
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
// Controlla lo stato del watermark per entrambi i sensori
lsm6dsv16x_fifo_status_get(&dev_ctx1, &fifo_status1);
lsm6dsv16x_fifo_status_get(&dev_ctx2, &fifo_status2);
if (fifo_status1.fifo_th == 1 && fifo_status2.fifo_th == 1) {
// Entrambi i sensori hanno raggiunto il livello di watermark
get_data();
reset_fifos();
}
if (fifo_status1.fifo_ovr || fifo_status2.fifo_ovr) {
HAL_UART_Transmit(&huart2, (uint8_t*)"FIFO Overflow detected! Resetting...\n", 38, HAL_MAX_DELAY);
reset_fifos();
continue;
}
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}
/**
* @brief System Clock Configuration
* @retval None
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
/** Configure the main internal regulator output voltage
*/
__HAL_RCC_PWR_CLK_ENABLE();
__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE2);
/** Initializes the RCC Oscillators according to the specified parameters
* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
RCC_OscInitStruct.PLL.PLLM = 16;
RCC_OscInitStruct.PLL.PLLN = 336;
RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV4;
RCC_OscInitStruct.PLL.PLLQ = 7;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
Error_Handler();
}
/** Initializes the CPU, AHB and APB buses clocks
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
{
Error_Handler();
}
}
/**
* @brief I2C1 Initialization Function
* None
* @retval None
*/
static void MX_I2C1_Init(void)
{
/* USER CODE BEGIN I2C1_Init 0 */
/* USER CODE END I2C1_Init 0 */
/* USER CODE BEGIN I2C1_Init 1 */
/* USER CODE END I2C1_Init 1 */
hi2c1.Instance = I2C1;
hi2c1.Init.ClockSpeed = 100000;
hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
hi2c1.Init.OwnAddress1 = 0;
hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
hi2c1.Init.OwnAddress2 = 0;
hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
if (HAL_I2C_Init(&hi2c1) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN I2C1_Init 2 */
/* USER CODE END I2C1_Init 2 */
}
/**
* @brief USART2 Initialization Function
* None
* @retval None
*/
static void MX_USART2_UART_Init(void)
{
/* USER CODE BEGIN USART2_Init 0 */
/* USER CODE END USART2_Init 0 */
/* USER CODE BEGIN USART2_Init 1 */
/* USER CODE END USART2_Init 1 */
huart2.Instance = USART2;
huart2.Init.BaudRate = 912600;
huart2.Init.WordLength = UART_WORDLENGTH_8B;
huart2.Init.StopBits = UART_STOPBITS_1;
huart2.Init.Parity = UART_PARITY_NONE;
huart2.Init.Mode = UART_MODE_TX_RX;
huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart2.Init.OverSampling = UART_OVERSAMPLING_16;
if (HAL_UART_Init(&huart2) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN USART2_Init 2 */
/* USER CODE END USART2_Init 2 */
}
/**
* @brief GPIO Initialization Function
* None
* @retval None
*/
static void MX_GPIO_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
/* USER CODE BEGIN MX_GPIO_Init_1 */
/* USER CODE END MX_GPIO_Init_1 */
/* GPIO Ports Clock Enable */
__HAL_RCC_GPIOC_CLK_ENABLE();
__HAL_RCC_GPIOH_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(LD2_GPIO_Port, LD2_Pin, GPIO_PIN_RESET);
/*Configure GPIO pin : B1_Pin */
GPIO_InitStruct.Pin = B1_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(B1_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pin : LD2_Pin */
GPIO_InitStruct.Pin = LD2_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(LD2_GPIO_Port, &GPIO_InitStruct);
/* USER CODE BEGIN MX_GPIO_Init_2 */
/* USER CODE END MX_GPIO_Init_2 */
}
/* USER CODE BEGIN 4 */
void initialize_sensors()
{
// Configurazione sensore 1
static uint8_t sensor1_addr = 0x6B << 1; // Indirizzo I2C del sensore 1
dev_ctx1.write_reg = platform_write;
dev_ctx1.read_reg = platform_read;
dev_ctx1.mdelay = platform_delay;
dev_ctx1.handle = &sensor1_addr;
uint8_t whoamI1 = 0x00;
if (lsm6dsv16x_device_id_get(&dev_ctx1, &whoamI1) != 0) {
HAL_UART_Transmit(&huart2, (uint8_t*)"Errore: lettura fallita sul sensore 1\r\n", 40, HAL_MAX_DELAY);
while (1);
}
if (whoamI1 != LSM6DSV16X_ID) {
char msg[64];
snprintf(msg, sizeof(msg), "Errore: Sensore 1 risponde con ID %02X, atteso %02X\r\n", whoamI1, LSM6DSV16X_ID);
HAL_UART_Transmit(&huart2, (uint8_t*)msg, strlen(msg), HAL_MAX_DELAY);
while (1);
}
HAL_UART_Transmit(&huart2, (uint8_t*)"Sensore 1 inizializzato correttamente\r\n", 40, HAL_MAX_DELAY);
// Configurazione sensore 2
static uint8_t sensor2_addr = 0x6A << 1; // Indirizzo I2C del sensore 2
dev_ctx2.write_reg = platform_write;
dev_ctx2.read_reg = platform_read;
dev_ctx2.mdelay = platform_delay;
dev_ctx2.handle = &sensor2_addr;
uint8_t whoamI2 = 0x00;
if (lsm6dsv16x_device_id_get(&dev_ctx2, &whoamI2) != 0) {
HAL_UART_Transmit(&huart2, (uint8_t*)"Errore: lettura fallita sul sensore 2\r\n", 40, HAL_MAX_DELAY);
while (1);
}
if (whoamI2 != LSM6DSV16X_ID) {
char msg[64];
snprintf(msg, sizeof(msg), "Errore: Sensore 2 risponde con ID %02X, atteso %02X\r\n", whoamI2, LSM6DSV16X_ID);
HAL_UART_Transmit(&huart2, (uint8_t*)msg, strlen(msg), HAL_MAX_DELAY);
while (1);
}
HAL_UART_Transmit(&huart2, (uint8_t*)"Sensore 2 inizializzato correttamente\r\n", 40, HAL_MAX_DELAY);
}
void set_sensors()
{
// Restore default configuration sensore 1
lsm6dsv16x_reset_t rst1;
lsm6dsv16x_reset_set(&dev_ctx1, LSM6DSV16X_RESTORE_CTRL_REGS);
do {
lsm6dsv16x_reset_get(&dev_ctx1, &rst1);
} while (rst1 != LSM6DSV16X_READY);
// Restore default configuration sensore 2
lsm6dsv16x_reset_t rst2;
lsm6dsv16x_reset_set(&dev_ctx2, LSM6DSV16X_RESTORE_CTRL_REGS);
do {
lsm6dsv16x_reset_get(&dev_ctx2, &rst2);
} while (rst2 != LSM6DSV16X_READY);
// Abilita l'aggiornamento dei dati sensore 1
lsm6dsv16x_block_data_update_set(&dev_ctx1, PROPERTY_ENABLE);
// Abilita l'aggiornamento dei dati sensore 2
lsm6dsv16x_block_data_update_set(&dev_ctx2, PROPERTY_ENABLE);
// Imposta i range di full scale sensore 1
lsm6dsv16x_xl_full_scale_set(&dev_ctx1, LSM6DSV16X_4g);
lsm6dsv16x_gy_full_scale_set(&dev_ctx1, LSM6DSV16X_2000dps);
// Imposta i range di full scale sensore 2
lsm6dsv16x_xl_full_scale_set(&dev_ctx2, LSM6DSV16X_4g);
lsm6dsv16x_gy_full_scale_set(&dev_ctx2, LSM6DSV16X_2000dps);
// Imposta la FIFO watermark sensore 1
lsm6dsv16x_fifo_watermark_set(&dev_ctx1, FIFO_WATERMARK1);
lsm6dsv16x_fifo_stop_on_wtm_set(&dev_ctx1, PROPERTY_ENABLE);// Abilita STOP_ON_WTM sensore 1
// Imposta la FIFO watermark sensore 2
lsm6dsv16x_fifo_watermark_set(&dev_ctx2, FIFO_WATERMARK2);
lsm6dsv16x_fifo_stop_on_wtm_set(&dev_ctx2, PROPERTY_ENABLE);// Abilita STOP_ON_WTM sensore 2
// Imposta batch FIFO di dati sflp sensore 1
fifo_sflp1.game_rotation = 1;
fifo_sflp1.gravity = 1;
fifo_sflp1.gbias = 1;
lsm6dsv16x_fifo_sflp_batch_set(&dev_ctx1, fifo_sflp1);
// Imposta batch FIFO di dati sflp sensore 2
fifo_sflp2.game_rotation = 1;
fifo_sflp2.gravity = 1;
fifo_sflp2.gbias = 1;
lsm6dsv16x_fifo_sflp_batch_set(&dev_ctx2, fifo_sflp2);
// Abilita il FIFO e impostalo in modalità FIFO MODE sensore 1
lsm6dsv16x_fifo_mode_set(&dev_ctx1, LSM6DSV16X_FIFO_MODE);
// Abilita il FIFO e impostalo in modalità FIFO MODE sensore 2
lsm6dsv16x_fifo_mode_set(&dev_ctx2, LSM6DSV16X_FIFO_MODE);
// Imposta batch FIFO XL/Gyro ODR sensore 1
lsm6dsv16x_fifo_xl_batch_set(&dev_ctx1, LSM6DSV16X_XL_BATCHED_AT_120Hz);
lsm6dsv16x_fifo_gy_batch_set(&dev_ctx1, LSM6DSV16X_GY_BATCHED_AT_120Hz);
// Imposta batch FIFO XL/Gyro ODR sensore 2
lsm6dsv16x_fifo_xl_batch_set(&dev_ctx2, LSM6DSV16X_XL_BATCHED_AT_120Hz);
lsm6dsv16x_fifo_gy_batch_set(&dev_ctx2, LSM6DSV16X_GY_BATCHED_AT_120Hz);
// Imposta ODR per accelerometro e giroscopio sensore 1
lsm6dsv16x_xl_data_rate_set(&dev_ctx1, LSM6DSV16X_ODR_AT_120Hz); // ODR dell'accelerometro
lsm6dsv16x_gy_data_rate_set(&dev_ctx1, LSM6DSV16X_ODR_AT_120Hz); // ODR del giroscopio
lsm6dsv16x_sflp_data_rate_set(&dev_ctx1, LSM6DSV16X_SFLP_120Hz);
lsm6dsv16x_sflp_game_rotation_set(&dev_ctx1, PROPERTY_ENABLE);
// Imposta ODR per accelerometro e giroscopio sensore 2
lsm6dsv16x_xl_data_rate_set(&dev_ctx2, LSM6DSV16X_ODR_AT_120Hz); // ODR dell'accelerometro
lsm6dsv16x_gy_data_rate_set(&dev_ctx2, LSM6DSV16X_ODR_AT_120Hz); // ODR del giroscopio
lsm6dsv16x_sflp_data_rate_set(&dev_ctx2, LSM6DSV16X_SFLP_120Hz);
lsm6dsv16x_sflp_game_rotation_set(&dev_ctx2, PROPERTY_ENABLE);
// SFLP GBIAS value sensore 1
lsm6dsv16x_sflp_gbias_t gbias1;
gbias1.gbias_x = 0.0f;
gbias1.gbias_y = 0.0f;
gbias1.gbias_z = 0.0f;
lsm6dsv16x_sflp_game_gbias_set(&dev_ctx1, &gbias1);
// SFLP GBIAS value sensore 2
lsm6dsv16x_sflp_gbias_t gbias2;
gbias2.gbias_x = 0.0f;
gbias2.gbias_y = 0.0f;
gbias2.gbias_z = 0.0f;
lsm6dsv16x_sflp_game_gbias_set(&dev_ctx2, &gbias2);
// Imposta il filtro anti-spike sensore 1
lsm6dsv16x_filt_anti_spike_t anti_spike_mode = LSM6DSV16X_ALWAYS_ACTIVE;
lsm6dsv16x_filt_anti_spike_set(&dev_ctx1, anti_spike_mode);
// Imposta il filtro anti-spike sensore 2
lsm6dsv16x_filt_anti_spike_set(&dev_ctx2, anti_spike_mode);
// Controlla se il filtro anti-spike è attivo sensore 1
lsm6dsv16x_filt_anti_spike_t anti_spike_status;
lsm6dsv16x_filt_anti_spike_get(&dev_ctx1, &anti_spike_status);
if (anti_spike_status == LSM6DSV16X_ALWAYS_ACTIVE) {
snprintf((char*)tx_buffer1, sizeof(tx_buffer1), "Filtro anti-spike attivo.\r\n");
tx_com(tx_buffer1, strlen((char const*)tx_buffer1));
}
// Controlla se il filtro anti-spike è attivo sensore 2
lsm6dsv16x_filt_anti_spike_get(&dev_ctx2, &anti_spike_status);
if (anti_spike_status == LSM6DSV16X_ALWAYS_ACTIVE) {
snprintf((char*)tx_buffer2, sizeof(tx_buffer2), "Filtro anti-spike attivo.\r\n");
tx_com(tx_buffer2, strlen((char const*)tx_buffer2));
}
}
void reset_fifos() {
// Metti i sensori in modalità bypass per resettare il FIFO
lsm6dsv16x_fifo_mode_set(&dev_ctx1, LSM6DSV16X_BYPASS_MODE);
lsm6dsv16x_fifo_mode_set(&dev_ctx2, LSM6DSV16X_BYPASS_MODE);
HAL_Delay(1); // Aspetta un breve intervallo per il reset del FIFO
// Ripristina la modalità desiderata (es. FIFO o Continuous-to-FIFO)
lsm6dsv16x_fifo_mode_set(&dev_ctx1, LSM6DSV16X_FIFO_MODE);
lsm6dsv16x_fifo_mode_set(&dev_ctx2, LSM6DSV16X_FIFO_MODE);
}
void get_data()
{
uint16_t num_samples1 = fifo_status1.fifo_level;
uint16_t num_samples2 = fifo_status2.fifo_level;
while (num_samples1-- && num_samples2--) {
// Sensore 1
lsm6dsv16x_fifo_out_raw_t raw_data1;
lsm6dsv16x_fifo_out_raw_get(&dev_ctx1, &raw_data1);
// Sensore 2
lsm6dsv16x_fifo_out_raw_t raw_data2;
lsm6dsv16x_fifo_out_raw_get(&dev_ctx2, &raw_data2);
// Se i dati sono del Game Rotation Vector
if (raw_data1.tag == LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG) {
// Converti i dati grezzi in quaternioni
sflp2q(quaternion1, (uint16_t*)&raw_data1.data[0]);
snprintf((char*)tx_buffer1, sizeof(tx_buffer1),
"Game Rotation 1: X: %2.3f Y: %2.3f Z: %2.3f W: %2.3f\n",
quaternion1[0], quaternion1[1], quaternion1[2], quaternion1[3] );
tx_com(tx_buffer1, strlen((char const*)tx_buffer1));
}
else if (raw_data1.tag ==LSM6DSV16X_XL_NC_TAG)
{
acc1[0] = lsm6dsv16x_from_fs4_to_mg(*(int16_t*)&raw_data1.data[0]);
acc1[1] = lsm6dsv16x_from_fs4_to_mg(*(int16_t*)&raw_data1.data[2]);
acc1[2] = lsm6dsv16x_from_fs4_to_mg(*(int16_t*)&raw_data1.data[4]);
snprintf((char*)tx_buffer1, sizeof(tx_buffer1),
"ACC1 [mg]: %4.2f %4.2f %4.2f\n",
acc1[0], acc1[1], acc1[2] );
tx_com(tx_buffer1, strlen((char const*)tx_buffer1));
}
else if (raw_data1.tag == LSM6DSV16X_GY_NC_TAG)
{
gyr1[0] = lsm6dsv16x_from_fs2000_to_mdps(*(int16_t*)&raw_data1.data[0]);
gyr1[1] = lsm6dsv16x_from_fs2000_to_mdps(*(int16_t*)&raw_data1.data[2]);
gyr1[2] = lsm6dsv16x_from_fs2000_to_mdps(*(int16_t*)&raw_data1.data[4]);
snprintf((char*)tx_buffer1, sizeof(tx_buffer1),
"GYR1 [mdps]: %4.2f %4.2f %4.2f\n",
gyr1[0], gyr1[1], gyr1[2]);
tx_com(tx_buffer1, strlen((char const*)tx_buffer1));
}
// Sensore 2
if (raw_data2.tag == LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG) {
// Converti i dati grezzi in quaternioni
sflp2q(quaternion2, (uint16_t*)&raw_data2.data[0]);
snprintf((char*)tx_buffer2, sizeof(tx_buffer2),
"Game Rotation 2: X: %2.3f Y: %2.3f Z: %2.3f W: %2.3f\n",
quaternion2[0], quaternion2[1], quaternion2[2], quaternion2[3] );
tx_com(tx_buffer2, strlen((char const*)tx_buffer2));
}
else if (raw_data2.tag ==LSM6DSV16X_XL_NC_TAG)
{
acc2[0] = lsm6dsv16x_from_fs4_to_mg(*(int16_t*)&raw_data2.data[0]);
acc2[1] = lsm6dsv16x_from_fs4_to_mg(*(int16_t*)&raw_data2.data[2]);
acc2[2] = lsm6dsv16x_from_fs4_to_mg(*(int16_t*)&raw_data2.data[4]);
snprintf((char*)tx_buffer2, sizeof(tx_buffer2),
"ACC2 [mg]: %4.2f %4.2f %4.2f\n",
acc2[0], acc2[1], acc2[2] );
tx_com(tx_buffer2, strlen((char const*)tx_buffer2));
}
else if (raw_data2.tag == LSM6DSV16X_GY_NC_TAG)
{
gyr2[0] = lsm6dsv16x_from_fs2000_to_mdps(*(int16_t*)&raw_data2.data[0]);
gyr2[1] = lsm6dsv16x_from_fs2000_to_mdps(*(int16_t*)&raw_data2.data[2]);
gyr2[2] = lsm6dsv16x_from_fs2000_to_mdps(*(int16_t*)&raw_data2.data[4]);
snprintf((char*)tx_buffer2, sizeof(tx_buffer2),
"GYR2 [mdps]: %4.2f %4.2f %4.2f\n",
gyr2[0], gyr2[1], gyr2[2]);
tx_com(tx_buffer2, strlen((char const*)tx_buffer2));
}
}
}
int32_t platform_write(void* handle, uint8_t reg, uint8_t* bufp, uint16_t len)
{
uint8_t addr = *(uint8_t*)handle; // Ottieni l'indirizzo I2C dal contesto
return HAL_I2C_Mem_Write(&hi2c1, addr, reg, I2C_MEMADD_SIZE_8BIT, bufp, len, 1000);
}
int32_t platform_read(void* handle, uint8_t reg, uint8_t* bufp, uint16_t len)
{
uint8_t addr = *(uint8_t*)handle; // Ottieni l'indirizzo I2C dal contesto
return HAL_I2C_Mem_Read(&hi2c1, addr, reg, I2C_MEMADD_SIZE_8BIT, bufp, len, 1000);
}
void platform_delay(uint32_t ms)
{
HAL_Delay(ms);
}
static void tx_com(uint8_t* tx_buffer, uint16_t len)
{
HAL_UART_Transmit(&huart2, tx_buffer, len, 1000);
}
static uint32_t npy_halfbits_to_floatbits(uint16_t h)
{
uint16_t h_exp, h_sig;
uint32_t f_sgn, f_exp, f_sig;
h_exp = (h & 0x7c00u);
f_sgn = ((uint32_t)h & 0x8000u) << 16;
switch (h_exp) {
case 0x0000u: /* 0 or subnormal */
h_sig = (h & 0x03ffu);
/* Signed zero */
if (h_sig == 0) {
return f_sgn;
}
/* Subnormal */
h_sig <<= 1;
while ((h_sig & 0x0400u) == 0) {
h_sig <<= 1;
h_exp++;
}
f_exp = ((uint32_t)(127 - 15 - h_exp)) << 23;
f_sig = ((uint32_t)(h_sig & 0x03ffu)) << 13;
return f_sgn + f_exp + f_sig;
case 0x7c00u: /* inf or NaN */
/* All-ones exponent and a copy of the significand */
return f_sgn + 0x7f800000u + (((uint32_t)(h & 0x03ffu)) << 13);
default: /* normalized */
/* Just need to adjust the exponent and shift */
return f_sgn + (((uint32_t)(h & 0x7fffu) + 0x1c000u) << 13);
}
}
static float_t npy_half_to_float(uint16_t h)
{
union { float_t ret; uint32_t retbits; } conv;
conv.retbits = npy_halfbits_to_floatbits(h);
return conv.ret;
}
static void sflp2q(float quat[4], uint16_t sflp[3])
{
float sumsq = 0;
quat[0] = npy_half_to_float(sflp[0]);
quat[1] = npy_half_to_float(sflp[1]);
quat[2] = npy_half_to_float(sflp[2]);
for (uint8_t i = 0; i < 3; i++)
sumsq += quat[i] * quat[i];
if (sumsq > 1.0f) {
float n = sqrtf(sumsq);
quat[0] /= n;
quat[1] /= n;
quat[2] /= n;
sumsq = 1.0f;
}
quat[3] = sqrtf(1.0f - sumsq);
}
/* USER CODE END 4 */
/**
* @brief This function is executed in case of error occurrence.
* @retval None
*/
void Error_Handler(void)
{
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
__disable_irq();
while (1)
{
}
/* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
* @brief Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* file: pointer to the source file name
* line: assert_param error line source number
* @retval None
*/
void assert_failed(uint8_t *file, uint32_t line)
{
/* USER CODE BEGIN 6 */
/* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
I have acquired both FIFO_WATERMARK set to 64 and 200, but the results I get are not satisfactory. Unlike "Continuous Mode", with "FIFO Mode" there is less lag between the data from the two sensors:
Furthermore, trying to obtain the angles from the acquired data I get the following graphs:
So I don't know which of the two modes to use because both still have problems and I don't know how to get the best data.
