概述

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

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

視頻教學(xué)

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

樣品申請

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

源碼下載

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

開啟CRC

串口設(shè)置

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

開啟X-CUBE-MEMS1

設(shè)置加速度和角速度量程

這里設(shè)置加速度量程為4g和角速度為4000dps。

/* 設(shè)置加速度計(jì)和陀螺儀的滿量程范圍 */
  lsm6ds3tr_c_xl_full_scale_set(&dev_ctx, LSM6DS3TR_C_4g);
  lsm6ds3tr_c_gy_full_scale_set(&dev_ctx, LSM6DS3TR_C_2000dps);

速率選擇

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

XL_HM_MODE 位定義了加速度計(jì)的電源模式，可以在高性能模式和低功耗模式之間切換。這有助于在不同應(yīng)用場景下優(yōu)化傳感器的功耗和性能。 對于高速率，需要配置為高性能模式。

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_208Hz);
  lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, LSM6DS3TR_C_GY_ODR_208Hz);

設(shè)置FIFO速率

LSM6DS3TR-C傳感器的FIFO控制寄存器3（FIFO_CTRL3）的內(nèi)容，該寄存器用于選擇陀螺儀和加速度計(jì)數(shù)據(jù)寫入FIFO的批處理數(shù)據(jù)速率（BDR，Batch Data Rate）。以下是詳細(xì)描述：
FIFO_CTRL3寄存器（地址09h）,該寄存器包含兩個主要字段：
● DEC_FIFO_GYRO [2:0]：選擇陀螺儀數(shù)據(jù)的批處理速率。
● DEC_FIFO_XL [2:0]：選擇加速度計(jì)數(shù)據(jù)的批處理速率。

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

初始化定義

/* 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 */
  stmdev_ctx_t dev_ctx;
  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ì)和陀螺儀的滿量程范圍 */
  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支持時 */
  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_208Hz);
  lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, LSM6DS3TR_C_GY_ODR_208Hz);


  /* 設(shè)置FIFO水印為模式的倍數(shù)
   * 在本例中，我們將水印設(shè)置為10個模式
   * 這意味著10個序列：
   * (陀螺儀 + 加速度計(jì)) = 12字節(jié)
   * (外部傳感器 + 時間戳) = 12字節(jié)
   */
    lsm6ds3tr_c_int1_route_t int_1_reg;
  uint16_t pattern_len = 24;  // 每個數(shù)據(jù)集由6個字節(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);

  /* 啟用時間戳并將其添加到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è)置為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);


    // 啟用時間戳寫入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_208Hz);

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

MotionFX文件

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

卡爾曼濾波算法

運(yùn)行卡爾曼濾波傳播算法MotionFX_propagate。
根據(jù)需要更新卡爾曼濾波器MotionFX_update。
需要注意的是這2各算法非常吃資源，需要注意MCU算力分配。

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

主程序執(zhí)行流程

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

/* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    uint16_t num = 0,num1=0;
    uint16_t num_pattern = 0;
    uint8_t waterm = 0;
    /* 讀取LSM6DS3TR-C的水印標(biāo)志 */
    lsm6ds3tr_c_fifo_wtm_flag_get(&dev_ctx, &waterm);        
    if (waterm) {
      /* 讀取FIFO中的字?jǐn)?shù) */
      lsm6ds3tr_c_fifo_data_level_get(&dev_ctx, &num);
      num_pattern = num / 24*2;
            printf("num=%dn",num);
      while (num_pattern-- > 0) {
                printf ("num1=%dn",num1);
                num1++;
        /* 根據(jù)傳感器的ODR配置，F(xiàn)IFO模式由以下樣本序列組成：GYRO, XL 外部傳感器 時間戳*/
        lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx,
                                  data_raw_angular_rate.u8bit,
                                  3 * sizeof(int16_t));
        gyr_x =
          lsm6ds3tr_c_from_fs2000dps_to_mdps(data_raw_angular_rate.i16bit[0]);
        gyr_y =
          lsm6ds3tr_c_from_fs2000dps_to_mdps(data_raw_angular_rate.i16bit[1]);
        gyr_z=
          lsm6ds3tr_c_from_fs2000dps_to_mdps(data_raw_angular_rate.i16bit[2]);
//        printf(
//                "Angular rate [mdps]:%4.2ft%4.2ft%4.2frn",
//                gyr_x, gyr_y, gyr_z);
        lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx,
                                  data_raw_acceleration.u8bit,
                                  3 * sizeof(int16_t));
        acc_x =
          lsm6ds3tr_c_from_fs4g_to_mg(data_raw_acceleration.i16bit[0]);
        acc_y =
          lsm6ds3tr_c_from_fs4g_to_mg(data_raw_acceleration.i16bit[1]);
        acc_z =
          lsm6ds3tr_c_from_fs4g_to_mg(data_raw_acceleration.i16bit[2]);
