陀螺仪LSM6DSV16X与AI集成(13)----中断获取SFLP四元数

记帖

已于 2024-10-09 09:57:25 修改

阅读量1.1w

点赞数 25

分类专栏：单片机传感器 stm32cube 文章标签： LSM6DSV16X 陀螺仪四元数欧拉角低功耗传感器融合（SFLP） x-cube-mems1 stm32h503cb

于 2024-10-06 13:40:48 首次发布

本文链接：https://blog.csdn.net/qq_24312945/article/details/142726166

版权

单片机同时被 3 个专栏收录

309 篇文章

订阅专栏

stm32cube

188 篇文章

订阅专栏

传感器

135 篇文章

订阅专栏

概述

本文将介绍如何通过中断机制获取 LSM6DSV16X 传感器的 SFLP（Sensor Fusion Low Power）四元数数据。LSM6DSV16X 是一款高性能的 6 轴惯性传感器，支持低功耗传感器融合（SFLP）功能。SFLP 功能允许在低功耗模式下实时融合加速度计和陀螺仪数据，以生成设备姿态的四元数表示。
为了优化系统功耗，我们将通过配置中断引脚，在四元数数据更新时触发中断。这样可以避免频繁轮询传感器数据，从而降低功耗，尤其适用于需要实时姿态检测但对功耗敏感的场景，例如可穿戴设备和手势识别系统。

在这里插入图片描述

视频教学

https://www.bilibili.com/video/BV1ic1fYjEj2/

陀螺仪LSM6DSV16X与AI集成(14)----上报匿名上位机

样品申请

https://www.wjx.top/vm/OhcKxJk.aspx#

最近在弄ST的课程，需要样片的可以加群申请：615061293 。

源码下载

https://download.csdn.net/download/qq_24312945/89851770

硬件准备

首先需要准备一个开发板，这里我准备的是自己绘制的开发板，需要的可以进行申请。
主控为STM32H503CB，陀螺仪为LSM6DSV16X，磁力计为LIS2MDL。

在这里插入图片描述

SFLP

LSM6DSV16X 特性涉及到的是一种低功耗的传感器融合算法（Sensor Fusion Low Power, SFLP）.
低功耗传感器融合（SFLP）算法：
该算法旨在以节能的方式结合加速度计和陀螺仪的数据。传感器融合算法通过结合不同传感器的优势，提供更准确、可靠的数据。
6轴游戏旋转向量：
SFLP算法能够生成游戏旋转向量。这种向量是一种表示设备在空间中方向的数据，特别适用于游戏和增强现实应用，这些应用中理解设备的方向和运动非常关键。
四元数表示法：
旋转向量以四元数的形式表示。四元数是一种编码3D旋转的方法，它避免了欧拉角等其他表示法的一些限制（如万向节锁）。一个四元数有四个分量（X, Y, Z 和 W），其中 X, Y, Z 代表向量部分，W 代表标量部分。
FIFO存储：
四元数的 X, Y, Z 分量存储在 LSM6DSV16X 的 FIFO（先进先出）缓冲区中。FIFO 缓冲区是一种数据存储方式，允许临时存储传感器数据。这对于有效管理数据流非常有用，特别是在数据处理可能不如数据收集那么快的系统中。

在这里插入图片描述

图片包含了关于 LSM6DSV16X 传感器的低功耗传感器融合（Sensor Fusion Low Power, SFLP）功能的说明。这里是对图片内容的解释：
SFLP 功能：

SFLP 单元用于生成基于加速度计和陀螺仪数据处理的以下数据：
游戏旋转向量：以四元数形式表示设备的姿态。
重力向量：提供一个三维向量，表示重力方向。
陀螺仪偏差：提供一个三维向量，表示陀螺仪的偏差。
激活与重置：
通过在 EMB_FUNC_EN_A（04h）嵌入式功能寄存器中设置 SFLP_GAME_EN 位为 1 来激活 SFLP 单元。
通过在 EMB_FUNC_INIT_A（66h）嵌入式功能寄存器中设置 SFLP_GAME_INIT 位为 1 来重置 SFLP 单元。
性能参数表：
表格展示了 SFLP 功能在不同情况下的性能，包括静态精度、低动态精度和高动态精度，以及校准时间和方向稳定时间。这些参数反映了传感器在不同运动状态下的精确度和响应速度。

在这里插入图片描述

开启INT中断

陀螺仪LSM6DSV16X的中断管脚接到了PB0，需要将PB0设置为中端口。
在这里插入图片描述

