FAQs
cancel
Turn on suggestions
Auto-suggest helps you quickly narrow down your search results by suggesting possible matches as you type.
Showing results for 
Search instead for 
Did you mean: 

Issue with Misaligned Accelerometer and Gyroscope Samples on LSM6DSOX FIFO

Rturo
Associate II

‎2024-10-12 06:14 AM

Hello, I am using the LSM6DSOX sensor and reading data from the FIFO, following these examples on GitHub (https://github.com/STMicroelectronics/STMems_Standard_C_drivers/blob/master/lsm6dsox_STdC/examples/lsm6dsox_fifo.c). I've set the same frequency for both the accelerometer and gyroscope, but when I read the data from the FIFO in continuous mode, the samples are not aligned. For example, I receive the first two accelerometer samples followed by one gyroscope sample, then three accelerometer samples, two gyroscope samples, and so on.

Since the frequencies are the same, shouldn't I receive one gyroscope sample and one accelerometer sample consecutively in a 1:1 ratio? Is this behavior normal? Or is it necessary to adjust the accelerometer's configuration in some way to ensure that the samples are aligned? If so, how should I do that?

Thank you!

0 Kudos
2 REPLIES 2
Federica Bossi
ST Employee

‎2024-10-23 11:41 PM

Hi @Rturo ,

If you followed that example, having set the same ODR and the same Batch Data Rate for both sensors, the stored values for accelerometer and gyroscope should have the same frequency. Make sure to check the sensor configuration so that you have matching values in both cases. 

Did you try changing the Watermark to 1 to see what is the behaviour in this case?

In order to give better visibility on the answered topics, please click on 'Accept as Solution' on the reply which solved your issue or answered your question.
0 Kudos
marti3110
Associate III
In response to Federica Bossi

‎2024-12-10 07:50 AM

I have the same problem with LSM6DSV16X. I tried to change the watermark to 1 and the behaviour is always the same.

This is my code (I use 2 IMU)

/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * @file           : main.c
 * @brief          : Main program body
 ******************************************************************************
 * @attention
 *
 * Copyright (c) 2024 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.
 *
 ******************************************************************************
 */
/* USER CODE END Header */
/* 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 typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

// Il flag del watermark si attiva quando il numero di byte scritti nel FIFO
// è maggiore o uguale al livello di soglia.
#define FIFO_WATERMARK1    1
#define FIFO_WATERMARK2    1
/* 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[2000];  				// Buffer per la trasmissione UART sensore 1
static uint8_t tx_buffer2[2000];  				// 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
static int16_t *datax1;
static int16_t *datay1;
static int16_t *dataz1;

float quaternion2[4]; 							// Array 2 per i dati del quaternione: w, x, y, z
static int16_t *datax2;
static int16_t *datay2;
static int16_t *dataz2;

float acc1[3];
float acc2[3];
float gyr1[3];
float gyr2[3];


static lsm6dsv16x_filt_settling_mask_t filt_settling_mask;

/* 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 get_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 FIFO
		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) {
			// Leggi i dati dal FIFO
			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_data;
				lsm6dsv16x_fifo_out_raw_get(&dev_ctx1, &raw_data);
				datax1=(int16_t *)&raw_data.data[0];
				datay1=(int16_t *)&raw_data.data[2];
				dataz1=(int16_t *)&raw_data.data[4];

				// Sensore 2
				lsm6dsv16x_fifo_out_raw_t raw_data2;
				lsm6dsv16x_fifo_out_raw_get(&dev_ctx2, &raw_data2);
				datax2=(int16_t *)&raw_data2.data[0];
				datay2=(int16_t *)&raw_data2.data[2];
				dataz2=(int16_t *)&raw_data2.data[4];

				// 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(quaternion1, (uint16_t*)&raw_data.data[0]);

					// Invia i dati via UART
					snprintf((char*)tx_buffer1, sizeof(tx_buffer1), "Game Rotation 1 \tX: %2.3f\tY: %2.3f\tZ: %2.3f\tW: %2.3f\r\n",
							quaternion1[0], quaternion1[1], quaternion1[2], quaternion1[3]);
					tx_com(tx_buffer1, strlen((char const*)tx_buffer1));
				}
				else if (raw_data.tag ==LSM6DSV16X_XL_NC_TAG)
				{
					snprintf((char *)tx_buffer1, sizeof(tx_buffer1), "ACC1 [mg]:\t%4.2f\t%4.2f\t%4.2f\r\n",
							lsm6dsv16x_from_fs4_to_mg(*datax1),
							lsm6dsv16x_from_fs4_to_mg(*datay1),
							lsm6dsv16x_from_fs4_to_mg(*dataz1));
					tx_com(tx_buffer1, strlen((char const *)tx_buffer1));
				}
				else if (raw_data.tag == LSM6DSV16X_GY_NC_TAG)
				{
					snprintf((char *)tx_buffer1, sizeof(tx_buffer1), "GYR1 [mdps]:\t%4.2f\t%4.2f\t%4.2f\r\n",
							lsm6dsv16x_from_fs2000_to_mdps(*datax1),
							lsm6dsv16x_from_fs2000_to_mdps(*datay1),
							lsm6dsv16x_from_fs2000_to_mdps(*dataz1));
					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]);

					// Invia i dati via UART
					snprintf((char*)tx_buffer2, sizeof(tx_buffer2), "Game Rotation 2\tX: %2.3f\tY: %2.3f\tZ: %2.3f\tW: %2.3f\r\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)
				{
					snprintf((char *)tx_buffer2, sizeof(tx_buffer2), "ACC2 [mg]:\t%4.2f\t%4.2f\t%4.2f\r\n",
							lsm6dsv16x_from_fs4_to_mg(*datax2),
							lsm6dsv16x_from_fs4_to_mg(*datay2),
							lsm6dsv16x_from_fs4_to_mg(*dataz2));
					tx_com(tx_buffer2, strlen((char const *)tx_buffer2));
				}
				else if (raw_data2.tag == LSM6DSV16X_GY_NC_TAG)
				{
					snprintf((char *)tx_buffer2, sizeof(tx_buffer2), "GYR2 [mdps]:\t%4.2f\t%4.2f\t%4.2f\r\n",
							lsm6dsv16x_from_fs2000_to_mdps(*datax2),
							lsm6dsv16x_from_fs2000_to_mdps(*datay2),
							lsm6dsv16x_from_fs2000_to_mdps(*dataz2));
					tx_com(tx_buffer2, strlen((char const *)tx_buffer2));
				}


			}
		}
		/* 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
 * @PAram 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
 * @PAram 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
 * @PAram 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 1

	// 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);

	// 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);


	// Abilita il FIFO e impostalo in modalità streaming sensore 1
	// In modalità continua, se il FIFO è pieno, il campione appena raccolto sovrascrive quello vecchio
	lsm6dsv16x_fifo_mode_set(&dev_ctx1, LSM6DSV16X_STREAM_MODE);

	// Abilita il FIFO e impostalo in modalità streaming sensore 2
	lsm6dsv16x_fifo_mode_set(&dev_ctx2, LSM6DSV16X_STREAM_MODE);

	// 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));
	}

	/* Configure filtering chain */
	filt_settling_mask.drdy = PROPERTY_ENABLE;
	filt_settling_mask.irq_xl = PROPERTY_ENABLE;
	filt_settling_mask.irq_g = PROPERTY_ENABLE;

	/*lsm6dsv16x_filt_settling_mask_set(&dev_ctx1, filt_settling_mask);
	lsm6dsv16x_filt_gy_lp1_set(&dev_ctx1, PROPERTY_ENABLE);
	lsm6dsv16x_filt_gy_lp1_bandwidth_set(&dev_ctx1, LSM6DSV16X_GY_MEDIUM);
	lsm6dsv16x_filt_xl_lp2_set(&dev_ctx1, PROPERTY_ENABLE);
	lsm6dsv16x_filt_xl_lp2_bandwidth_set(&dev_ctx1, LSM6DSV16X_XL_STRONG);


	lsm6dsv16x_filt_settling_mask_set(&dev_ctx2, filt_settling_mask);
	lsm6dsv16x_filt_gy_lp1_set(&dev_ctx2, PROPERTY_ENABLE);
	lsm6dsv16x_filt_gy_lp1_bandwidth_set(&dev_ctx2, LSM6DSV16X_GY_MEDIUM);
	lsm6dsv16x_filt_xl_lp2_set(&dev_ctx2, PROPERTY_ENABLE);
	lsm6dsv16x_filt_xl_lp2_bandwidth_set(&dev_ctx2, LSM6DSV16X_XL_STRONG);*/


}
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.
 * @PAram  file: pointer to the source file name
 * @PAram  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 */

 

