概述

本文將探討如何使用中斷機(jī)制獲取FIFO數(shù)據(jù)并應(yīng)用MotionFX庫(kù)解析空間坐標(biāo)。MotionFX庫(kù)是一種用于傳感器融合的強(qiáng)大工具，可以將加速度計(jì)、陀螺儀和磁力計(jì)的數(shù)據(jù)融合在一起，實(shí)現(xiàn)精確的姿態(tài)和位置估計(jì)。本文將介紹如何初始化和配置MotionFX庫(kù)，使用中斷機(jī)制讀取FIFO中的傳感器數(shù)據(jù)。FIFO可以作為數(shù)據(jù)緩沖區(qū)，存儲(chǔ)傳感器的臨時(shí)數(shù)據(jù)，防止數(shù)據(jù)丟失，特別是在處理器忙于其他任務(wù)時(shí)。本文將利用這些數(shù)據(jù)進(jìn)行空間坐標(biāo)的解析。本章案例基于上節(jié)的demo進(jìn)行修改。

需要樣片的可以加群申請(qǐng)：615061293 。

視頻教學(xué)

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

樣品申請(qǐng)

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

源碼下載

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

開(kāi)啟LED

配置PB14為輸出模式。

開(kāi)啟INT中斷

陀螺儀LSM6DSV16X的中斷管腳接到了PA6，需要將PA6設(shè)置為中端口。

開(kāi)啟中斷。

參考驅(qū)動(dòng)程序

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

復(fù)位 FIFO

1. 設(shè)置 FIFO 進(jìn)入 Bypass 模式：將 FIFO_CTRL4 寄存器的 FIFO_MODE 位設(shè)置為 000。
2. 將 FIFO 重新設(shè)置為所需模式：在復(fù)位命令之后，可以通過(guò)將 FIFO_CTRL4 寄存器的 FIFO_MODE 位設(shè)置為所需的模式（例如 001 表示 FIFO 模式）。

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

添加到設(shè)置FIFO開(kāi)啟之前。

中斷讀取傳感器數(shù)據(jù)

為了使用回調(diào)函數(shù)并獲取FIFO中的數(shù)據(jù)，在main.c定義了以下變量。

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

/// 用于存儲(chǔ)FIFO中讀取的數(shù)據(jù)，每條數(shù)據(jù)包含7個(gè)字節(jié)（1個(gè)標(biāo)簽字節(jié)和6個(gè)數(shù)據(jù)字節(jié)）
uint8_t fifo_data[FIFO_WATERMARK*2][7];
// FIFO中當(dāng)前存儲(chǔ)的數(shù)據(jù)數(shù)量
uint16_t fifo_num = 0;
// FIFO中斷標(biāo)志，用于標(biāo)記是否有新的FIFO數(shù)據(jù)可供讀取
uint8_t fifo_flag=0;

/* USER CODE END 0 */

mian.c中開(kāi)啟中斷。

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中添加回調(diào)函數(shù)引用。

/* 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中添加回調(diào)函數(shù)的定義，檢查中斷是否由 GPIO_PIN_6 引腳觸發(fā)，每次發(fā)生中斷時(shí)從傳感器獲取當(dāng)前的FIFO狀態(tài)，并存儲(chǔ)在 fifo_status 變量中。讀取FIFO數(shù)據(jù)，并將這些數(shù)據(jù)存儲(chǔ)在一個(gè)全局?jǐn)?shù)組 fifo_data 中，以便在主循環(huán)或其他地方進(jìn)行處理。通過(guò)切換 LED 的狀態(tài)，可以直觀地了解中斷的發(fā)生。

/* 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 */

主程序

在主循環(huán)中檢查FIFO中斷標(biāo)志，如果有新的FIFO數(shù)據(jù)，則讀取并處理這些數(shù)據(jù)。處理完成后，調(diào)用MotionFX庫(kù)函數(shù)進(jìn)行數(shù)據(jù)融合計(jì)算，以獲得傳感器的姿態(tài)和位置。

/* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {    
    if(fifo_flag)// 如果 FIFO 中斷標(biāo)志被設(shè)置
    {
        uint8_t acc_flag=0,gyr_flag=0;//加速度角速度標(biāo)志位
        uint8_t deltatime_flag=0;//時(shí)間標(biāo)志位
        for(int i=0;i< fifo_num;i++)// 遍歷 FIFO 數(shù)據(jù)數(shù)組
        {
            // 獲取數(shù)據(jù)指針
            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];
            // 根據(jù)數(shù)據(jù)標(biāo)簽處理不同類型的數(shù)據(jù)
            switch (fifo_data[i][0]) {
        case LSM6DSV16X_XL_NC_TAG:// 加速度數(shù)據(jù)
                    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:// 角速度數(shù)據(jù)
                    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:// 時(shí)間戳數(shù)據(jù)
                    deltatime_flag=1;
                    if(deltatime_first==0)//第一次
                    {
                        deltatime_1=*ts;
                        deltatime_2=deltatime_1;
                        deltatime_first=1;
                    }
                    else
                    {
                        deltatime_2=*ts;
                    }                
          break;
        default:
          break;                
            }
            // 如果加速度、角速度和時(shí)間戳數(shù)據(jù)都已獲取
            if(acc_flag&&gyr_flag&&deltatime_flag)
            {
                lsm6dsv16x_motion_fx_determin();// 調(diào)用 MotionFX 處理函數(shù)
                acc_flag=0;
                gyr_flag=0;
                deltatime_flag=0;
                deltatime_1=deltatime_2;    // 更新時(shí)間戳        
            }            
        }
        // 清除 FIFO 標(biāo)志和數(shù)據(jù)量
        fifo_flag=0;
        fifo_num=0;
        }    
//            HAL_Delay(10);

    /* USER CODE END WHILE */

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

演示

初始位置和數(shù)據(jù)輸出如下所示。

逆時(shí)針旋轉(zhuǎn)90°

逆時(shí)針旋轉(zhuǎn)180°

逆時(shí)針旋轉(zhuǎn)270°

審核編輯 黃宇