陀螺仪LSM6DSV16X与AI集成(9)----中断获取FIFO数据并应用MotionFX库解析空间坐标

嵌入式单片机MCU开发 2024-07-18 942

描述

概述

本文将探讨如何使用中断机制获取FIFO数据并应用MotionFX库解析空间坐标。MotionFX库是一种用于传感器融合的强大工具，可以将加速度计、陀螺仪和磁力计的数据融合在一起，实现精确的姿态和位置估计。本文将介绍如何初始化和配置MotionFX库，使用中断机制读取FIFO中的传感器数据。FIFO可以作为数据缓冲区，存储传感器的临时数据，防止数据丢失，特别是在处理器忙于其他任务时。本文将利用这些数据进行空间坐标的解析。本章案例基于上节的demo进行修改。

需要样片的可以加群申请：615061293 。

fifo

视频教学

[https://www.bilibili.com/video/BV1rx4y147Yv/]

样品申请

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

源码下载

[https://download.csdn.net/download/qq_24312945/89491135]

开启LED

配置PB14为输出模式。

fifo

开启INT中断

陀螺仪LSM6DSV16X的中断管脚接到了PA6，需要将PA6设置为中端口。

fifo

开启中断。

fifo

参考驱动程序

[https://github.com/STMicroelectronics/lsm6dsv16x-pid/tree/main]

复位 FIFO

设置 FIFO 进入 Bypass 模式：将 FIFO_CTRL4 寄存器的 FIFO_MODE 位设置为 000。
将 FIFO 重新设置为所需模式：在复位命令之后，可以通过将 FIFO_CTRL4 寄存器的 FIFO_MODE 位设置为所需的模式（例如 001 表示 FIFO 模式）。

fifo

lsm6dsv16x_fifo_mode_set(&dev_ctx, LSM6DSV16X_BYPASS_MODE);
    HAL_Delay(10);

添加到设置FIFO开启之前。

fifo

中断读取传感器数据

为了使用回调函数并获取FIFO中的数据，在main.c定义了以下变量。

/* USER CODE END 0 */
stmdev_ctx_t dev_ctx;
lsm6dsv16x_fifo_status_t fifo_status;

/// 用于存储FIFO中读取的数据，每条数据包含7个字节（1个标签字节和6个数据字节）
uint8_t fifo_data[FIFO_WATERMARK*2][7];
// FIFO中当前存储的数据数量
uint16_t fifo_num = 0;
// FIFO中断标志，用于标记是否有新的FIFO数据可供读取
uint8_t fifo_flag=0;

/* USER CODE END 0 */

mian.c中开启中断。

lsm6dsv16x_pin_int_route_t pin_int;
    lsm6dsv16x_pin_int1_route_get(&dev_ctx, &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 */

处理PA6外部中断线6（EXTI Line6）的中断。

/**
  * @brief This function handles EXTI Line6 interrupt.
  */
void EXTI6_IRQHandler(void)
{
  /* USER CODE BEGIN EXTI6_IRQn 0 */

HAL_GPIO_EXTI_Callback(GPIO_PIN_6);

  /* USER CODE END EXTI6_IRQn 0 */
  HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_6);
  /* USER CODE BEGIN EXTI6_IRQn 1 */

  /* USER CODE END EXTI6_IRQn 1 */
}

在main.c中添加回调函数的定义，检查中断是否由 GPIO_PIN_6 引脚触发，每次发生中断时从传感器获取当前的FIFO状态，并存储在 fifo_status 变量中。读取FIFO数据，并将这些数据存储在一个全局数组 fifo_data 中，以便在主循环或其他地方进行处理。通过切换 LED 的状态，可以直观地了解中断的发生。

/* USER CODE BEGIN 4 */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin){

    if(GPIO_Pin == GPIO_PIN_6)
    {
        HAL_GPIO_TogglePin(LED1_GPIO_Port, LED1_Pin);
        lsm6dsv16x_fifo_status_get(&dev_ctx, &fifo_status);
        uint16_t num = 0;
    if (fifo_status.fifo_th == 1) {
            fifo_flag=1;
      num = fifo_status.fifo_level;
            fifo_num=num;
//      printf( "-- FIFO num %d rn", num);    
            uint16_t num_i;
      for(int i=0;i< num;i++)  {
        lsm6dsv16x_fifo_out_raw_t f_data;

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

                fifo_data[i][0]=f_data.tag;
                fifo_data[i][1]=f_data.data[0];
                fifo_data[i][2]=f_data.data[1];
                fifo_data[i][3]=f_data.data[2];
                fifo_data[i][4]=f_data.data[3];
                fifo_data[i][5]=f_data.data[4];
                fifo_data[i][6]=f_data.data[5];                

//                printf("data[0]=%d,data[1]=%d,data[2]=%d,data[3]=%d,data[4]=%d,data[5]=%d,data[6]=%dn",
//                fifo_data[i][0],
//                fifo_data[i][1],fifo_data[i][2],fifo_data[i][3],fifo_data[i][4],fifo_data[i][5],fifo_data[i][6]);

      }    
        }    

    }
}
/* USER CODE END 4 */

主程序

在主循环中检查FIFO中断标志，如果有新的FIFO数据，则读取并处理这些数据。处理完成后，调用MotionFX库函数进行数据融合计算，以获得传感器的姿态和位置。

/* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {    
    if(fifo_flag)// 如果 FIFO 中断标志被设置
    {
        uint8_t acc_flag=0,gyr_flag=0;//加速度角速度标志位
        uint8_t deltatime_flag=0;//时间标志位
        for(int i=0;i< fifo_num;i++)// 遍历 FIFO 数据数组
        {
            // 获取数据指针
            datax = (int16_t *)&fifo_data[i][1];
            datay = (int16_t *)&fifo_data[i][3];
            dataz = (int16_t *)&fifo_data[i][5];
            ts = (int32_t *)&fifo_data[i][1];
            // 根据数据标签处理不同类型的数据
            switch (fifo_data[i][0]) {
        case LSM6DSV16X_XL_NC_TAG:// 加速度数据
                    acc_flag=1;
                    acc_x=lsm6dsv16x_from_fs4_to_mg(*datax);
                    acc_y=lsm6dsv16x_from_fs4_to_mg(*datay);
                    acc_z=lsm6dsv16x_from_fs4_to_mg(*dataz);                
          break;
        case LSM6DSV16X_GY_NC_TAG:// 角速度数据
                    gyr_flag=1;
                    gyr_x=lsm6dsv16x_from_fs4000_to_mdps(*datax);
                    gyr_y=lsm6dsv16x_from_fs4000_to_mdps(*datay);
                    gyr_z=lsm6dsv16x_from_fs4000_to_mdps(*dataz);                    
          break;                
        case LSM6DSV16X_TIMESTAMP_TAG:// 时间戳数据
                    deltatime_flag=1;
                    if(deltatime_first==0)//第一次
                    {
                        deltatime_1=*ts;
                        deltatime_2=deltatime_1;
                        deltatime_first=1;
                    }
                    else
                    {
                        deltatime_2=*ts;
                    }                
          break;
        default:
          break;                
            }
            // 如果加速度、角速度和时间戳数据都已获取
            if(acc_flag&&gyr_flag&&deltatime_flag)
            {
                lsm6dsv16x_motion_fx_determin();// 调用 MotionFX 处理函数
                acc_flag=0;
                gyr_flag=0;
                deltatime_flag=0;
                deltatime_1=deltatime_2;    // 更新时间戳        
            }            
        }
        // 清除 FIFO 标志和数据量
        fifo_flag=0;
        fifo_num=0;
        }    
//            HAL_Delay(10);

    /* USER CODE END WHILE */

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

演示

初始位置和数据输出如下所示。

fifo

逆时针旋转90°

fifo

逆时针旋转180°

fifo

逆时针旋转270°

fifo

审核编辑黄宇

打开APP阅读更多精彩内容