概述
本文将探讨如何使用MotionFX库解析空间坐标。MotionFX库是一种用于传感器融合的强大工具,可以将加速度计、陀螺仪和磁力计的数据融合在一起,实现精确的姿态和位置估计。本文将介绍如何初始化和配置MotionFX库,使用FIFO读取传感器数据,FIFO可以作为数据缓冲区,存储传感器的临时数据。这样可以防止数据丢失,特别是在处理器忙于其他任务时,并利用这些数据进行空间坐标的解析。本章案例使用上节的demo进行修改。
最近在弄ST和瑞萨RA的课程,需要样片的可以加群申请:615061293 。
视频教学
[https://www.bilibili.com/video/BV12y411i72K/]
样品申请
[https://www.wjx.top/vm/OhcKxJk.aspx#]
源码下载
[https://download.csdn.net/download/qq_24312945/89602067]
硬件准备
首先需要准备一个开发板,这里我准备的是自己绘制的开发板,需要的可以进行申请。
主控为STM32H503CB,陀螺仪为LSM6DSOW,磁力计为LIS2MDL。
开启CRC
串口设置
设置串口速率为2000000。
开启X-CUBE-MEMS1
速率选择
加速度和角速度的速率尽量大于100Hz。
XL_HM_MODE 和 G_HM_MODE 是LSM6DSOW传感器中的配置位,用于设置加速度计和陀螺仪的高性能模式或低功耗模式。这些模式可以根据应用需求选择,以优化功耗或提高性能。
XL_HM_MODE 位于 CTRL6_C (15h) 寄存器中,用于控制加速度计的高性能模式。在高性能模式下,加速度计提供更高的采样率和精度,但功耗也会增加。
XL_HM_MODE 位描述
● 位位置: 4
● 默认值: 0(高性能模式启用)
● 描述: 控制加速度计的高性能模式。
○ 0: 启用高性能操作模式(默认)。
○ 1: 禁用高性能操作模式,启用低功耗模式。
G_HM_MODE 位于 CTRL7_G (16h) 寄存器中,用于控制陀螺仪的高性能模式。在高性能模式下,陀螺仪提供更高的采样率和精度,但功耗也会增加。
G_HM_MODE 位描述
● 位位置: 7
● 默认值: 0(高性能模式启用)
● 描述: 控制陀螺仪的高性能模式。
○ 0: 启用高性能操作模式(默认)。
○ 1: 禁用高性能操作模式,启用低功耗模式。
添加到初始化位置。
lsm6dso_xl_power_mode_set(&dev_ctx, LSM6DSO_HIGH_PERFORMANCE_MD);
lsm6dso_gy_power_mode_set(&dev_ctx, LSM6DSO_GY_HIGH_PERFORMANCE);
设置FIFO触发阈值为30.
lsm6dso_fifo_watermark_set(&dev_ctx, 30);
修改定义FIFO存储数组的长度为阈值的2倍。
uint8_t fifo_data[30*2][7];
初始化定义
/* 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);
/* Uncomment to configure INT 1 */
lsm6dso_pin_int1_route_t int1_route;
/* 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 */
lsm6dso_device_id_get(&dev_ctx, &whoamI);
printf("LSM6DSO_ID=0x%x,whoamI=0x%x",LSM6DSO_ID,whoamI);
if (whoamI != LSM6DSO_ID)
while (1);
/* Restore default configuration */
lsm6dso_reset_set(&dev_ctx, PROPERTY_ENABLE);
do {
lsm6dso_reset_get(&dev_ctx, &rst);
} while (rst);
/* Disable I3C interface */
lsm6dso_i3c_disable_set(&dev_ctx, LSM6DSO_I3C_DISABLE);
/* Enable Block Data Update */
lsm6dso_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
/* Set full scale */
lsm6dso_xl_full_scale_set(&dev_ctx, LSM6DSO_2g);
lsm6dso_gy_full_scale_set(&dev_ctx, LSM6DSO_2000dps);
/* Set FIFO watermark (number of unread sensor data TAG + 6 bytes
* stored in FIFO) to 10 samples
*/
lsm6dso_fifo_watermark_set(&dev_ctx, 30);
/* Set FIFO batch XL/Gyro ODR to 12.5Hz */
lsm6dso_fifo_xl_batch_set(&dev_ctx, LSM6DSO_XL_BATCHED_AT_417Hz);
lsm6dso_fifo_gy_batch_set(&dev_ctx, LSM6DSO_GY_BATCHED_AT_417Hz);
/* Set FIFO mode to Stream mode (aka Continuous Mode) */
lsm6dso_fifo_mode_set(&dev_ctx, LSM6DSO_STREAM_MODE);
/* Enable drdy 75 μs pulse: uncomment if interrupt must be pulsed */
lsm6dso_data_ready_mode_set(&dev_ctx, LSM6DSO_DRDY_PULSED);
/* Uncomment if interrupt generation on Free Fall INT1 pin */
lsm6dso_pin_int1_route_get(&dev_ctx, &int1_route);
int1_route.fifo_th = PROPERTY_ENABLE;
lsm6dso_pin_int1_route_set(&dev_ctx, int1_route);
/* Uncomment if interrupt generation on Free Fall INT2 pin */
//lsm6dso_pin_int2_route_get(&dev_ctx, &int2_route);
//int2_route.reg.int2_ctrl.int2_fifo_th = PROPERTY_ENABLE;
//lsm6dso_pin_int2_route_set(&dev_ctx, &int2_route);
/* Set Output Data Rate */
lsm6dso_xl_data_rate_set(&dev_ctx, LSM6DSO_XL_ODR_417Hz);
lsm6dso_gy_data_rate_set(&dev_ctx, LSM6DSO_GY_ODR_417Hz);
lsm6dso_fifo_timestamp_decimation_set(&dev_ctx, LSM6DSO_DEC_1);
/* Enable timestamp */
lsm6dso_timestamp_set(&dev_ctx, PROPERTY_ENABLE);
lsm6dso_xl_power_mode_set(&dev_ctx, LSM6DSO_HIGH_PERFORMANCE_MD);
lsm6dso_gy_power_mode_set(&dev_ctx, LSM6DSO_GY_HIGH_PERFORMANCE);
lsm6dso_init();
/* USER CODE END 2 */
MotionFX文件
主要包含lsm6dso_app.c和lsm6dso_app.h,这两个文件主要负责初始化和管理LSM6DSOW传感器的交互。它们提供了配置传感器、初始化通信接口以及读取传感器数据的功能。
该文件包含与LSM6DSOW传感器交互所需函数的实现。它提供了配置传感器、初始化通信接口以及读取传感器数据的功能。
lsm6dso_init(): 初始化MotionFX算法。
lsm6dso_motion_fx_determin(): 该函数主要用于读取传感器数据并使用MotionFX库进行数据融合处理。
卡尔曼滤波算法
运行卡尔曼滤波传播算法MotionFX_propagate。
根据需要更新卡尔曼滤波器MotionFX_update。
需要注意的是这2各算法非常吃资源,需要注意MCU算力分配。
函数结构如下所示。
对应的demo在2.2.9有提供。
主程序执行流程
- 读取FIFO水印标志:
○ 使用 中断以及lsm6dso_fifo_wtm_flag_get() 函数读取FIFO水印标志,判断FIFO中的数据是否达到设定的阈值。 - 处理FIFO数据:
○ 如果FIFO水印标志被设置,读取FIFO中的数据数量。
○ 使用 lsm6dso_fifo_out_raw_get() 函数逐项读取FIFO中的传感器数据。 - 调用姿态估计算法:
○ 当加速度计、陀螺仪和时间戳数据都已读取时,调用 lsm6dso_motion_fx_determin() 函数进行姿态估计。
○ 重置标志位并更新时间戳。
/* 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;//时间标志位
// printf("fifo_num=%dn",fifo_num);
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];
// 根据数据标签处理不同类型的数据
switch (fifo_data[i][0]) {
case LSM6DSO_XL_NC_TAG:// 加速度数据
acc_flag=1;
acc_x=lsm6dso_from_fs2_to_mg(*datax);
acc_y=lsm6dso_from_fs2_to_mg(*datay);
acc_z=lsm6dso_from_fs2_to_mg(*dataz);
// printf("Acceleration [mg]:%4.2ft%4.2ft%4.2frn",
// acc_x, acc_y, acc_z);
break;
case LSM6DSO_GYRO_NC_TAG:// 角速度数据
gyr_flag=1;
gyr_x=lsm6dso_from_fs2000_to_mdps(*datax);
gyr_y=lsm6dso_from_fs2000_to_mdps(*datay);
gyr_z=lsm6dso_from_fs2000_to_mdps(*dataz);
// printf("Angular rate [mdps]:%4.2ft%4.2ft%4.2frn",
// gyr_x,gyr_y,gyr_z);
break;
case LSM6DSO_TIMESTAMP_TAG:// 时间戳数据
deltatime_flag=1;
/* 读取时间戳数据 */
uint32_t timestamp=0;
timestamp+= fifo_data[i][1];
timestamp+= fifo_data[i][2]< < 8;
timestamp+= fifo_data[i][3]< < 16;
timestamp+= fifo_data[i][4]< < 24;
if(deltatime_first==0)//第一次
{
deltatime_1=timestamp;
deltatime_2=deltatime_1;
deltatime_first=1;
}
else
{
deltatime_2=timestamp;
}
// printf("timestamp=%drn",timestamp);
break;
default:
break;
}
// 如果加速度、角速度和时间戳数据都已获取
if(acc_flag&&gyr_flag&&deltatime_flag)
{
lsm6dso_motion_fx_determin();// 调用 MotionFX 处理函数
acc_flag=0;
gyr_flag=0;
deltatime_flag=0;
deltatime_1=deltatime_2; // 更新时间戳
}
}
// 清除 FIFO 标志和数据量
fifo_flag=0;
fifo_num=0;
}
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
欧拉角简介
欧拉角(Euler Angles)是一种表示三维旋转的方式,通过三个角度来描述物体在三维空间中的姿态。这三个角度通常称为滚转角(Roll)、俯仰角(Pitch)和偏航角(Yaw),它们分别表示绕物体的自身坐标系的三个轴的旋转。
横滚roll,俯仰pitch,偏航yaw的实际含义如下图:
● 优点
表示简单直观,易于理解。
适用于描述固定顺序的旋转操作。
● 缺点
存在万向节死锁问题(Gimbal Lock),即当俯仰角接近±90度时,会失去一个自由度,导致系统无法确定物体的姿态。
旋转顺序不同会导致不同的最终姿态,需要特别注意旋转顺序。
演示
初始位置和数据输出如下所示。
逆时针旋转90°
逆时针旋转180°
逆时针旋转270°
审核编辑 黄宇
-
传感器
+关注
关注
2541文章
49845浏览量
746941 -
mems
+关注
关注
128文章
3864浏览量
189789 -
陀螺仪
+关注
关注
44文章
768浏览量
98119 -
空间坐标
+关注
关注
0文章
4浏览量
5573
发布评论请先 登录
相关推荐
陀螺仪LSM6DSV16X与AI集成(7)----FIFO数据读取与配置
![<b class='flag-5'>陀螺仪</b><b class='flag-5'>LSM6</b>DSV16X与AI集成(7)----FIFO数据读取与配置](https://file1.elecfans.com/web2/M00/FE/0C/wKgaomaYfhCASGSHAAn1ptBupx4254.png)
陀螺仪LSM6DSV16X与AI集成(9)----中断获取FIFO数据并应用MotionFX库解析空间坐标
![<b class='flag-5'>陀螺仪</b><b class='flag-5'>LSM6</b>DSV16X与AI集成(9)----中断获取FIFO数据并应用<b class='flag-5'>MotionFX</b><b class='flag-5'>库</b><b class='flag-5'>解析</b><b class='flag-5'>空间</b><b class='flag-5'>坐标</b>](https://file1.elecfans.com/web2/M00/FE/0C/wKgaomaYfhCASGSHAAn1ptBupx4254.png)
驱动LSM6驱动LSM6DS3TR-C实现高效运动检测与数据采集(7)----MotionFX库解析空间坐标DS3TR-C实现高效运动检测与数据采
![驱动<b class='flag-5'>LSM6</b>驱动<b class='flag-5'>LSM6</b>DS3TR-C实现高效运动检测与数据采集(7)----<b class='flag-5'>MotionFX</b><b class='flag-5'>库</b><b class='flag-5'>解析</b><b class='flag-5'>空间</b><b class='flag-5'>坐标</b>DS3TR-C实现高效运动检测与数据采](https://file1.elecfans.com/web2/M00/FE/0F/wKgaomaYhQSACmgeAAkXM1pKhlA076.png)
陀螺仪LSM6DSOW开发(1)----轮询获取陀螺仪数据
![<b class='flag-5'>陀螺仪</b><b class='flag-5'>LSM6DSOW</b><b class='flag-5'>开发</b>(1)----轮询获取<b class='flag-5'>陀螺仪</b>数据](https://file1.elecfans.com/web2/M00/FE/0D/wKgaomaYgkSAUZwMAAlOsGN2SpE275.png)
陀螺仪LSM6DSOW开发(2)----上报匿名上位机实现可视化
![<b class='flag-5'>陀螺仪</b><b class='flag-5'>LSM6DSOW</b><b class='flag-5'>开发</b>(2)----上报匿名上位机实现可视化](https://file1.elecfans.com/web2/M00/FE/0D/wKgaomaYgkSAUZwMAAlOsGN2SpE275.png)
陀螺仪LSM6DSOW开发(3)----FIFO数据读取与配置
![<b class='flag-5'>陀螺仪</b><b class='flag-5'>LSM6DSOW</b><b class='flag-5'>开发</b>(3)----FIFO数据读取与配置](https://file1.elecfans.com/web2/M00/FE/0D/wKgaomaYgkSAUZwMAAlOsGN2SpE275.png)
陀螺仪LSM6DSV16X与AI集成(11)----融合磁力计进行姿态解算
![<b class='flag-5'>陀螺仪</b><b class='flag-5'>LSM6</b>DSV16X与AI集成(11)----融合磁力计进行姿态解算](https://file1.elecfans.com/web2/M00/00/21/wKgZomasjbqAcVQGAAkXM0hc4yg779.png)
供应LSM6DSLTR LSM6DSL 传感器 LSM6DSMTR LSM6DSM
MEMS陀螺仪技术原理_三轴陀螺仪技术原理
![MEMS<b class='flag-5'>陀螺仪</b>技术原理_三轴<b class='flag-5'>陀螺仪</b>技术原理](https://file1.elecfans.com//web2/M00/A7/00/wKgZomUMQb-AIlfqAAAMf-krFdE191.jpg)
光纤陀螺仪与旋转式陀螺仪的工作原理解析
![光纤<b class='flag-5'>陀螺仪</b>与旋转式<b class='flag-5'>陀螺仪</b>的工作原理<b class='flag-5'>解析</b>](https://file.elecfans.com/web1/M00/A9/90/o4YBAF2e3cmAJxVuAAEq-8SKHH0318.png)
驱动LSM6DS3TR-C实现高效运动检测与数据采集(8)----中断获取FIFO数据并应用MotionFX库解析空间坐标
![驱动<b class='flag-5'>LSM6</b>DS3TR-C实现高效运动检测与数据采集(8)----中断获取FIFO数据并应用<b class='flag-5'>MotionFX</b><b class='flag-5'>库</b><b class='flag-5'>解析</b><b class='flag-5'>空间</b><b class='flag-5'>坐标</b>](https://file1.elecfans.com/web2/M00/FE/7B/wKgaomabpfyAey7iAAkXM5ka7wg848.png)
评论