概述

在現代的運動跟蹤和姿態檢測應用中，低功耗、高精度的傳感器數據融合處理變得越來越重要。LSM6DSV16X傳感器集成了SFLP（Sensor Fusion Low Power）算法模塊，可以在低功耗模式下實現六軸傳感器數據的高效融合。SFLP模塊通過處理加速度計和陀螺儀的數據，生成一個表示設備姿態的四元數，這為游戲、增強現實（AR）、虛擬現實（VR）等應用中的精準運動追蹤提供了技術支持。在本文中，我們將深入探討如何利用SFLP模塊獲取四元數數據，并分析其在實際應用中的優勢和實現方法。

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

視頻教學

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

樣品申請

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

源碼下載

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

硬件準備

首先需要準備一個開發板，這里我準備的是自己繪制的開發板，需要的可以進行申請。

主控為STM32H503CB，陀螺儀為LSM6DSV16X，磁力計為LIS2MDL。

SFLP

LSM6DSV16X 特性涉及到的是一種低功耗的傳感器融合算法（Sensor Fusion Low Power, SFLP）.
低功耗傳感器融合（SFLP）算法：
該算法旨在以節能的方式結合加速度計和陀螺儀的數據。傳感器融合算法通過結合不同傳感器的優勢，提供更準確、可靠的數據。
6軸游戲旋轉向量：
SFLP算法能夠生成游戲旋轉向量。這種向量是一種表示設備在空間中方向的數據，特別適用于游戲和增強現實應用，這些應用中理解設備的方向和運動非常關鍵。
四元數表示法：
旋轉向量以四元數的形式表示。四元數是一種編碼3D旋轉的方法，它避免了歐拉角等其他表示法的一些限制（如萬向節鎖）。一個四元數有四個分量（X, Y, Z 和 W），其中 X, Y, Z 代表向量部分，W 代表標量部分。
FIFO存儲：
四元數的 X, Y, Z 分量存儲在 LSM6DSV16X 的 FIFO（先進先出）緩沖區中。FIFO 緩沖區是一種數據存儲方式，允許臨時存儲傳感器數據。這對于有效管理數據流非常有用，特別是在數據處理可能不如數據收集那么快的系統中。

圖片包含了關于 LSM6DSV16X 傳感器的低功耗傳感器融合（Sensor Fusion Low Power, SFLP）功能的說明。這里是對圖片內容的解釋： SFLP 功能：

1. SFLP 單元用于生成基于加速度計和陀螺儀數據處理的以下數據：
2. 游戲旋轉向量：以四元數形式表示設備的姿態。
3. 重力向量：提供一個三維向量，表示重力方向。
4. 陀螺儀偏差：提供一個三維向量，表示陀螺儀的偏差。 激活與重置：
5. 通過在 EMB_FUNC_EN_A（04h）嵌入式功能寄存器中設置 SFLP_GAME_EN 位為 1 來激活 SFLP 單元。
6. 通過在 EMB_FUNC_INIT_A（66h）嵌入式功能寄存器中設置 SFLP_GAME_INIT 位為 1 來重置 SFLP 單元。 性能參數表： 表格展示了 SFLP 功能在不同情況下的性能，包括靜態精度、低動態精度和高動態精度，以及校準時間和方向穩定時間。這些參數反映了傳感器在不同運動狀態下的精確度和響應速度。

生成STM32CUBEMX

用STM32CUBEMX生成例程，這里使用MCU為STM32H503CB。

配置時鐘樹，配置時鐘為250M。

串口配置

查看原理圖，PA9和PA10設置為開發板的串口。

配置串口，速率為2000000。

IIC配置

LSM6DSV16X最大IIC通訊速率為1M。

配置IIC速度為1M

CS和SA0設置

由于還有一個磁力計，需要把該CS也使能。

ICASHE

修改堆棧

串口重定向

打開魔術棒，勾選MicroLIB

在main.c中，添加頭文件，若不添加會出現 identifier "FILE" is undefined報錯。

/* USER CODE BEGIN Includes */
#include "stdio.h"
/* USER CODE END Includes */

函數聲明和串口重定向：

/* USER CODE BEGIN PFP */
int fputc(int ch, FILE *f){
    HAL_UART_Transmit(&huart1 , (uint8_t *)&ch, 1, 0xFFFF);
    return ch;
}
/* USER CODE END PFP */

參考程序

