驱动LSM6DS3TR-C实现高效运动检测与数据采集.8--中断获取FIFO数据并应用MotionFX库解析空间坐标
概述
本文将探讨如何使用中断机制获取FIFO数据并应用MotionFX库解析空间坐标。MotionFX库是一种用于传感器融合的强大工具,可以将加速度计、陀螺仪和磁力计的数据融合在一起,实现精确的姿态和位置估计。本文将介绍如何初始化和配置MotionFX库,使用中断机制读取FIFO中的传感器数据。FIFO可以作为数据缓冲区,存储传感器的临时数据,防止数据丢失,特别是在处理器忙于其他任务时。本文将利用这些数据进行空间坐标的解析。本章案例基于上节的demo进行修改。
需要样片的可以加群申请:615061293 。
视频教学
https://www.bilibili.com/video/BV1pm421G7XE/
驱动LSM6DS3TR-C实现高效运动检测与数据采集(8)----中断获取FIFO数据并应用MotionFX库解析空间坐标
样品申请
https://www.wjx.top/vm/OhcKxJk.aspx
源码下载
开启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=%d\r\n", 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.2f\t%4.2f\t%4.2f\r\n", // 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.2f\t%4.2f\t%4.2f\r\n", // 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: %u\r\n", 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°