陀螺仪LSM6DSV16X与AI集成.13--中断获取SFLP四元数
概述
本文将介绍如何通过中断机制获取 LSM6DSV16X 传感器的 SFLP(Sensor Fusion Low Power)四元数数据。LSM6DSV16X 是一款高性能的 6 轴惯性传感器,支持低功耗传感器融合(SFLP)功能。SFLP 功能允许在低功耗模式下实时融合加速度计和陀螺仪数据,以生成设备姿态的四元数表示。
为了优化系统功耗,我们将通过配置中断引脚,在四元数数据更新时触发中断。这样可以避免频繁轮询传感器数据,从而降低功耗,尤其适用于需要实时姿态检测但对功耗敏感的场景,例如可穿戴设备和手势识别系统。
视频教学
https://www.bilibili.com/video/BV1ic1fYjEj2/
<iframe allowfullscreen="true" data-mediaembed="bilibili" frameborder="0" id="OBeS8iox-1728193008481" src="https://player.bilibili.com/player.html?aid=113256458881074"></iframe>陀螺仪LSM6DSV16X与AI集成(14)----上报匿名上位机
样品申请
https://www.wjx.top/vm/OhcKxJk.aspx#
最近在弄ST的课程,需要样片的可以加群申请:615061293 。
源码下载
硬件准备
首先需要准备一个开发板,这里我准备的是自己绘制的开发板,需要的可以进行申请。
主控为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 */
}
需要注意优化等级。