[https://github.com/STMicroelectronics/lsm6dsv16x-pid]

初始換管腳

由于需要向LSM6DSV16X_I2C_ADD_L寫入以及為IIC模式。

所以使能CS為高電平，配置為IIC模式。 配置SA0為高電平。

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


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

獲取ID

可以向WHO_AM_I (0Fh)獲取固定值，判斷是否為0x70。

lsm6dsv16x_device_id_get為獲取函數。

對應的獲取ID驅動程序,如下所示。

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

復位操作

可以向CTRL3 (12h)的SW_RESET寄存器寫入1進行復位。

lsm6dsv16x_reset_set為重置函數。

對應的驅動程序,如下所示。

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

BDU設置

在很多傳感器中，數據通常被存儲在輸出寄存器中，這些寄存器分為兩部分：MSB和LSB。這兩部分共同表示一個完整的數據值。例如，在一個加速度計中，MSB和LSB可能共同表示一個加速度的測量值。
連續更新模式（BDU = ‘0’）：在默認模式下，輸出寄存器的值會持續不斷地被更新。這意味著在你讀取MSB和LSB的時候，寄存器中的數據可能會因為新的測量數據而更新。這可能導致一個問題：當你讀取MSB時，如果寄存器更新了，接下來讀取的LSB可能就是新的測量值的一部分，而不是與MSB相對應的值。這樣，你得到的就是一個“拼湊”的數據，它可能無法準確代表任何實際的測量時刻。
塊數據更新（BDU）模式（BDU = ‘1’）：當激活BDU功能時，輸出寄存器中的內容不會在讀取MSB和LSB之間更新。這就意味著一旦開始讀取數據（無論是先讀MSB還是LSB），寄存器中的那一組數據就被“鎖定”，直到兩部分都被讀取完畢。這樣可以確保你讀取的MSB和LSB是同一測量時刻的數據，避免了讀取到代表不同采樣時刻的數據。
簡而言之，BDU位的作用是確保在讀取數據時，輸出寄存器的內容保持穩定，從而避免讀取到拼湊或錯誤的數據。這對于需要高精度和穩定性的應用尤為重要。
可以向CTRL3 (12h)的BDU寄存器寫入1進行開啟。

對應的驅動程序,如下所示。

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

設置量程

速率可以通過CTRL1 (10h)設置加速度速率和CTRL2 (11h)進行設置角速度速率。

設置加速度量程可以通過CTRL8 (17h)進行設置。 設置角速度量程可以通過CTRL6 (15h)進行設置。

設置加速度和角速度的量程和速率可以使用如下函數。

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

初始化SFLP步驟

啟用 LSM6DSV16X 傳感器中的旋轉向量低功耗傳感器融合（Rotation Vector SFLP）功能的步驟。旋轉向量是一個四元數，它提供了一個精確的設備姿態估計。這通常用于游戲控制、增強現實和虛擬現實等應用。下面是函數各部分的作用：

1. 函數定義：LSM6DSV16XSensor_Enable_Rotation_Vector 旨在啟用旋轉向量功能，并返回操作的結果。如果成功，返回 0；如果出現錯誤，則返回錯誤代碼。
2. 設置滿量程：函數首先設置加速度計和陀螺儀的滿量程，這是傳感器能夠測量的最大范圍。這里分別設置為 4g 和 2000 度每秒（dps）。
3. 獲取 FIFO SFLP 設置：然后，它讀取當前的 FIFO SFLP（傳感器融合低功耗）配置。
4. 啟用旋轉向量 SFLP 特性：通過將 fifo_sflp.game_rotation 設為 1 來啟用游戲旋轉向量功能。
5. 設置 FIFO 模式：將 FIFO 設置為流模式（也稱為連續模式），在此模式下，數據持續地流入 FIFO，如果 FIFO 滿了，新數據會覆蓋舊數據。
6. 設置數據輸出率：為加速度計和陀螺儀以及 SFLP 設置數據輸出率（ODR），在這里都設置為每秒 120 次采樣（120Hz）。
7. 啟用 SFLP 低功耗模式：最后，啟用 SFLP 游戲旋轉向量特性，確保以低功耗模式運行。

初始化SFLP

開啟嵌入式函數訪問需要向 FUNC_CFG_ACCESS (01h)的EMB_FUNC_REG_ACCESS寫入1進行開啟。

/**
  * @brief  Change memory bank.[set]
  *
  * @param  ctx      read / write interface definitions
  * @param  val      MAIN_MEM_BANK, EMBED_FUNC_MEM_BANK,
  * @retval          interface status (MANDATORY: return 0 - > no Error)
  *
  */
int32_t lsm6dsv16x_mem_bank_set(stmdev_ctx_t *ctx, lsm6dsv16x_mem_bank_t val)
{
  lsm6dsv16x_func_cfg_access_t func_cfg_access;
  int32_t ret;

  ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1);
  if (ret != 0) { return ret; }

  func_cfg_access.shub_reg_access = ((uint8_t)val & 0x02U) > > 1;
  func_cfg_access.emb_func_reg_access = (uint8_t)val & 0x01U;
  ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1);

  return ret;
}

SFLP_GAME_FIFO_EN 是 LSM6DSV16X 傳感器中 EMB_FUNC_FIFO_EN_A（44h）寄存器的一個設置位。這個特定的位用于控制是否啟用將 SFLP（Sensor Fusion Low Power）算法計算出的游戲旋轉向量（四元數）值存儲到 FIFO（先進先出）緩沖區中的功能。當這個位被設置為 1 時，啟用了這個功能，使得算法計算出的游戲旋轉向量可以批量存儲到 FIFO 緩沖區中。默認值為 0，表示該功能默認是禁用的。

