概述

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

最近在弄ST和瑞薩RA的課程，需要樣片的可以加群申請：615061293 。

視頻教學(xué)

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

樣品申請

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

源碼下載

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

硬件準(zhǔn)備

首先需要準(zhǔn)備一個(gè)開發(fā)板，這里我準(zhǔn)備的是自己繪制的開發(fā)板，需要的可以進(jìn)行申請。 主控為STM32H503CB，陀螺儀為LSM6DSOW，磁力計(jì)為LIS2MDL。

開啟LED

配置PB14為輸出模式。

開啟INT中斷

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

開啟中斷。

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

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

/// 用于存儲FIFO中讀取的數(shù)據(jù)，每條數(shù)據(jù)包含7個(gè)字節(jié)（1個(gè)標(biāo)簽字節(jié)和6個(gè)數(shù)據(jù)字節(jié)）
uint8_t fifo_data[10*2][7];
stmdev_ctx_t dev_ctx;
uint8_t wmflag = 0;
uint16_t num = 0;
lsm6dso_fifo_tag_t reg_tag;
axis3bit16_t dummy;

// FIFO中當(dāng)前存儲的數(shù)據(jù)數(shù)量
uint16_t fifo_num = 0;
// FIFO中斷標(biāo)志，用于標(biāo)記是否有新的FIFO數(shù)據(jù)可供讀取
uint8_t fifo_flag=0;

static int16_t *datax;
static int16_t *datay;
static int16_t *dataz;


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;
int out_num=0;

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

完整初始化如下所示。

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



  /* Uncomment to configure INT 1 */
  lsm6dso_pin_int1_route_t int1_route;    
  /* 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 */
  lsm6dso_device_id_get(&dev_ctx, &whoamI);
    printf("LSM6DSO_ID=0x%x,whoamI=0x%x",LSM6DSO_ID,whoamI);
  if (whoamI != LSM6DSO_ID)
    while (1);

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

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

  /* Disable I3C interface */
  lsm6dso_i3c_disable_set(&dev_ctx, LSM6DSO_I3C_DISABLE);
  /* Enable Block Data Update */
  lsm6dso_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
  /* Set full scale */
  lsm6dso_xl_full_scale_set(&dev_ctx, LSM6DSO_2g);
  lsm6dso_gy_full_scale_set(&dev_ctx, LSM6DSO_2000dps);
  /* Set FIFO watermark (number of unread sensor data TAG + 6 bytes
   * stored in FIFO) to 10 samples
   */
  lsm6dso_fifo_watermark_set(&dev_ctx, 10);
  /* Set FIFO batch XL/Gyro ODR to 12.5Hz */
  lsm6dso_fifo_xl_batch_set(&dev_ctx, LSM6DSO_XL_BATCHED_AT_417Hz);
  lsm6dso_fifo_gy_batch_set(&dev_ctx, LSM6DSO_GY_BATCHED_AT_417Hz);
  /* Set FIFO mode to Stream mode (aka Continuous Mode) */
  lsm6dso_fifo_mode_set(&dev_ctx, LSM6DSO_STREAM_MODE);
  /* Enable drdy 75 μs pulse: uncomment if interrupt must be pulsed */
  lsm6dso_data_ready_mode_set(&dev_ctx, LSM6DSO_DRDY_PULSED);
  /* Uncomment if interrupt generation on Free Fall INT1 pin */
  lsm6dso_pin_int1_route_get(&dev_ctx, &int1_route);
  int1_route.fifo_th = PROPERTY_ENABLE;
  lsm6dso_pin_int1_route_set(&dev_ctx, int1_route);
  /* Uncomment if interrupt generation on Free Fall INT2 pin */
  //lsm6dso_pin_int2_route_get(&dev_ctx, &int2_route);
  //int2_route.reg.int2_ctrl.int2_fifo_th = PROPERTY_ENABLE;
  //lsm6dso_pin_int2_route_set(&dev_ctx, &int2_route);
  /* Set Output Data Rate */
  lsm6dso_xl_data_rate_set(&dev_ctx, LSM6DSO_XL_ODR_417Hz);
  lsm6dso_gy_data_rate_set(&dev_ctx, LSM6DSO_GY_ODR_417Hz);



    lsm6dso_fifo_timestamp_decimation_set(&dev_ctx, LSM6DSO_DEC_1);
  /* Enable timestamp */
  lsm6dso_timestamp_set(&dev_ctx, PROPERTY_ENABLE);

  /* USER CODE END 2 */

開啟時(shí)間戳

