概述
本文將探討如何使用中斷機制獲取FIFO數據并應用MotionFX庫解析空間坐標。MotionFX庫是一種用于傳感器融合的強大工具,可以將加速度計、陀螺儀和磁力計的數據融合在一起,實現精確的姿態和位置估計。本文將介紹如何初始化和配置MotionFX庫,使用中斷機制讀取FIFO中的傳感器數據。FIFO可以作為數據緩沖區,存儲傳感器的臨時數據,防止數據丟失,特別是在處理器忙于其他任務時。本文將利用這些數據進行空間坐標的解析。本章案例基于上節的demo進行修改。
需要樣片的可以加群申請:615061293 。
視頻教學
[https://www.bilibili.com/video/BV1rx4y147Yv/]
樣品申請
[https://www.wjx.top/vm/OhcKxJk.aspx#]
源碼下載
[https://download.csdn.net/download/qq_24312945/89491135]
開啟LED
配置PB14為輸出模式。
開啟INT中斷
陀螺儀LSM6DSV16X的中斷管腳接到了PA6,需要將PA6設置為中端口。
開啟中斷。
參考驅動程序
[https://github.com/STMicroelectronics/lsm6dsv16x-pid/tree/main]
復位 FIFO
- 設置 FIFO 進入 Bypass 模式:將 FIFO_CTRL4 寄存器的 FIFO_MODE 位設置為 000。
- 將 FIFO 重新設置為所需模式:在復位命令之后,可以通過將 FIFO_CTRL4 寄存器的 FIFO_MODE 位設置為所需的模式(例如 001 表示 FIFO 模式)。
lsm6dsv16x_fifo_mode_set(&dev_ctx, LSM6DSV16X_BYPASS_MODE);
HAL_Delay(10);
添加到設置FIFO開啟之前。
中斷讀取傳感器數據
為了使用回調函數并獲取FIFO中的數據,在main.c定義了以下變量。
/* USER CODE END 0 */
stmdev_ctx_t dev_ctx;
lsm6dsv16x_fifo_status_t fifo_status;
/// 用于存儲FIFO中讀取的數據,每條數據包含7個字節(1個標簽字節和6個數據字節)
uint8_t fifo_data[FIFO_WATERMARK*2][7];
// FIFO中當前存儲的數據數量
uint16_t fifo_num = 0;
// FIFO中斷標志,用于標記是否有新的FIFO數據可供讀取
uint8_t fifo_flag=0;
/* USER CODE END 0 */
mian.c中開啟中斷。
lsm6dsv16x_pin_int_route_t pin_int;
lsm6dsv16x_pin_int1_route_get(&dev_ctx, &pin_int);
pin_int.fifo_th = PROPERTY_ENABLE;
lsm6dsv16x_pin_int1_route_set(&dev_ctx, &pin_int);
在stm32h5xx_it.c中添加回調函數引用。
/* USER CODE BEGIN 0 */
extern void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin);
/* USER CODE END 0 */
處理PA6外部中斷線6(EXTI Line6)的中斷。
/**
* @brief This function handles EXTI Line6 interrupt.
*/
void EXTI6_IRQHandler(void)
{
/* USER CODE BEGIN EXTI6_IRQn 0 */
HAL_GPIO_EXTI_Callback(GPIO_PIN_6);
/* USER CODE END EXTI6_IRQn 0 */
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_6);
/* USER CODE BEGIN EXTI6_IRQn 1 */
/* USER CODE END EXTI6_IRQn 1 */
}
在main.c中添加回調函數的定義,檢查中斷是否由 GPIO_PIN_6 引腳觸發,每次發生中斷時從傳感器獲取當前的FIFO狀態,并存儲在 fifo_status 變量中。讀取FIFO數據,并將這些數據存儲在一個全局數組 fifo_data 中,以便在主循環或其他地方進行處理。通過切換 LED 的狀態,可以直觀地了解中斷的發生。
/* USER CODE BEGIN 4 */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin){
if(GPIO_Pin == GPIO_PIN_6)
{
HAL_GPIO_TogglePin(LED1_GPIO_Port, LED1_Pin);
lsm6dsv16x_fifo_status_get(&dev_ctx, &fifo_status);
uint16_t num = 0;
if (fifo_status.fifo_th == 1) {
fifo_flag=1;
num = fifo_status.fifo_level;
fifo_num=num;
// printf( "-- FIFO num %d rn", num);
uint16_t num_i;
for(int i=0;i< num;i++) {
lsm6dsv16x_fifo_out_raw_t f_data;
/* Read FIFO sensor value */
lsm6dsv16x_fifo_out_raw_get(&dev_ctx, &f_data);
fifo_data[i][0]=f_data.tag;
fifo_data[i][1]=f_data.data[0];
fifo_data[i][2]=f_data.data[1];
fifo_data[i][3]=f_data.data[2];
fifo_data[i][4]=f_data.data[3];
fifo_data[i][5]=f_data.data[4];
fifo_data[i][6]=f_data.data[5];
// printf("data[0]=%d,data[1]=%d,data[2]=%d,data[3]=%d,data[4]=%d,data[5]=%d,data[6]=%dn",
// fifo_data[i][0],
// fifo_data[i][1],fifo_data[i][2],fifo_data[i][3],fifo_data[i][4],fifo_data[i][5],fifo_data[i][6]);
}
}
}
}
/* USER CODE END 4 */
主程序
在主循環中檢查FIFO中斷標志,如果有新的FIFO數據,則讀取并處理這些數據。處理完成后,調用MotionFX庫函數進行數據融合計算,以獲得傳感器的姿態和位置。
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
if(fifo_flag)// 如果 FIFO 中斷標志被設置
{
uint8_t acc_flag=0,gyr_flag=0;//加速度角速度標志位
uint8_t deltatime_flag=0;//時間標志位
for(int i=0;i< fifo_num;i++)// 遍歷 FIFO 數據數組
{
// 獲取數據指針
datax = (int16_t *)&fifo_data[i][1];
datay = (int16_t *)&fifo_data[i][3];
dataz = (int16_t *)&fifo_data[i][5];
ts = (int32_t *)&fifo_data[i][1];
// 根據數據標簽處理不同類型的數據
switch (fifo_data[i][0]) {
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);
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);
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;
}
break;
default:
break;
}
// 如果加速度、角速度和時間戳數據都已獲取
if(acc_flag&&gyr_flag&&deltatime_flag)
{
lsm6dsv16x_motion_fx_determin();// 調用 MotionFX 處理函數
acc_flag=0;
gyr_flag=0;
deltatime_flag=0;
deltatime_1=deltatime_2; // 更新時間戳
}
}
// 清除 FIFO 標志和數據量
fifo_flag=0;
fifo_num=0;
}
// HAL_Delay(10);
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
演示
初始位置和數據輸出如下所示。
逆時針旋轉90°
逆時針旋轉180°
逆時針旋轉270°
審核編輯 黃宇
-
陀螺儀
+關注
關注
44文章
780瀏覽量
98589 -
fifo
+關注
關注
3文章
387瀏覽量
43560 -
AI
+關注
關注
87文章
30239瀏覽量
268475
發布評論請先 登錄
相關推薦
評論