LSM6DSV16X 傳感器的 FIFO_CTRL4 (0Ah) 寄存器配置信息。這個寄存器控制著 FIFO（先進先出）緩沖區的各種操作和數據批處理（batching）的設置。 連續模式，如果 FIFO 已滿，新采集的樣本會覆蓋舊樣本。

在AN5763手冊中，也說明了融合數據會輸出在FIFO中，同時有如下的輸出速率，我們可以配置默認的速率。

最后對EMB_FUNC_EN_A (04h) 寄存器的SFLP_GAME_EN設置為1。

讀取四元數數據

FIFO_STATUS1（1Bh）和 FIFO_STATUS2（1Ch）寄存器中的 DIFF_FIFO [8:0] 字段包含在 FIFO 中收集的字（1 字節標簽 + 6 字節數據）的數量。

/* Read watermark flag */
    status=lsm6dsv16x_fifo_status_get(&dev_ctx, &fifo_status);


  // Check the number of samples inside FIFO
  if (status != LSM6DSV16X_OK) {
    printf("LSM6DSV16X Sensor failed to get number of samples inside FIFO");
    while (1);
  }
    fifo_samples = fifo_status.fifo_level;

之后需要通過FIFO_DATA_OUT_TAG (78h)判斷是什么數據準備好，當為SFLP game rotation vector(0X13)時候，為四元數準備完畢。

之后讀取FIFO_DATA_OUT_X_L (79h)到FIFO_DATA_OUT_Z_H (7Eh)共6個字節數據，進行四元數讀取。

最后轉換為姿態角。

/* USER CODE BEGIN WHILE */
  while (1)
  {

    uint16_t num = 0;

    /* Read watermark flag */
    lsm6dsv16x_fifo_status_get(&dev_ctx, &fifo_status);

    if (fifo_status.fifo_th == 1) {
      num = fifo_status.fifo_level;

      sprintf((char *)tx_buffer, "-- FIFO num %d rn", num);

      while (num--) {
        lsm6dsv16x_fifo_out_raw_t f_data;
        int16_t *axis;
        float quat[4];
        float gravity_mg[3];
        float gbias_mdps[3];

        /* Read FIFO sensor value */
        lsm6dsv16x_fifo_out_raw_get(&dev_ctx, &f_data);

        switch (f_data.tag) {
//        case LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG:
//          axis = (int16_t *)&f_data.data[0];
//          gbias_mdps[0] = lsm6dsv16x_from_fs125_to_mdps(axis[0]);
//          gbias_mdps[1] = lsm6dsv16x_from_fs125_to_mdps(axis[1]);
//          gbias_mdps[2] = lsm6dsv16x_from_fs125_to_mdps(axis[2]);
//          printf("GBIAS [mdps]:%4.2ft%4.2ft%4.2frn",
//                         (double_t)gbias_mdps[0], (double_t)gbias_mdps[1], (double_t)gbias_mdps[2]);

//          break;
//        case LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG:
//          axis = (int16_t *)&f_data.data[0];
//          gravity_mg[0] = lsm6dsv16x_from_sflp_to_mg(axis[0]);
//          gravity_mg[1] = lsm6dsv16x_from_sflp_to_mg(axis[1]);
//          gravity_mg[2] = lsm6dsv16x_from_sflp_to_mg(axis[2]);
//          printf("Gravity [mg]:%4.2ft%4.2ft%4.2frn",
//                         (double_t)gravity_mg[0], (double_t)gravity_mg[1], (double_t)gravity_mg[2]);

//          break;
        case LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG:
          sflp2q(quat, (uint16_t *)&f_data.data[0]);
//          printf("Game Rotation tX: %2.3ftY: %2.3ftZ: %2.3ftW: %2.3frn",
//                  (double_t)quat[0], (double_t)quat[1], (double_t)quat[2], (double_t)quat[3]);

                    float sx=quat[1];  
                    float sy=quat[2];  
                    float sz=quat[0];  
                    float sw=quat[3];

                    if (sw< 0.0f) 
                    {
                        sx*=-1.0f;
                        sy*=-1.0f;
                        sz*=-1.0f;
                        sw*=-1.0f;
                    }

                    float sqx = sx * sx;
                    float sqy = sy * sy;
                    float sqz = sz * sz;
                    float euler[3];
                    euler[0] = -atan2f(2.0f* (sy*sw+sx*sz), 1.0f-2.0f*(sqy+sqx));
                    euler[1] = -atan2f(2.0f * (sx*sy+sz*sw),1.0f-2.0f*(sqx+sqz));
                    euler[2] = -asinf(2.0f* (sx*sw-sy*sz));

                    if (euler[0] < 0.0f)
                        euler[0] +=2.0f*3.1415926;

                    for(uint8_t i=0; i< 3; i++){
                            euler[i] = 57.29578 * (euler[i]);
                    }

                    printf("euler[0]=%f,euler[1]=%f,euler[2]=%fn",euler[0],euler[1],euler[2]);

          break;
        default:
         break;
        }
      }

    }        


    /* USER CODE END WHILE */

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

審核編輯 黃宇