//        printf("Acc [mg]:%4.2ft%4.2ft%4.2frn",
//                acc_x, acc_y, acc_z);
                //外部傳感器數(shù)據(jù)                
        lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx,
                                  data_raw_none.u8bit,
                                  3 * sizeof(int16_t));        
        // 打印外部傳感器數(shù)據(jù)
//        printf("External sensor data: %02x %02x %02x %02x %02x %02xrn",
//               data_raw_none.u8bit[0], data_raw_none.u8bit[1], data_raw_none.u8bit[2],
//               data_raw_none.u8bit[3], data_raw_none.u8bit[4], data_raw_none.u8bit[5]);


        /* 讀取時間戳數(shù)據(jù) */
        uint32_t timestamp=0;
        lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx,
                                      data_raw_Timestamp.u8bit,
                                      3*sizeof(int16_t));//
                    timestamp=(data_raw_Timestamp.u8bit[1]< 16)|(data_raw_Timestamp.u8bit[0]< 8)
                        |(data_raw_Timestamp.u8bit[3]);
//        printf("Timestamp: %urn", timestamp);        
                if(deltatime_first==0)//第一次
                    {
                        deltatime_1=timestamp;
                        deltatime_2=timestamp;
                        deltatime_first=1;
                    }
                    else
                    {
                        deltatime_2=timestamp;
                    }

                    lsm6ds3tr_c_motion_fx_determin();
//                    acc_flag=0;
//                    gyr_flag=0;
//                    deltatime_flag=0;
                    deltatime_1=deltatime_2;    


      }
    }        


    /* USER CODE END WHILE */

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

lsm6ds3tr_c_motion_fx_determin

● 外部變量聲明：
○ acc_x, acc_y, acc_z: 加速度計(jì)數(shù)據(jù)。
○ gyr_x, gyr_y, gyr_z: 陀螺儀數(shù)據(jù)。
○ deltatime_1, deltatime_2: 時間戳數(shù)據(jù)。
○ out_num: 輸出計(jì)數(shù)器。
● 讀取并存儲傳感器數(shù)據(jù)：
○ 將全局變量中的加速度計(jì)和陀螺儀數(shù)據(jù)存儲到 sensor_hub_data 結(jié)構(gòu)體中。
● 準(zhǔn)備 MotionFX 輸入數(shù)據(jù)：
○ 將讀取到的加速度計(jì)和陀螺儀數(shù)據(jù)轉(zhuǎn)換為 MotionFX 庫所需的單位（g 和 dps），并存儲在 mfx_data_in 結(jié)構(gòu)體中。
○ 初始化磁力計(jì)數(shù)據(jù)為 0。
● 計(jì)算時間差：
○ 計(jì)算兩個時間戳之間的差值（單位：秒），并存儲在 delta_time 數(shù)組中。
● 卡爾曼濾波算法：
○ 使用 MotionFX_propagate 函數(shù)運(yùn)行卡爾曼濾波傳播算法。
○ 使用 MotionFX_update 函數(shù)更新卡爾曼濾波器。

extern    float acc_x,acc_y,acc_z;
extern    float gyr_x,gyr_y,gyr_z;
extern uint32_t deltatime_1,deltatime_2;