在这里插入图片描述
开启中断。

在这里插入图片描述

中断读取传感器数据

INT1_CTRL (0Dh) 是 LSM6DSV16X 传感器的中断控制寄存器，用于配置和启用 INT1 引脚的各种中断信号。该寄存器的每一位对应于不同的中断源，通过设置这些位可以启用或禁用相应的中断信号。
INT1_FIFO_TH (bit 3)：
● 启用 FIFO 阈值中断，将其路由到 INT1 引脚。当 FIFO 达到设定的阈值时触发该中断。默认值为 0（禁用）。

在这里插入图片描述

mian.c中定义变量。

/* USER CODE BEGIN 0 */
uint8_t fifo_flag = 0;
/* USER CODE END 0 */

mian.c中开启中断。

	lsm6dsv16x_pin_int_route_t pin_int;	
  pin_int.fifo_th = PROPERTY_ENABLE;
  lsm6dsv16x_pin_int1_route_set(&dev_ctx, &pin_int);

在stm32h5xx_it.c中添加回调函数引用。

/* USER CODE BEGIN 0 */
extern void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin);


/* USER CODE END 0 */

处理PB0外部中断线0（EXTI Line0）的中断。

/**
  * @brief This function handles EXTI Line0 interrupt.
  */
void EXTI0_IRQHandler(void)
{
  /* USER CODE BEGIN EXTI0_IRQn 0 */
	HAL_GPIO_EXTI_Callback(INT1_Pin);
  /* USER CODE END EXTI0_IRQn 0 */
  HAL_GPIO_EXTI_IRQHandler(INT1_Pin);
  /* USER CODE BEGIN EXTI0_IRQn 1 */

  /* USER CODE END EXTI0_IRQn 1 */
}

在main.c中添加回调函数的定义，检查中断是否由 GPIO_PIN_0引脚触发。

/* USER CODE BEGIN 4 */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin){
	if(GPIO_Pin == GPIO_PIN_0)
	{
		mlc_flag=1;
		}	
}
/* USER CODE END 4 */

主程序

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
		
		if(mlc_flag==1)
		{
			mlc_flag=0;
			uint16_t num = 0;

    /* Read watermark flag */
    lsm6dsv16x_fifo_status_get(&dev_ctx, &fifo_status);
    if (fifo_status.fifo_th == 1) {
      num = fifo_status.fifo_level;

      printf( "-- FIFO num %d \r\n", num);

      while (num--) {
        lsm6dsv16x_fifo_out_raw_t f_data;
        int16_t *axis;
        float quat[4];
        float gravity_mg[3];
        float gbias_mdps[3];

        /* Read FIFO sensor value */
        lsm6dsv16x_fifo_out_raw_get(&dev_ctx, &f_data);

        switch (f_data.tag) {
//        case LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG:
//          axis = (int16_t *)&f_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]);
//          printf("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]);

//          break;
//        case LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG:
//          axis = (int16_t *)&f_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]);
//          printf("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]);

//          break;
        case LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG:
          sflp2q(quat, (uint16_t *)&f_data.data[0]);
//          printf("Game Rotation \tX: %2.3f\tY: %2.3f\tZ: %2.3f\tW: %2.3f\r\n",
//                  (double_t)quat[0], (double_t)quat[1], (double_t)quat[2], (double_t)quat[3]);
				
					float sx=quat[1];  
					float sy=quat[2];  
					float sz=quat[0];  
					float sw=quat[3];
				
					if (sw< 0.0f) 
					{
						sx*=-1.0f;
						sy*=-1.0f;
						sz*=-1.0f;
						sw*=-1.0f;
					}
				
					float sqx = sx * sx;
					float sqy = sy * sy;
					float sqz = sz * sz;
					float euler[3];
					euler[0] = -atan2f(2.0f* (sy*sw+sx*sz), 1.0f-2.0f*(sqy+sqx));
					euler[1] = -atan2f(2.0f * (sx*sy+sz*sw),1.0f-2.0f*(sqx+sqz));
					euler[2] = -asinf(2.0f* (sx*sw-sy*sz));
				
					if (euler[0] <0.0f)
						euler[0] +=2.0f*3.1415926;
					
					for(uint8_t i=0; i<3; i++){
							euler[i] = 57.29578 * (euler[i]);
					}
					
					printf("euler[0]=%f,euler[1]=%f,euler[2]=%f\n",euler[0],euler[1],euler[2]);
												
					
          break;
        default:
         break;
        }
      }

    }				
		
		
		
		}  
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}