概述

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

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

視頻教學

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

樣品申請

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

源碼下載

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

開啟CRC

串口設置

設置串口速率為2000000。

開啟X-CUBE-MEMS1

設置加速度和角速度量程

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

/* Set full scale */
  lsm6dsv16x_xl_full_scale_set(&dev_ctx, LSM6DSV16X_4g);
  lsm6dsv16x_gy_full_scale_set(&dev_ctx, LSM6DSV16X_4000dps);

速率選擇

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

設置FIFO速率

LSM6DSV16X傳感器的FIFO控制寄存器3（FIFO_CTRL3）的內容，該寄存器用于選擇陀螺儀和加速度計數據寫入FIFO的批處理數據速率（BDR，Batch Data Rate）。以下是詳細描述：
FIFO_CTRL3寄存器（地址09h）,該寄存器包含兩個主要字段：
● BDR_GY_[3:0]：選擇陀螺儀數據的批處理速率。
● BDR_XL_[3:0]：選擇加速度計數據的批處理速率。

將加速度計的數據速率（Output Data Rate, ODR）設置為60Hz。這意味著加速度計的數據將以每秒60次的頻率批量寫入FIFO。
將陀螺儀的數據速率設置為15Hz。這意味著陀螺儀的數據將以每秒15次的頻率批量寫入FIFO。

/* Set FIFO batch XL/Gyro ODR to 12.5Hz */
  lsm6dsv16x_fifo_xl_batch_set(&dev_ctx, LSM6DSV16X_XL_BATCHED_AT_480Hz);
  lsm6dsv16x_fifo_gy_batch_set(&dev_ctx, LSM6DSV16X_GY_BATCHED_AT_480Hz);

設置FIFO時間戳批處理速率

LSM6DSV16X傳感器的時間戳批處理速率、溫度數據批處理速率、增強的EIS陀螺儀輸出批處理，以及FIFO的工作模式。這些配置確保傳感器數據能夠以適當的速率和模式進行批處理和存儲，以滿足不同的應用需求。

/* Set Output Data Rate */
  lsm6dsv16x_xl_data_rate_set(&dev_ctx, LSM6DSV16X_ODR_AT_480Hz);
  lsm6dsv16x_gy_data_rate_set(&dev_ctx, LSM6DSV16X_ODR_AT_480Hz);
  lsm6dsv16x_fifo_timestamp_batch_set(&dev_ctx, LSM6DSV16X_TMSTMP_DEC_1);

配置過濾鏈

lsm6dsv16x_filt_gy_lp1_set(&dev_ctx, PROPERTY_ENABLE);
  lsm6dsv16x_filt_gy_lp1_bandwidth_set(&dev_ctx, LSM6DSV16X_GY_ULTRA_LIGHT);
  lsm6dsv16x_filt_xl_lp2_set(&dev_ctx, PROPERTY_ENABLE);
  lsm6dsv16x_filt_xl_lp2_bandwidth_set(&dev_ctx, LSM6DSV16X_XL_STRONG);

初始化定義

/* USER CODE BEGIN 2 */
    printf("HELLO!n");
  HAL_GPIO_WritePin(CS_GPIO_Port, CS_Pin, GPIO_PIN_SET);
  HAL_GPIO_WritePin(SA0_GPIO_Port, SA0_Pin, GPIO_PIN_RESET);
    HAL_Delay(100);
//lsm6dsdtr_init();

  lsm6dsv16x_fifo_status_t fifo_status;
  stmdev_ctx_t dev_ctx;
  lsm6dsv16x_reset_t rst;

  /* 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(dev_ctx.handle);
  /* Wait sensor boot time */
  platform_delay(BOOT_TIME);

  /* Check device ID */
  lsm6dsv16x_device_id_get(&dev_ctx, &whoamI);
    printf("LSM6DSV16X_ID=0x%x,whoamI=0x%x",LSM6DSV16X_ID,whoamI);
  if (whoamI != LSM6DSV16X_ID)
    while (1);

  /* Restore default configuration */
  lsm6dsv16x_reset_set(&dev_ctx, LSM6DSV16X_RESTORE_CTRL_REGS);
  do {
    lsm6dsv16x_reset_get(&dev_ctx, &rst);
  } while (rst != LSM6DSV16X_READY);

  /* Enable Block Data Update */
  lsm6dsv16x_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);

  /* Set full scale */
  lsm6dsv16x_xl_full_scale_set(&dev_ctx, LSM6DSV16X_4g);
  lsm6dsv16x_gy_full_scale_set(&dev_ctx, LSM6DSV16X_4000dps);

  /*
   * Set FIFO watermark (number of unread sensor data TAG + 6 bytes
   * stored in FIFO) to FIFO_WATERMARK samples
   */
  lsm6dsv16x_fifo_watermark_set(&dev_ctx, FIFO_WATERMARK);
  /* Set FIFO batch XL/Gyro ODR to 12.5Hz */
  lsm6dsv16x_fifo_xl_batch_set(&dev_ctx, LSM6DSV16X_XL_BATCHED_AT_480Hz);
  lsm6dsv16x_fifo_gy_batch_set(&dev_ctx, LSM6DSV16X_GY_BATCHED_AT_480Hz);

  /* Set FIFO mode to Stream mode (aka Continuous Mode) */
  lsm6dsv16x_fifo_mode_set(&dev_ctx, LSM6DSV16X_STREAM_MODE);

  /* Set Output Data Rate */
  lsm6dsv16x_xl_data_rate_set(&dev_ctx, LSM6DSV16X_ODR_AT_480Hz);
  lsm6dsv16x_gy_data_rate_set(&dev_ctx, LSM6DSV16X_ODR_AT_480Hz);
  lsm6dsv16x_fifo_timestamp_batch_set(&dev_ctx, LSM6DSV16X_TMSTMP_DEC_1);
  lsm6dsv16x_timestamp_set(&dev_ctx, PROPERTY_ENABLE);


  lsm6dsv16x_filt_gy_lp1_set(&dev_ctx, PROPERTY_ENABLE);
  lsm6dsv16x_filt_gy_lp1_bandwidth_set(&dev_ctx, LSM6DSV16X_GY_ULTRA_LIGHT);
  lsm6dsv16x_filt_xl_lp2_set(&dev_ctx, PROPERTY_ENABLE);
  lsm6dsv16x_filt_xl_lp2_bandwidth_set(&dev_ctx, LSM6DSV16X_XL_STRONG);    