void lsm6ds3tr_c_motion_fx_determin(void){



    sensor_hub_data.acceleration[0]=acc_x;
    sensor_hub_data.acceleration[1]=acc_y;
    sensor_hub_data.acceleration[2]=acc_z;

    sensor_hub_data.angular_rate[0]=gyr_x;
    sensor_hub_data.angular_rate[1]=gyr_y;
    sensor_hub_data.angular_rate[2]=gyr_z;    

    /*----------------------------------------------------------------------------------

                                                      fx motion 移動算法（卡爾曼濾波）

        ----------------------------------------------------------------------------------*/
    MFX_input_t mfx_data_in;

    /* MotionFX 算法庫，計(jì)算四元數(shù)，參考自 AlgoBuilded 生成代碼 */
    mfx_data_in.acc[0] = sensor_hub_data.acceleration[0] * FROM_MG_TO_G;
    mfx_data_in.acc[1] = sensor_hub_data.acceleration[1] * FROM_MG_TO_G;
    mfx_data_in.acc[2] = sensor_hub_data.acceleration[2] * FROM_MG_TO_G;

    mfx_data_in.gyro[0] = sensor_hub_data.angular_rate[0] * FROM_MDPS_TO_DPS;
    mfx_data_in.gyro[1] = sensor_hub_data.angular_rate[1] * FROM_MDPS_TO_DPS;
    mfx_data_in.gyro[2] = sensor_hub_data.angular_rate[2] * FROM_MDPS_TO_DPS;

    mfx_data_in.mag[0] = 0;
    mfx_data_in.mag[1] = 0;
    mfx_data_in.mag[2] = 0; 

//    printf("Acceleration [mg]:t%4.2f t%4.2f t%4.2frn",mfx_data_in.acc[0], 
//                                                                        mfx_data_in.acc[1], mfx_data_in.acc[2]);



    /* 跟傳感器輸出速率ODR相關(guān)，△time 要比采樣周期小 */
//        float delta_time = DELATE_TIME;
    float delta_time[1];
    if(deltatime_2 >deltatime_1)
    {
        delta_time[0]=(float)((double)(deltatime_2-deltatime_1)*25.0f/1000000);
//        printf("d=%fn",delta_time[0]);
        /* 運(yùn)行卡爾曼濾波傳播算法 */
        MotionFX_propagate(mfxstate_6x, &sensor_hub_data.mfx_6x, &mfx_data_in, delta_time);
        /* 更新卡爾曼濾波器 */
        MotionFX_update(mfxstate_6x, &sensor_hub_data.mfx_6x, &mfx_data_in, delta_time, NULL);
    }
    else if(deltatime_1 >deltatime_2)
    {
        delta_time[0]=(float)((double)(0xffffff-deltatime_2+deltatime_1)*25.0f/1000000);    
        /* 運(yùn)行卡爾曼濾波傳播算法 */
        MotionFX_propagate(mfxstate_6x, &sensor_hub_data.mfx_6x, &mfx_data_in, delta_time);
        /* 更新卡爾曼濾波器 */
        MotionFX_update(mfxstate_6x, &sensor_hub_data.mfx_6x, &mfx_data_in, delta_time, NULL);        
    }
    else if(deltatime_1==deltatime_2)
    {
        delta_time[0]=0.0f;    
    }
//    /* 運(yùn)行卡爾曼濾波傳播算法 */
//    MotionFX_propagate(mfxstate_6x, &sensor_hub_data.mfx_6x, &mfx_data_in, &delta_time);
//    /* 更新卡爾曼濾波器 */
//    MotionFX_update(mfxstate_6x, &sensor_hub_data.mfx_6x, &mfx_data_in, &delta_time, NULL);

    /* 將四元數(shù)存儲到數(shù)組，方便后續(xù)操作 */
//    Quaternions_data[0] = sensor_hub_data.mfx_6x.quaternion[0];
//    Quaternions_data[1] = sensor_hub_data.mfx_6x.quaternion[1];
//    Quaternions_data[2] = sensor_hub_data.mfx_6x.quaternion[2];
//    Quaternions_data[3] = sensor_hub_data.mfx_6x.quaternion[3];

    /* 按照 VOFA+ 的 FireWater 數(shù)據(jù)協(xié)議格式，輸出四元數(shù)數(shù)據(jù) */
    /* 斜視圖 右前上視角：scalar | x | y | z */
//    printf("%f, %f, %f, %f n",Quaternions_data[3],
                        Quaternions_data[1],Quaternions_data[2],Quaternions_data[0]);

    printf("%f, %f, %fn",sensor_hub_data.mfx_6x.rotation[0],
                    sensor_hub_data.mfx_6x.rotation[1],sensor_hub_data.mfx_6x.rotation[2]);
}

歐拉角簡介

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

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

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

演示

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

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

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

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

審核編輯 黃宇