概述

本文將探討如何使用中斷機(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/BV1pm421G7XE/]

樣品申請(qǐng)

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

源碼下載

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

開(kāi)啟LED

配置PB14為輸出模式。

開(kāi)啟INT中斷

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

開(kāi)啟中斷。

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

[https://github.com/STMicroelectronics/lsm6ds3tr-c-pid]

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

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

float acc_x,acc_y,acc_z;
float gyr_x,gyr_y,gyr_z;
uint32_t deltatime_1,deltatime_2;
uint8_t deltatime_first=0;




  stmdev_ctx_t dev_ctx;
    uint8_t waterm = 0;
/// 用于存儲(chǔ)FIFO中讀取的數(shù)據(jù)，每條數(shù)據(jù)包含24個(gè)字節(jié),*2保證數(shù)據(jù)不溢出
uint8_t fifo_data[20*3*2][6];
// 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;

uint8_t acc_fifo[20*2][6];
uint8_t gyr_fifo[20*2][6];
uint8_t timestamp_fifo[20*2][6];

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

lsm6ds3tr_c_int1_route_t pin_int;
    lsm6ds3tr_c_pin_int1_route_get(&dev_ctx, &pin_int);
  pin_int.int1_fifo_ovr = PROPERTY_ENABLE;
  lsm6ds3tr_c_pin_int1_route_set(&dev_ctx, pin_int);

需要注意優(yōu)化等級(jí)。

完整初始化如下所示。

/* USER CODE BEGIN 2 */
    printf("HELLO!n");
  HAL_GPIO_WritePin(CS1_GPIO_Port, CS1_Pin, GPIO_PIN_SET);
  HAL_GPIO_WritePin(SA0_GPIO_Port, SA0_Pin, GPIO_PIN_RESET);
  HAL_GPIO_WritePin(CS2_GPIO_Port, CS2_Pin, GPIO_PIN_SET);
    HAL_Delay(100);


  /* Initialize mems driver interface */

  dev_ctx.write_reg = platform_write;
  dev_ctx.read_reg = platform_read;
  dev_ctx.mdelay = platform_delay;
  dev_ctx.handle = &SENSOR_BUS;
  /* Init test platform */
//  platform_init();
  /* Wait sensor boot time */
  platform_delay(BOOT_TIME);
  /* Check device ID */
  whoamI = 0;
  lsm6ds3tr_c_device_id_get(&dev_ctx, &whoamI);
    printf("LSM6DS3TR-C_ID=0x%x,whoamI=0x%x",LSM6DS3TR_C_ID,whoamI);
  if ( whoamI != LSM6DS3TR_C_ID )
    while (1); /*manage here device not found */

  /* Restore default configuration */
  lsm6ds3tr_c_reset_set(&dev_ctx, PROPERTY_ENABLE);

  do {
    lsm6ds3tr_c_reset_get(&dev_ctx, &rst);
  } while (rst);



  /* 設(shè)置加速度計(jì)和陀螺儀的滿(mǎn)量程范圍 */
  lsm6ds3tr_c_xl_full_scale_set(&dev_ctx, LSM6DS3TR_C_4g);
  lsm6ds3tr_c_gy_full_scale_set(&dev_ctx, LSM6DS3TR_C_2000dps);
  /* 啟用塊數(shù)據(jù)更新（BDU），當(dāng)FIFO支持時(shí) */
  lsm6ds3tr_c_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);

    lsm6ds3tr_c_xl_power_mode_set(&dev_ctx, LSM6DS3TR_C_XL_HIGH_PERFORMANCE);    
  /* 設(shè)置加速度計(jì)和陀螺儀的輸出數(shù)據(jù)速率：
   * 在本例中，我們將加速度計(jì)和陀螺儀的速率設(shè)置為26 Hz
   */
  lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, LSM6DS3TR_C_XL_ODR_416Hz);
  lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, LSM6DS3TR_C_GY_ODR_416Hz);


  lsm6ds3tr_c_fifo_mode_set(&dev_ctx, LSM6DS3TR_C_BYPASS_MODE);    
    HAL_Delay(10);
  /* 設(shè)置FIFO水印為模式的倍數(shù)
   * 在本例中，我們將水印設(shè)置為10個(gè)模式
   * 這意味著10個(gè)序列：
   * (陀螺儀 + 加速度計(jì)) = 12字節(jié)
   * (外部傳感器 + 時(shí)間戳) = 12字節(jié)
   */
    lsm6ds3tr_c_int1_route_t int_1_reg;
  uint16_t pattern_len = 24;  // 每個(gè)數(shù)據(jù)集由6個(gè)字節(jié)組成,4*6=24
  lsm6ds3tr_c_fifo_watermark_set(&dev_ctx, 10 * pattern_len);


  /* 將FIFO模式設(shè)置為流模式 */
    //FIFO_CTRL5(0x0A)- >STREAM_MODE
  lsm6ds3tr_c_fifo_mode_set(&dev_ctx, LSM6DS3TR_C_STREAM_MODE);

  /* 啟用時(shí)間戳并將其添加到FIFO */
    //CTRL10_C (19h)- >TIMER_EN
  lsm6ds3tr_c_timestamp_set(&dev_ctx, PROPERTY_ENABLE);
    //CTRL10_C (19h)- >PEDO_EN    
    lsm6ds3tr_c_pedo_sens_set(&dev_ctx, PROPERTY_ENABLE); // 根據(jù)需求配置步數(shù)計(jì)數(shù)


  /* 將時(shí)間戳分辨率設(shè)置為25 μs (WAKE_UP_DUR寄存器中的TIMER_HR位) */
    //WAKE_UP_DUR (5Ch)- >TIMER_HR
    lsm6ds3tr_c_timestamp_res_set(&dev_ctx, LSM6DS3TR_C_LSB_25us);

    //設(shè)置第3數(shù)據(jù)集（Dataset 3）的降采樣因子
        lsm6ds3tr_c_fifo_dataset_3_batch_set(&dev_ctx, LSM6DS3TR_C_FIFO_DS3_NO_DEC);    
    //設(shè)置第4數(shù)據(jù)集（Dataset 4）的降采樣因子
    //FIFO_CTRL4 (09h)- >DEC_DS4_FIFO[2:0]
        lsm6ds3tr_c_fifo_dataset_4_batch_set(&dev_ctx, LSM6DS3TR_C_FIFO_DS4_NO_DEC);


    // 啟用時(shí)間戳寫(xiě)入FIFO第四數(shù)據(jù)集
    //FIFO_CTRL2 (07h)- >TIMER_PEDO_FIFO_EN
  lsm6ds3tr_c_fifo_pedo_and_timestamp_batch_set(&dev_ctx, PROPERTY_ENABLE);            


  /* 設(shè)置FIFO傳感器的降采樣因子 */
  lsm6ds3tr_c_fifo_xl_batch_set(&dev_ctx, LSM6DS3TR_C_FIFO_XL_NO_DEC);
  lsm6ds3tr_c_fifo_gy_batch_set(&dev_ctx, LSM6DS3TR_C_FIFO_GY_NO_DEC);

  /* 設(shè)置FIFO的輸出數(shù)據(jù)速率 */
    //FIFO_CTRL5 (0Ah)
  lsm6ds3tr_c_fifo_data_rate_set(&dev_ctx, LSM6DS3TR_C_FIFO_416Hz);


  lsm6ds3tr_c_int1_route_t pin_int;
    lsm6ds3tr_c_pin_int1_route_get(&dev_ctx, &pin_int);
  pin_int.int1_fth = PROPERTY_ENABLE;
  lsm6ds3tr_c_pin_int1_route_set(&dev_ctx, pin_int);    


    lsm6ds3tr_c_init();
  /* USER CODE END 2 */