And the results are:

Game Rotation 2 X: 0.304        Y: 0.687        Z: -0.211       W: 0.625
Game Rotation 1         X: 0.296        Y: 0.654        Z: -0.241       W: 0.653
GYR2 [mdps]:    -700.00 210.00  -280.00
GYR1 [mdps]:    -210.00 210.00  350.00
ACC2 [mg]:      -989.42 91.74   -129.93
ACC1 [mg]:      -997.96 70.52   -28.91
Game Rotation 2 X: 0.304        Y: 0.687        Z: -0.211       W: 0.625
Game Rotation 1         X: 0.296        Y: 0.654        Z: -0.241       W: 0.653
GYR2 [mdps]:    -630.00 280.00  -210.00
GYR1 [mdps]:    -140.00 210.00  350.00
ACC2 [mg]:      -989.79 92.11   -128.71
ACC1 [mg]:      -998.08 70.15   -29.40
Game Rotation 2 X: 0.304        Y: 0.687        Z: -0.211       W: 0.625
Game Rotation 1         X: 0.296        Y: 0.654        Z: -0.241       W: 0.653
GYR2 [mdps]:    -700.00 210.00  -210.00
GYR1 [mdps]:    -140.00 210.00  350.00
ACC2 [mg]:      -988.69 91.62   -128.34
Game Rotation 1         X: 0.296        Y: 0.654        Z: -0.241       W: 0.653
Game Rotation 2 X: 0.304        Y: 0.687        Z: -0.211       W: 0.625
GYR1 [mdps]:    -210.00 140.00  350.00
GYR2 [mdps]:    -630.00 280.00  -210.00
ACC1 [mg]:      -998.08 70.52   -29.40
Game Rotation 2 X: 0.304        Y: 0.687        Z: -0.211       W: 0.625
Game Rotation 1         X: 0.296        Y: 0.654        Z: -0.241       W: 0.653
GYR2 [mdps]:    -630.00 280.00  -210.00
GYR1 [mdps]:    -210.00 280.00  350.00
ACC2 [mg]:      -989.79 92.23   -128.95
ACC1 [mg]:      -997.96 70.64   -29.28
Game Rotation 2 X: 0.304        Y: 0.687        Z: -0.211       W: 0.625
Game Rotation 1         X: 0.296        Y: 0.654        Z: -0.241       W: 0.653
GYR2 [mdps]:    -700.00 280.00  -210.00
GYR1 [mdps]:    -140.00 210.00  350.00
ACC2 [mg]:      -989.79 92.35   -128.95
Game Rotation 1         X: 0.296        Y: 0.654        Z: -0.241       W: 0.653
Game Rotation 2 X: 0.304        Y: 0.687        Z: -0.211       W: 0.625
GYR1 [mdps]:    -140.00 210.00  350.00
GYR2 [mdps]:    -700.00 210.00  -280.00
ACC1 [mg]:      -998.08 70.15   -29.40
ACC2 [mg]:      -989.18 92.35   -128.22
Game Rotation 1         X: 0.296        Y: 0.654        Z: -0.241       W: 0.653
Game Rotation 2 X: 0.304        Y: 0.687        Z: -0.211       W: 0.625
GYR1 [mdps]:    -140.00 210.00  350.00
GYR2 [mdps]:    -700.00 210.00  -210.00
ACC1 [mg]:      -997.96 70.88   -29.28
Game Rotation 2 X: 0.304        Y: 0.687        Z: -0.211       W: 0.625
Game Rotation 1         X: 0.296        Y: 0.654        Z: -0.241       W: 0.653
GYR2 [mdps]:    -700.00 280.00  -210.00
GYR1 [mdps]:    -140.00 280.00  350.00
ACC2 [mg]:      -989.54 91.87   -127.61
Game Rotation 1         X: 0.296        Y: 0.654        Z: -0.241       W: 0.653
Game Rotation 2 X: 0.304        Y: 0.687        Z: -0.211       W: 0.625
GYR1 [mdps]:    -210.00 210.00  350.00
GYR2 [mdps]:    -700.00 280.00  -210.00
ACC1 [mg]:      -997.926
Game Rotation 1         X: 0.296        Y: 0.654        Z: -0.241       W: 0.653
GYR2 [mdps]:    -700.00 210.00  -280.00
GYR1 [mdps]:    -210.00 210.00  350.00
ACC2 [mg]:      -989.79 91.99   -128.47
Game Rotation 1         X: 0.296        Y: 0.654        Z: -0.241       W: 0.653
Game Rotation 2 X: 0.304        Y: 0.687        Z: -0.210       W: 0.626
GYR1 [mdps]:    -140.00 210.00  350.00
GYR2 [mdps]:    -700.00 210.00  -280.00
ACC1 [mg]:      -997.47 70.27   -30.01
Game Rotation 2 X: 0.304        Y: 0.687        Z: -0.210       W: 0.626
Game Rotation 1         X: 0.296        Y: 0.654        Z: -0.241       W: 0.653
GYR2 [mdps]:    -630.00 210.00  -210.00
GYR1 [mdps]:    -140.00 210.00  350.00
ACC2 [mg]:      -989.54 92.35   -128.10
Game Rotation 2 X: 0.304        Y: 0.687        Z: -0.210       W: 0.626
Game Rotation 1         X: 0.296        Y: 0.654        Z: -0.241       W: 0.653
GYR2 [mdps]:    -700.00 280.00  -210.00
GYR1 [mdps]:    -140.00 210.00  350.00
ACC2 [mg]:      -989.18 92.23   -128.83
Game Rotation 1         X: 0.296        Y: 0.654        Z: -0.241       W: 0.653
Game Rotation 2 X: 0.304        Y: 0.687        Z: -0.210       W: 0.626
GYR1 [mdps]:    -210.00 210.00  350.00
GYR2 [mdps]:    -700.00 280.00  -210.00
ACC1 [mg]:      -997.72 70.39   -30.38
ACC2 [mg]:      -989.54 91.50   -128.59
Game Rotation 1         X: 0.296        Y: 0.654        Z: -0.241       W: 0.653
Game Rotation 2 X: 0.304        Y: 0.687        Z: -0.210       W: 0.626
GYR1 [mdps]:    -140.00 280.00  350.00
GYR2 [mdps]:    -700.00 210.00  -280.00
ACC1 [mg]:      -997.96 70.64   -29.04
ACC2 [mg]:      -989.42 91.74   -128.47
Game Rotation 1         X: 0.296        Y: 0.654        Z: -0.241       W: 0.653
Game Rotation 2 X: 0.304        Y: 0.687        Z: -0.210       W: 0.626
GYR1 [mdps]:    -140.00 210.00  350.00
GYR2 [mdps]:    -700.00 210.00  -210.00
Game Rotation 1         X: 0.296        Y: 0.654        Z: -0.241       W: 0.653
Game Rotation 2 X: 0.304        Y: 0.687        Z: -0.210       W: 0.626
GYR1 [mdps]:    -210.00 140.00  350.00
GYR2 [mdps]:    -700.00 210.00  -210.00
ACC1 [mg]:      -997.96 70.15   -30.26
ACC2 [mg]:      -990.15 92.35   -129.08
Game Rotation 1         X: 0.296        Y: 0.654        Z: -0.241       W: 0.653
Game Rotation 2 X: 0.304        Y: 0.687        Z: -0.210       W: 0.626
GYR1 [mdps]:    -140.00 210.00  350.00
GYR2 [mdps]:    -700.00 210.00  -280.00
ACC1 [mg]:      -997.72 70.27   -30.62
ACC2 [mg]:      -989.05 92.35   -128.10
Game Rotation 1         X: 0.296        Y: 0.654        Z: -0.241       W: 0.653
Game Rotation 2 X: 0.304        Y: 0.687        Z: -0.210       W: 0.626
GYR1 [mdps]:    -210.00 210.00  350.00
GYR2 [mdps]:    -700.00 210.00  -280.00
Game Rotation 1         X: 0.296        Y: 0.655        Z: -0.241       W: 0.652
ACC2 [mg]:      -988.69 92.48   -128.59
GYR1 [mdps]:    -140.00 210.00  280.00
Game Rotation 2 X: 0.304        Y: 0.687        Z: -0.210       W: 0.626
ACC1 [mg]:      -997.59 70.27   -30.50

 

So I have 21 Values of "Game Rotation ​​1" and only 13 values of "ACC1". How it is possible?

0 Kudos
Top