//  lsm6dsv16x_pin_int_route_t pin_int;
//  pin_int.fifo_th = PROPERTY_ENABLE;
//  lsm6dsv16x_pin_int1_route_set(&dev_ctx, &pin_int);


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

MotionFX文件

主要包含lsm6dsv16x_app.c和lsm6dsv16x_app.h，這兩個文件主要負責初始化和管理LSM6DSV16X傳感器的交互。它們提供了配置傳感器、初始化通信接口以及讀取傳感器數據的功能。
該文件包含與LSM6DSV16X傳感器交互所需函數的實現。它提供了配置傳感器、初始化通信接口以及讀取傳感器數據的功能。
lsm6dsv16x_init(): 初始化MotionFX算法。
lsm6dsv16x_motion_fx_determin(): 該函數主要用于讀取傳感器數據并使用MotionFX庫進行數據融合處理

卡爾曼濾波算法

運行卡爾曼濾波傳播算法MotionFX_propagate。

根據需要更新卡爾曼濾波器MotionFX_update。

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

對應的demo在2.2.9有提供。

主程序執行流程

讀取FIFO水印標志：
○ 使用 lsm6dsv16x_fifo_status_get() 函數讀取FIFO水印標志，判斷FIFO中的數據是否達到設定的閾值。
處理FIFO數據：
○ 如果FIFO水印標志被設置，讀取FIFO中的數據數量。
○ 切換LED狀態，用于指示數據讀取狀態。
○ 使用 lsm6dsv16x_fifo_out_raw_get() 函數逐項讀取FIFO中的傳感器數據。
○ 根據數據標簽（tag）識別數據類型并處理：
■ 加速度計數據：設置 acc_flag 標志位，并轉換數據單位。
■ 陀螺儀數據：設置 gyr_flag 標志位，并轉換數據單位。
■ 時間戳數據：設置 deltatime_flag 標志位，并計算時間差。
調用姿態估計算法：
○ 當加速度計、陀螺儀和時間戳數據都已讀取時，調用 lsm6dsv16x_motion_fx_determin() 函數進行姿態估計。
○ 重置標志位并更新時間戳。

/* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    uint16_t num = 0;
    /* Read watermark flag */
    lsm6dsv16x_fifo_status_get(&dev_ctx, &fifo_status);
        uint8_t acc_flag=0,gyr_flag=0;//加速度角速度標志位
        uint8_t deltatime_flag=0;//時間標志位
    if (fifo_status.fifo_th == 1) {
      num = fifo_status.fifo_level;
//      printf( "-- FIFO num %d rn", num);

      while (num--) {
        lsm6dsv16x_fifo_out_raw_t f_data;

        /* Read FIFO sensor value */
        lsm6dsv16x_fifo_out_raw_get(&dev_ctx, &f_data);
        datax = (int16_t *)&f_data.data[0];
        datay = (int16_t *)&f_data.data[2];
        dataz = (int16_t *)&f_data.data[4];
        ts = (int32_t *)&f_data.data[0];

        switch (f_data.tag) {
        case LSM6DSV16X_XL_NC_TAG:
                    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);
//          printf( "ACC [mg]:t%4.2ft%4.2ft%4.2frn",
//                  lsm6dsv16x_from_fs4_to_mg(*datax),
//                  lsm6dsv16x_from_fs4_to_mg(*datay),
//                  lsm6dsv16x_from_fs4_to_mg(*dataz));
          break;
        case LSM6DSV16X_GY_NC_TAG:
                    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);            
//          printf("GYR [mdps]:t%4.2ft%4.2ft%4.2frn",
//                  lsm6dsv16x_from_fs4000_to_mdps(*datax),
//                  lsm6dsv16x_from_fs4000_to_mdps(*datay),
//                  lsm6dsv16x_from_fs4000_to_mdps(*dataz));
          break;
        case LSM6DSV16X_TIMESTAMP_TAG:
                    deltatime_flag=1;
                    if(deltatime_first==0)//第一次
                    {
                        deltatime_1=*ts;
                        deltatime_2=deltatime_1;
                        deltatime_first=1;
                    }
                    else
                    {
                        deltatime_2=*ts;
                    }

//          printf( "TIMESTAMP [ms] %drn", *ts);

          break;
        default:
          break;
        }
                if(acc_flag&&gyr_flag&&deltatime_flag)
                {
                    lsm6dsv16x_motion_fx_determin();
                    acc_flag=0;
                    gyr_flag=0;
                    deltatime_flag=0;
                    deltatime_1=deltatime_2;            
                }    
      }
