概述

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

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

視頻教學(xué)

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

樣品申請(qǐng)

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

源碼下載

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

硬件準(zhǔn)備

首先需要準(zhǔn)備一個(gè)開發(fā)板，這里我準(zhǔn)備的是自己繪制的開發(fā)板，需要的可以進(jìn)行申請(qǐng)。

主控為STM32H503CB，陀螺儀為L(zhǎng)SM6DSOW，磁力計(jì)為L(zhǎng)IS2MDL。

開啟CRC

串口設(shè)置

設(shè)置串口速率為2000000。

開啟X-CUBE-MEMS1

速率選擇

加速度和角速度的速率盡量大于100Hz。

XL_HM_MODE 和 G_HM_MODE 是LSM6DSOW傳感器中的配置位，用于設(shè)置加速度計(jì)和陀螺儀的高性能模式或低功耗模式。這些模式可以根據(jù)應(yīng)用需求選擇，以優(yōu)化功耗或提高性能。

XL_HM_MODE 位于 CTRL6_C (15h) 寄存器中，用于控制加速度計(jì)的高性能模式。在高性能模式下，加速度計(jì)提供更高的采樣率和精度，但功耗也會(huì)增加。
XL_HM_MODE 位描述
● 位位置: 4
● 默認(rèn)值: 0（高性能模式啟用）
● 描述: 控制加速度計(jì)的高性能模式。
○ 0: 啟用高性能操作模式（默認(rèn)）。
○ 1: 禁用高性能操作模式，啟用低功耗模式。

G_HM_MODE 位于 CTRL7_G (16h) 寄存器中，用于控制陀螺儀的高性能模式。在高性能模式下，陀螺儀提供更高的采樣率和精度，但功耗也會(huì)增加。
G_HM_MODE 位描述
● 位位置: 7
● 默認(rèn)值: 0（高性能模式啟用）
● 描述: 控制陀螺儀的高性能模式。
○ 0: 啟用高性能操作模式（默認(rèn)）。
○ 1: 禁用高性能操作模式，啟用低功耗模式。

添加到初始化位置。

lsm6dso_xl_power_mode_set(&dev_ctx, LSM6DSO_HIGH_PERFORMANCE_MD);
    lsm6dso_gy_power_mode_set(&dev_ctx, LSM6DSO_GY_HIGH_PERFORMANCE);

設(shè)置FIFO觸發(fā)閾值為30.

lsm6dso_fifo_watermark_set(&dev_ctx, 30);

修改定義FIFO存儲(chǔ)數(shù)組的長(zhǎng)度為閾值的2倍。

uint8_t fifo_data[30*2][7];

初始化定義

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

    lsm6dso_xl_power_mode_set(&dev_ctx, LSM6DSO_HIGH_PERFORMANCE_MD);
    lsm6dso_gy_power_mode_set(&dev_ctx, LSM6DSO_GY_HIGH_PERFORMANCE);


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

MotionFX文件

主要包含lsm6dso_app.c和lsm6dso_app.h，這兩個(gè)文件主要負(fù)責(zé)初始化和管理LSM6DSOW傳感器的交互。它們提供了配置傳感器、初始化通信接口以及讀取傳感器數(shù)據(jù)的功能。
該文件包含與LSM6DSOW傳感器交互所需函數(shù)的實(shí)現(xiàn)。它提供了配置傳感器、初始化通信接口以及讀取傳感器數(shù)據(jù)的功能。
lsm6dso_init(): 初始化MotionFX算法。
lsm6dso_motion_fx_determin(): 該函數(shù)主要用于讀取傳感器數(shù)據(jù)并使用MotionFX庫(kù)進(jìn)行數(shù)據(jù)融合處理。

卡爾曼濾波算法

運(yùn)行卡爾曼濾波傳播算法MotionFX_propagate。

根據(jù)需要更新卡爾曼濾波器MotionFX_update。

需要注意的是這2各算法非常吃資源，需要注意MCU算力分配。

函數(shù)結(jié)構(gòu)如下所示。

對(duì)應(yīng)的demo在2.2.9有提供。

主程序執(zhí)行流程

1. 讀取FIFO水印標(biāo)志：
   ○ 使用 中斷以及l(fā)sm6dso_fifo_wtm_flag_get() 函數(shù)讀取FIFO水印標(biāo)志，判斷FIFO中的數(shù)據(jù)是否達(dá)到設(shè)定的閾值。
2. 處理FIFO數(shù)據(jù)：
   ○ 如果FIFO水印標(biāo)志被設(shè)置，讀取FIFO中的數(shù)據(jù)數(shù)量。
   ○ 使用 lsm6dso_fifo_out_raw_get() 函數(shù)逐項(xiàng)讀取FIFO中的傳感器數(shù)據(jù)。
3. 調(diào)用姿態(tài)估計(jì)算法：
   ○ 當(dāng)加速度計(jì)、陀螺儀和時(shí)間戳數(shù)據(jù)都已讀取時(shí)，調(diào)用 lsm6dso_motion_fx_determin() 函數(shù)進(jìn)行姿態(tài)估計(jì)。
   ○ 重置標(biāo)志位并更新時(shí)間戳。

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

歐拉角簡(jiǎn)介

歐拉角（Euler Angles）是一種表示三維旋轉(zhuǎn)的方式，通過(guò)三個(gè)角度來(lái)描述物體在三維空間中的姿態(tài)。這三個(gè)角度通常稱為滾轉(zhuǎn)角（Roll）、俯仰角（Pitch）和偏航角（Yaw），它們分別表示繞物體的自身坐標(biāo)系的三個(gè)軸的旋轉(zhuǎn)。

橫滾roll，俯仰pitch，偏航y(tǒng)aw的實(shí)際含義如下圖：

● 優(yōu)點(diǎn)
表示簡(jiǎn)單直觀，易于理解。
適用于描述固定順序的旋轉(zhuǎn)操作。
● 缺點(diǎn)
存在萬(wàn)向節(jié)死鎖問(wèn)題（Gimbal Lock），即當(dāng)俯仰角接近±90度時(shí)，會(huì)失去一個(gè)自由度，導(dǎo)致系統(tǒng)無(wú)法確定物體的姿態(tài)。
旋轉(zhuǎn)順序不同會(huì)導(dǎo)致不同的最終姿態(tài)，需要特別注意旋轉(zhuǎn)順序。

演示

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

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

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

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

審核編輯 黃宇