在stm32h5xx_it.c中添加回調(diào)函數(shù)引用。

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


/* USER CODE END 0 */

處理PB0外部中斷線(xiàn)0（EXTI Line0）的中斷。

/**
  * @brief This function handles EXTI Line0 interrupt.
  */
void EXTI0_IRQHandler(void)
{
  /* USER CODE BEGIN EXTI0_IRQn 0 */
    HAL_GPIO_EXTI_Callback(GPIO_PIN_0);
  /* 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中添加回調(diào)函數(shù)的定義，檢查中斷是否由 GPIO_PIN_0 引腳觸發(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_0)
    {
        HAL_GPIO_TogglePin(LED1_GPIO_Port, LED1_Pin);
    /* 讀取LSM6DS3TR-C的水印標(biāo)志 */
    lsm6ds3tr_c_fifo_wtm_flag_get(&dev_ctx, &waterm);        
    uint16_t num = 0,num1=0;
    uint16_t num_pattern = 0;

        if (waterm) {
            fifo_flag=1;
      /* 讀取FIFO中的字?jǐn)?shù) */
      lsm6ds3tr_c_fifo_data_level_get(&dev_ctx, &num);        
            num_pattern = num / 24*2;        
//            printf( "-- FIFO num %d num_pattern=%drn", num,num_pattern);
            fifo_num=num_pattern;
            for(int i=0;i< num_pattern;i++)  {



        /* 根據(jù)傳感器的ODR配置，F(xiàn)IFO模式由以下樣本序列組成：GYRO, XL 外部傳感器 時(shí)間戳*/
        lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx,
                                  &gyr_fifo[i][0],
                                   3 * sizeof(int16_t));                
        lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx,
                                  &acc_fifo[i][0],
                                  6);                        

                //外部傳感器數(shù)據(jù)                
        lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx,
                                  data_raw_none.u8bit,
                                   3 * sizeof(int16_t));                        
        lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx,
                                      ×tamp_fifo[i][0],
                                       3 * sizeof(int16_t));            

            }    
        }
    }
}
/* 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)
    {
        for(int i=0;i< fifo_num;i++)// 遍歷 FIFO 數(shù)據(jù)數(shù)組
        {    

            int16_t gyr;                
            gyr=(gyr_fifo[i][1]< 8) + gyr_fifo[i][0];
            gyr_x =lsm6ds3tr_c_from_fs2000dps_to_mdps(gyr);
            gyr=(gyr_fifo[i][3]< 8) + gyr_fifo[i][2];                
            gyr_y =lsm6ds3tr_c_from_fs2000dps_to_mdps(gyr);
            gyr=(gyr_fifo[i][5]< 8) + gyr_fifo[i][4];                
            gyr_z =lsm6ds3tr_c_from_fs2000dps_to_mdps(gyr);
//            printf(
//                            "gyr_x:%4.2ft%4.2ft%4.2frn",
//                            gyr_x, gyr_y, gyr_z);            
            int16_t acc;
            acc=(acc_fifo[i][1]< 8) + acc_fifo[i][0];
            acc_x =lsm6ds3tr_c_from_fs4g_to_mg(acc);
            acc=(acc_fifo[i][3]< 8) + acc_fifo[i][2];                
            acc_y =lsm6ds3tr_c_from_fs4g_to_mg(acc);
            acc=(acc_fifo[i][5]< 8) + acc_fifo[i][4];                
            acc_z =lsm6ds3tr_c_from_fs4g_to_mg(acc);

//            printf(
//                            "acc_x:%4.2ft%4.2ft%4.2frn",
//                            acc_x, acc_y, acc_z);    

         /* 讀取時(shí)間戳數(shù)據(jù) */
            uint32_t timestamp=0;
            timestamp=(timestamp_fifo[i][1]< 16)|(timestamp_fifo[i][0]< 8)
                |(timestamp_fifo[i][3]);
//        printf("Timestamp: %urn", timestamp);        

            if(deltatime_first==0)//第一次
                {
                    deltatime_1=timestamp;
                    deltatime_2=deltatime_1;
                    deltatime_first=1;
                }
                else
                {
                    deltatime_2=timestamp;
                }
                    lsm6ds3tr_c_motion_fx_determin();    
                deltatime_1=deltatime_2;

            }
            fifo_flag=0;                
        }

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

審核編輯 黃宇