//      printf("------ rnrn");
    }
//            HAL_Delay(10);

    /* USER CODE END WHILE */

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

lsm6dsv16x_motion_fx_determin

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

extern    float acc_x,acc_y,acc_z;
extern    float gyr_x,gyr_y,gyr_z;
extern uint32_t deltatime_1,deltatime_2;
extern int out_num;
void lsm6dsv16x_motion_fx_determin(void){

//    lsm6dsv16x_data_ready_t drdy;
//    
//    /* Read output only if new xl value is available */
//  lsm6dsv16x_flag_data_ready_get(&dev_ctx, &drdy);
//    
//    if (drdy.drdy_xl){
//        /* Read acceleration field data */
//        memset(data_raw_acceleration, 0x00, 3 * sizeof(int16_t));
//        
//        lsm6dsv16x_acceleration_raw_get(&dev_ctx, 
//                                                        data_raw_acceleration);
//        
//        sensor_hub_data.acceleration[0] =    lsm6dsv16x_from_fs2_to_mg(
//                                                        data_raw_acceleration[0]);
//        sensor_hub_data.acceleration[1] =    lsm6dsv16x_from_fs2_to_mg(
//                                                        data_raw_acceleration[1]);
//        sensor_hub_data.acceleration[2] =    lsm6dsv16x_from_fs2_to_mg(
//                                                        data_raw_acceleration[2]);
//    }
//    
//    if (drdy.drdy_gy){
//        memset(data_raw_angular_rate, 0x00, 3 * sizeof(int16_t));

//        /* 讀取角速度數據,并將 角速度 轉換為 dps */
//        lsm6dsv16x_angular_rate_raw_get(&dev_ctx,
//                                                                         data_raw_angular_rate);
//        sensor_hub_data.angular_rate[0] = lsm6dsv16x_from_fs2000_to_mdps(
//                                                         data_raw_angular_rate[0]);
//        sensor_hub_data.angular_rate[1] = lsm6dsv16x_from_fs2000_to_mdps(
//                                                         data_raw_angular_rate[1]);
//        sensor_hub_data.angular_rate[2] = lsm6dsv16x_from_fs2000_to_mdps(
//                                                         data_raw_angular_rate[2]);
//    }
//    
    // 將全局變量中的加速度計和陀螺儀數據存儲在 sensor_hub_data 結構體中
    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 算法庫，計算四元數，參考自 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相關，delta_time為2次數據的間隔 */
//        float delta_time = DELATE_TIME;
    float delta_time[1];
    if(deltatime_2 >deltatime_1)
    {
        delta_time[0]=(float)((double)(deltatime_2-deltatime_1)*21.75f/1000000);
//        printf("d=%fn",delta_time[0]);
        /* 運行卡爾曼濾波傳播算法 */
        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)(0xffffffff-deltatime_2+deltatime_1)*21.75f/1000000);    
        /* 運行卡爾曼濾波傳播算法 */
        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;    
    }
//    /* 運行卡爾曼濾波傳播算法 */
//    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);

    /* 將四元數存儲到數組，方便后續操作 */
//    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 數據協議格式，輸出四元數數據 */
    /* 斜視圖 右前上視角：scalar | x | y | z */
//    printf("%f, %f, %f, %f n",Quaternions_data[3],
                        Quaternions_data[1],Quaternions_data[2],Quaternions_data[0]);
    if(out_num< 10)// 每10次輸出一次旋轉數據
        out_num++;
    else
    {
    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]);
        out_num=0;
    }
}

歐拉角簡介

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

橫滾roll，俯仰pitch，偏航yaw的實際含義如下圖：

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

演示

初始位置和數據輸出如下所示。

逆時針旋轉90°

逆時針旋轉180°

逆時針旋轉270°

審核編輯 黃宇