驱动LSM6DS3TR-C实现高效运动检测与数据采集(8)----中断获取FIFO数据并应用MotionFX库解析空间坐标
描述
概述
本文将探讨如何使用中断机制获取FIFO数据并应用MotionFX库解析空间坐标。MotionFX库是一种用于传感器融合的强大工具，可以将加速度计、陀螺仪和磁力计的数据融合在一起，实现精确的姿态和位置估计。本文将介绍如何初始化和配置MotionFX库，使用中断机制读取FIFO中的传感器数据。FIFO可以作为数据缓冲区，存储传感器的临时数据，防止数据丢失，特别是在处理器忙于其他任务时。本文将利用这些数据进行空间坐标的解析。本章案例基于上节的demo进行修改。
需要样片的可以加群申请：615061293 。
视频教学
[https://www.bilibili.com/video/BV1pm421G7XE/]
样品申请
[https://www.wjx.top/vm/OhcKxJk.aspx#]
源码下载
[https://download.csdn.net/download/qq_24312945/89537268]
开启LED
配置PB14为输出模式。
开启INT中断
陀螺仪LSM6DS3TR-C的中断管脚接到了PB0，需要将PB0设置为中端口。
开启中断。
参考驱动程序
[https://github.com/STMicroelectronics/lsm6ds3tr-c-pid]
中断读取传感器数据
为了使用回调函数并获取FIFO中的数据，在main.c定义了以下变量。
float acc_x,acc_y,acc_z;
float gyr_x,gyr_y,gyr_z;
uint32_t deltatime_1,deltatime_2;
uint8_t deltatime_first=0;
stmdev_ctx_t dev_ctx;
uint8_t waterm = 0;
/// 用于存储FIFO中读取的数据，每条数据包含24个字节,*2保证数据不溢出
uint8_t fifo_data[20*3*2][6];
// FIFO中当前存储的数据数量
uint16_t fifo_num = 0;
// FIFO中断标志，用于标记是否有新的FIFO数据可供读取
uint8_t fifo_flag=0;
uint8_t acc_fifo[20*2][6];
uint8_t gyr_fifo[20*2][6];
uint8_t timestamp_fifo[20*2][6];
mian.c中开启中断。
lsm6ds3tr_c_int1_route_t pin_int;
lsm6ds3tr_c_pin_int1_route_get(&dev_ctx, &pin_int);
pin_int.int1_fifo_ovr = PROPERTY_ENABLE;
lsm6ds3tr_c_pin_int1_route_set(&dev_ctx, pin_int);
需要注意优化等级。
完整初始化如下所示。
/* 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 */
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);
/* 设置加速度计和陀螺仪的满量程范围 */
lsm6ds3tr_c_xl_full_scale_set(&dev_ctx, LSM6DS3TR_C_4g);
lsm6ds3tr_c_gy_full_scale_set(&dev_ctx, LSM6DS3TR_C_2000dps);
/* 启用块数据更新（BDU），当FIFO支持时 */
lsm6ds3tr_c_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
lsm6ds3tr_c_xl_power_mode_set(&dev_ctx, LSM6DS3TR_C_XL_HIGH_PERFORMANCE);
/* 设置加速度计和陀螺仪的输出数据速率：
* 在本例中，我们将加速度计和陀螺仪的速率设置为26 Hz
*/
lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, LSM6DS3TR_C_XL_ODR_416Hz);
lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, LSM6DS3TR_C_GY_ODR_416Hz);
lsm6ds3tr_c_fifo_mode_set(&dev_ctx, LSM6DS3TR_C_BYPASS_MODE);
HAL_Delay(10);
/* 设置FIFO水印为模式的倍数
* 在本例中，我们将水印设置为10个模式
* 这意味着10个序列：
* (陀螺仪 + 加速度计) = 12字节
* (外部传感器 + 时间戳) = 12字节
*/
lsm6ds3tr_c_int1_route_t int_1_reg;
uint16_t pattern_len = 24; // 每个数据集由6个字节组成,4*6=24
lsm6ds3tr_c_fifo_watermark_set(&dev_ctx, 10 * pattern_len);
/* 将FIFO模式设置为流模式 */
//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); // 根据需求配置步数计数
/* 将时间戳分辨率设置为25 μs (WAKE_UP_DUR寄存器中的TIMER_HR位) */
//WAKE_UP_DUR (5Ch)- >TIMER_HR
lsm6ds3tr_c_timestamp_res_set(&dev_ctx, LSM6DS3TR_C_LSB_25us);
//设置第3数据集（Dataset 3）的降采样因子
lsm6ds3tr_c_fifo_dataset_3_batch_set(&dev_ctx, LSM6DS3TR_C_FIFO_DS3_NO_DEC);
//设置第4数据集（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第四数据集
//FIFO_CTRL2 (07h)- >TIMER_PEDO_FIFO_EN
lsm6ds3tr_c_fifo_pedo_and_timestamp_batch_set(&dev_ctx, PROPERTY_ENABLE);
/* 设置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);
/* 设置FIFO的输出数据速率 */
//FIFO_CTRL5 (0Ah)
lsm6ds3tr_c_fifo_data_rate_set(&dev_ctx, LSM6DS3TR_C_FIFO_416Hz);
lsm6ds3tr_c_int1_route_t pin_int;
lsm6ds3tr_c_pin_int1_route_get(&dev_ctx, &pin_int);
pin_int.int1_fth = PROPERTY_ENABLE;
lsm6ds3tr_c_pin_int1_route_set(&dev_ctx, pin_int);
lsm6ds3tr_c_init();
/* USER CODE END 2 */
在stm32h5xx_it.c中添加回调函数引用。
/* USER CODE BEGIN 0 */
extern void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin);
/* USER CODE END 0 */
处理PB0外部中断线0（EXTI Line0）的中断。
/**
* @brief This function handles EXTI Line0 interrupt.
*/
void EXTI0_IRQHandler(void)
{
/* USER CODE BEGIN EXTI0_IRQn 0 */
HAL_GPIO_EXTI_Callback(GPIO_PIN_0);
/* USER CODE END EXTI0_IRQn 0 */
HAL_GPIO_EXTI_IRQHandler(INT1_Pin);
/* USER CODE BEGIN EXTI0_IRQn 1 */
/* USER CODE END EXTI0_IRQn 1 */
}
在main.c中添加回调函数的定义，检查中断是否由 GPIO_PIN_0 引脚触发，每次发生中断时从传感器获取当前的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_0)
{
HAL_GPIO_TogglePin(LED1_GPIO_Port, LED1_Pin);
/* 读取LSM6DS3TR-C的水印标志 */
lsm6ds3tr_c_fifo_wtm_flag_get(&dev_ctx, &waterm);
uint16_t num = 0,num1=0;
uint16_t num_pattern = 0;
if (waterm) {
fifo_flag=1;
/* 读取FIFO中的字数 */
lsm6ds3tr_c_fifo_data_level_get(&dev_ctx, &num);
num_pattern = num / 24*2;
// printf( "-- FIFO num %d num_pattern=%drn", num,num_pattern);
fifo_num=num_pattern;
for(int i=0;i< num_pattern;i++) {
/* 根据传感器的ODR配置，FIFO模式由以下样本序列组成：GYRO, XL 外部传感器 时间戳*/
lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx,
&gyr_fifo[i][0],
3 * sizeof(int16_t));
lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx,
&acc_fifo[i][0],
6);
//外部传感器数据
lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx,
data_raw_none.u8bit,
3 * sizeof(int16_t));
lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx,
×tamp_fifo[i][0],
3 * sizeof(int16_t));
}
}
}
}
/* USER CODE END 4 */
主程序
在主循环中检查FIFO中断标志，如果有新的FIFO数据，则读取并处理这些数据。处理完成后，调用MotionFX库函数进行数据融合计算，以获得传感器的姿态和位置。
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
if(fifo_flag)
{
for(int i=0;i< fifo_num;i++)// 遍历 FIFO 数据数组
{
int16_t gyr;
gyr=(gyr_fifo[i][1]< < 8) + gyr_fifo[i][0];
gyr_x =lsm6ds3tr_c_from_fs2000dps_to_mdps(gyr);
gyr=(gyr_fifo[i][3]< < 8) + gyr_fifo[i][2];
gyr_y =lsm6ds3tr_c_from_fs2000dps_to_mdps(gyr);
gyr=(gyr_fifo[i][5]< < 8) + gyr_fifo[i][4];
gyr_z =lsm6ds3tr_c_from_fs2000dps_to_mdps(gyr);
// printf(
// "gyr_x:%4.2ft%4.2ft%4.2frn",
// gyr_x, gyr_y, gyr_z);
int16_t acc;
acc=(acc_fifo[i][1]< < 8) + acc_fifo[i][0];
acc_x =lsm6ds3tr_c_from_fs4g_to_mg(acc);
acc=(acc_fifo[i][3]< < 8) + acc_fifo[i][2];
acc_y =lsm6ds3tr_c_from_fs4g_to_mg(acc);
acc=(acc_fifo[i][5]< < 8) + acc_fifo[i][4];
acc_z =lsm6ds3tr_c_from_fs4g_to_mg(acc);
// printf(
// "acc_x:%4.2ft%4.2ft%4.2frn",
// acc_x, acc_y, acc_z);
/* 读取时间戳数据 */
uint32_t timestamp=0;
timestamp=(timestamp_fifo[i][1]< < 16)|(timestamp_fifo[i][0]< < 8)
|(timestamp_fifo[i][3]);
// printf("Timestamp: %urn", timestamp);
if(deltatime_first==0)//第一次
{
deltatime_1=timestamp;
deltatime_2=deltatime_1;
deltatime_first=1;
}
else
{
deltatime_2=timestamp;
}
lsm6ds3tr_c_motion_fx_determin();
deltatime_1=deltatime_2;
}
fifo_flag=0;
}
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
演示
初始位置和数据输出如下所示。
逆时针旋转90°
逆时针旋转180°
逆时针旋转270°
审核编辑 黄宇
声明：本文内容及配图由入驻作者撰写或者入驻合作网站授权转载。文章观点仅代表作者本人，不代表电子发烧友网立场。文章及其配图仅供工程师学习之用，如有内容侵权或者其他违规问题，请联系本站处理。
举报投诉
-
LSM6DS3TR-C使用时工作电流比datasheet上大很多是什么原因？2024-03-07 0
-
LSM6DS3TR-C数据读取异常是安利的问题？怎么处理？2024-03-19 0
-
LSM6DS3TR-C的FIFO读取数据出错是什么原因造成的？怎么解决？2024-03-27 0
-
驱动LSM6DS3TR-C实现高效运动检测与数据采集(1)----获取ID2023-11-13 1710
-
驱动LSM6DS3TR-C实现高效运动检测与数据采集(2)----配置滤波器2023-11-14 731
-
陀螺仪LSM6DSOW开发(5)----MotionFX库解析空间坐标2024-08-15 1273
-
请问LSM6DS3TR和LSM6DS3TR-C两个型号能否完全兼容？2024-03-14 0
-
驱动LSM6DS3TR-C实现高效运动检测与数据采集(3)----获取传感器数据2023-11-14 624
-
驱动LSM6DS3TR-C实现高效运动检测与数据采集(4)----上报匿名上位机实现可视化2023-11-14 682
-
驱动LSM6DS3TR-C实现高效运动检测与数据采集(5)----姿态解算2023-11-14 611
-
陀螺仪LSM6DSV16X与AI集成(8)----MotionFX库解析空间坐标2024-07-18 683
-
陀螺仪LSM6DSV16X与AI集成(9)----中断获取FIFO数据并应用MotionFX库解析空间坐标2024-07-18 682
-
驱动LSM6DS3TR-C实现高效运动检测与数据采集(6)----FIFO数据读取与配置2024-07-18 714
-
驱动LSM6驱动LSM6DS3TR-C实现高效运动检测与数据采集(7)----MotionFX库解析空间坐标DS3TR-C实现高效运动检测与数据采2024-07-18 762
-
陀螺仪LSM6DSOW开发(4)----中断获取FIFO数据2024-08-08 223
全部0条评论
快来发表一下你的评论吧 !