FIFO_CTRL4 (0Ah) 寄存器用于配置LSM6DSOW傳感器的FIFO行為，包括時(shí)間戳批處理減量、溫度數(shù)據(jù)批處理速率和FIFO模式選擇。該寄存器包含多個(gè)字段，每個(gè)字段的功能如下所述。

DEC_TS_BATCH 位于 FIFO_CTRL4 (0Ah) 寄存器中，用于配置時(shí)間戳在FIFO中的批處理減量。它決定了時(shí)間戳數(shù)據(jù)在FIFO中寫入的頻率。

CTRL10_C 寄存器用于啟用和配置LSM6DSOW傳感器的時(shí)間戳計(jì)數(shù)器。啟用時(shí)間戳計(jì)數(shù)器可以在數(shù)據(jù)記錄過程中提供精確的時(shí)間標(biāo)記，以便更好地分析和同步傳感器數(shù)據(jù)。
TIMESTAMP_EN: 用于啟用或禁用時(shí)間戳計(jì)數(shù)器。

時(shí)間戳單位為25us，當(dāng)啟用時(shí)間戳計(jì)數(shù)器時(shí)，計(jì)數(shù)器的值可以從以下寄存器讀取：TIMESTAMPO (40h)，TIMESTAMP1 (41h)，TIMESTAMP2 (42h)，TIMESTAMP3 (43h)

lsm6dso_fifo_timestamp_decimation_set(&dev_ctx, LSM6DSO_DEC_1);
  /* Enable timestamp */
  lsm6dso_timestamp_set(&dev_ctx, PROPERTY_ENABLE);

中斷配置

在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外部中斷線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)，并存儲在 fifo_status 變量中。讀取FIFO數(shù)據(jù)，并將這些數(shù)據(jù)存儲在一個(gè)全局?jǐn)?shù)組 fifo_data 中，以便在主循環(huán)或其他地方進(jìn)行處理。通過切換 LED 的狀態(tài)，可以直觀地了解中斷的發(fā)生。

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin){

    if(GPIO_Pin == GPIO_PIN_0)
    {
        HAL_GPIO_TogglePin(LED1_GPIO_Port, LED1_Pin);
    /* Read watermark flag */
    lsm6dso_fifo_wtm_flag_get(&dev_ctx, &wmflag);

    if (wmflag > 0) {
            fifo_flag=1;
      /* Read number of samples in FIFO */
      lsm6dso_fifo_data_level_get(&dev_ctx, &num);
            fifo_num=num;
//                    printf("num=%dn",num);
      for(int i=0;i< num;i++)  {
        /* Read FIFO tag */
        lsm6dso_fifo_sensor_tag_get(&dev_ctx, ®_tag);
                fifo_data[i][0]=reg_tag;
                memset(data_raw_acceleration.u8bit, 0x00, 3 * sizeof(int16_t));
                lsm6dso_fifo_out_raw_get(&dev_ctx, data_raw_acceleration.u8bit);            
                fifo_data[i][1]=data_raw_acceleration.u8bit[0];
                fifo_data[i][2]=data_raw_acceleration.u8bit[1];
                fifo_data[i][3]=data_raw_acceleration.u8bit[2];
                fifo_data[i][4]=data_raw_acceleration.u8bit[3];
                fifo_data[i][5]=data_raw_acceleration.u8bit[4];
                fifo_data[i][6]=data_raw_acceleration.u8bit[5];        
//    printf("f1=%d f2=%d",fifo_data[i][1],fifo_data[i][2]);            

//        switch (reg_tag) {
//          case LSM6DSO_XL_NC_TAG:
//            memset(data_raw_acceleration.u8bit, 0x00, 3 * sizeof(int16_t));
//            lsm6dso_fifo_out_raw_get(&dev_ctx, data_raw_acceleration.u8bit);
//            acceleration_mg[0] =
//              lsm6dso_from_fs2_to_mg(data_raw_acceleration.i16bit[0]);
//            acceleration_mg[1] =
//              lsm6dso_from_fs2_to_mg(data_raw_acceleration.i16bit[1]);
//            acceleration_mg[2] =
//              lsm6dso_from_fs2_to_mg(data_raw_acceleration.i16bit[2]);
//            printf("Acceleration [mg]:%4.2ft%4.2ft%4.2frn",
//                    acceleration_mg[0], acceleration_mg[1], acceleration_mg[2]);
//            break;

//          case LSM6DSO_GYRO_NC_TAG:
//            memset(data_raw_angular_rate.u8bit, 0x00, 3 * sizeof(int16_t));
//            lsm6dso_fifo_out_raw_get(&dev_ctx, data_raw_angular_rate.u8bit);
//            angular_rate_mdps[0] =
//              lsm6dso_from_fs2000_to_mdps(data_raw_angular_rate.i16bit[0]);
//            angular_rate_mdps[1] =
//              lsm6dso_from_fs2000_to_mdps(data_raw_angular_rate.i16bit[1]);
//            angular_rate_mdps[2] =
//              lsm6dso_from_fs2000_to_mdps(data_raw_angular_rate.i16bit[2]);
//            printf("Angular rate [mdps]:%4.2ft%4.2ft%4.2frn",
//                    angular_rate_mdps[0], angular_rate_mdps[1], angular_rate_mdps[2]);
//            break;

//                    
//          case LSM6DSO_TIMESTAMP_TAG:    
//                        /* Read temperature data */
//                        memset(&data_raw_timestamp, 0x00, sizeof(int16_t));
//            lsm6dso_fifo_out_raw_get(&dev_ctx, data_raw_timestamp.u8bit);
//                        uint32_t timestamp=0;
//                        timestamp+= data_raw_timestamp.u8bit[0];
//                        timestamp+= data_raw_timestamp.u8bit[1]< /                        timestamp+= data_raw_timestamp.u8bit[2]< /                        timestamp+= data_raw_timestamp.u8bit[3]< /                        printf("timestamp=%drn",
//              timestamp);
//            break;                
//                    
//                    
//                    
//                    
//          default:
//            /* Flush unused samples */
//            memset(dummy.u8bit, 0x00, 3 * sizeof(int16_t));
//            lsm6dso_fifo_out_raw_get(&dev_ctx, dummy.u8bit);
//            break;
//        }
      }
    }        

    }
}

