陀螺儀LSM6DSV16X與AI集成.2--姿態解算
- 概述
- 視頻教學
- 樣品申請
- 完整代碼下載
- 使用demo板
- 生成STM32CUBEMX
- 串口配置
- IIC配置
- CS和SA0設置
- 串口重定向
- 參考程序
- 初始化SFLP步驟
- 初始化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 緩沖區是一種數據存儲方式，允許臨時存儲傳感器數據。這對于有效管理數據流非常有用，特別是在數據處理可能不如數據收集那么快的系統中。
最近在弄ST和瑞薩RA的課程，需要樣片的可以加群申請：615061293 。
圖片包含了關于 LSM6DSV16X 傳感器的低功耗傳感器融合（Sensor Fusion Low Power, SFLP）功能的說明。這里是對圖片內容的解釋：
SFLP 功能：
- SFLP 單元用于生成基于加速度計和陀螺儀數據處理的以下數據：
- 游戲旋轉向量：以四元數形式表示設備的姿態。
- 重力向量：提供一個三維向量，表示重力方向。
- 陀螺儀偏差：提供一個三維向量，表示陀螺儀的偏差。
激活與重置： - 通過在 EMB_FUNC_EN_A（04h）嵌入式功能寄存器中設置 SFLP_GAME_EN 位為 1 來激活 SFLP 單元。
- 通過在 EMB_FUNC_INIT_A（66h）嵌入式功能寄存器中設置 SFLP_GAME_INIT 位為 1 來重置 SFLP 單元。
性能參數表：
表格展示了 SFLP 功能在不同情況下的性能，包括靜態精度、低動態精度和高動態精度，以及校準時間和方向穩定時間。這些參數反映了傳感器在不同運動狀態下的精確度和響應速度。
視頻教學
https://www.bilibili.com/video/BV1Jw41187c5/
陀螺儀LSM6DSV16X與AI集成(3)----讀取融合算法輸出的四元數
樣品申請
https://www.wjx.top/vm/OhcKxJk.aspx#
完整代碼下載
使用demo板
最近在弄ST和瑞薩RA的課程，需要樣片的可以加群申請：615061293 。
生成STM32CUBEMX
用STM32CUBEMX生成例程，這里使用MCU為STM32WB55RG。
配置時鐘樹，配置時鐘為32M。
串口配置
查看原理圖，PB6和PB7設置為開發板的串口。
配置串口。
IIC配置
配置IIC為快速模式，速度為400k。
CS和SA0設置
串口重定向
打開魔術棒，勾選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/stm32duino/LSM6DSV16X/blob/main/examples/LSM6DSV16X_Sensor_Fusion/LSM6DSV16X_Sensor_Fusion.ino
初始化SFLP步驟
啟用 LSM6DSV16X 傳感器中的旋轉向量低功耗傳感器融合（Rotation Vector SFLP）功能的步驟。旋轉向量是一個四元數，它提供了一個精確的設備姿態估計。這通常用于游戲控制、增強現實和虛擬現實等應用。下面是函數各部分的作用：
- 函數定義：LSM6DSV16XSensor_Enable_Rotation_Vector 旨在啟用旋轉向量功能，并返回操作的結果。如果成功，返回 0；如果出現錯誤，則返回錯誤代碼。
- 設置滿量程：函數首先設置加速度計和陀螺儀的滿量程，這是傳感器能夠測量的最大范圍。這里分別設置為 4g 和 2000 度每秒（dps）。
- 獲取 FIFO SFLP 設置：然后，它讀取當前的 FIFO SFLP（傳感器融合低功耗）配置。
- 啟用旋轉向量 SFLP 特性：通過將 fifo_sflp.game_rotation 設為 1 來啟用游戲旋轉向量功能。
- 設置 FIFO 模式：將 FIFO 設置為流模式（也稱為連續模式），在此模式下，數據持續地流入 FIFO，如果 FIFO 滿了，新數據會覆蓋舊數據。
- 設置數據輸出率：為加速度計和陀螺儀以及 SFLP 設置數據輸出率（ODR），在這里都設置為每秒 120 次采樣（120Hz）。
- 啟用 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 FIFOif (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個字節數據，進行四元數讀取。
最后轉換為姿態角。
/* Infinite loop *//* USER CODE BEGIN WHILE */while (1){uint16_t fifo_samples;lsm6dsv16x_fifo_out_raw_t f_data;/* Read watermark flag */status=lsm6dsv16x_fifo_status_get(&dev_ctx, &fifo_status);// Check the number of samples inside FIFOif (status != LSM6DSV16X_OK) {printf("LSM6DSV16X Sensor failed to get number of samples inside FIFO");while (1);}fifo_samples = fifo_status.fifo_level;// Read the FIFO if there is one stored sampleif (fifo_samples > 0) {for (int i = 0; i < fifo_samples; i++) {lsm6dsv16x_fifo_out_raw_get(&dev_ctx,&f_data);if(f_data.tag==LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG){LSM6DSV16XSensor_FIFO_Get_Rotation_Vector(&dev_ctx,&quaternions[0]);// Print Quaternion data
// printf("Quaternion: %.4lf,%.4lf,%.4lf,%.4lf\n",quaternions[3],quaternions[0],quaternions[1],quaternions[2]); q0=quaternions[3];q1=quaternions[0];q2=quaternions[1];q3=quaternions[2];Pitch = asin(2 * q2 * q3 + 2 * q0* q1)* 57.3; // pitch ,轉換為度數Roll = atan2(-2 * q1 * q3 + 2 * q0 * q2, q0*q0-q1*q1-q2*q2+q3*q3)* 57.3; // rollvYaw = atan2(2*(q1*q2 - q0*q3),q0*q0-q1*q1+q2*q2-q3*q3) * 57.3; //偏移太大， Roll=Roll*100;Pitch=Pitch*100;Yaw=Yaw*100;data_angular_rate_raw[8]=(int16_t)Roll>>8;//rolldata_angular_rate_raw[7]=(int16_t)Roll;data_angular_rate_raw[10]=(int16_t)Pitch>>8;//pitchdata_angular_rate_raw[9]=(int16_t)Pitch;data_angular_rate_raw[12]=(int16_t)Yaw>>8;//yawdata_angular_rate_raw[11]=(int16_t)Yaw;sumcheck = 0;addcheck = 0;for(uint16_t i=0; i < 14; i++){sumcheck += data_angular_rate_raw[i]; //從幀頭開始，對每一字節進行求和，直到 DATA 區結束addcheck += sumcheck; //每一字節的求和操作，進行一次 sumcheck 的累加}data_angular_rate_raw[14]=sumcheck;data_angular_rate_raw[15]=addcheck;HAL_UART_Transmit(&huart1 , (uint8_t *)&data_angular_rate_raw, 16, 0xFFFF);
// printf("Roll=%.2f,Pitch=%.2f,Yaw=%.2f\n",Roll,Pitch,Yaw); }}}HAL_Delay(10);/* USER CODE END WHILE *//* USER CODE BEGIN 3 */}/* USER CODE END 3 */
演示
切換主題色)
案例教程)