主程序

在主循環(huán)中檢查FIFO中斷標(biāo)志，如果有新的FIFO數(shù)據(jù)，則讀取并處理這些數(shù)據(jù)。

/* 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)志位        
        printf("fifo_num=%dn",fifo_num);
        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];

            // 根據(jù)數(shù)據(jù)標(biāo)簽處理不同類型的數(shù)據(jù)
            switch (fifo_data[i][0]) {
        case LSM6DSO_XL_NC_TAG:// 加速度數(shù)據(jù)
                    acc_flag=1;                
                    acc_x=lsm6dso_from_fs2_to_mg(*datax);
                    acc_y=lsm6dso_from_fs2_to_mg(*datay);
                    acc_z=lsm6dso_from_fs2_to_mg(*dataz);        
                    printf("Acceleration [mg]:%4.2ft%4.2ft%4.2frn",
                    acc_x, acc_y, acc_z);                
          break;                
        case LSM6DSO_GYRO_NC_TAG:// 角速度數(shù)據(jù)
                    gyr_flag=1;
                    gyr_x=lsm6dso_from_fs2000_to_mdps(*datax);
                    gyr_y=lsm6dso_from_fs2000_to_mdps(*datay);
                    gyr_z=lsm6dso_from_fs2000_to_mdps(*dataz);    
                    printf("Angular rate [mdps]:%4.2ft%4.2ft%4.2frn",
                                    gyr_x,gyr_y,gyr_z);            
          break;                
        case LSM6DSO_TIMESTAMP_TAG:// 時(shí)間戳數(shù)據(jù)
                    deltatime_flag=1;
             /* 讀取時(shí)間戳數(shù)據(jù) */
                uint32_t timestamp=0;
                        timestamp+= fifo_data[i][1];
                        timestamp+= fifo_data[i][2]< 8;                    
                        timestamp+= fifo_data[i][3]< 16;                        
                        timestamp+= fifo_data[i][4]< 24;    
                    if(deltatime_first==0)//第一次
                    {
                        deltatime_1=timestamp;
                        deltatime_2=deltatime_1;
                        deltatime_first=1;
                    }
                    else
                    {
                        deltatime_2=timestamp;
                    }
                    printf("timestamp=%drn",timestamp);                            
          break;    
        default:
          break;                
                }
            // 如果加速度、角速度和時(shí)間戳數(shù)據(jù)都已獲取
            if(acc_flag&&gyr_flag&&deltatime_flag)
            {
//                lsm6dso_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;

        }


    /* USER CODE END WHILE */

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

演示

由于分別開啟了加速度，角速度，時(shí)間戳到FIFO，F(xiàn)IFO溢出閾值設(shè)置為10，那么到12的時(shí)候數(shù)據(jù)溢出進(jìn)行打印。 時(shí)間戳數(shù)據(jù)16697842-16697746=96*25us=2400us。 速率為416HZ，即2.4ms

審核編輯 黃宇