概述
MotionFX库包含用于校准陀螺仪、加速度计和磁力计传感器的例程。 将磁力计的数据与加速度计和陀螺仪的数据融合,可以大幅提高姿态估计的精度。三轴加速度计提供设备的倾斜信息,陀螺仪提供角速度信息,而磁力计提供方位信息,三者结合可以提供更加准确和稳定的三维方向和姿态信息。
需要样片的可以加群申请:615061293 。
视频教学
[https://www.bilibili.com/video/BV194421U7iw/]
样品申请
[https://www.wjx.top/vm/OhcKxJk.aspx#]
源码下载
[https://download.csdn.net/download/qq_24312945/89610048]
硬件准备
首先需要准备一个开发板,这里我准备的是自己绘制的开发板,需要的可以进行申请。
主控为STM32H503CB,陀螺仪为LSM6DSOW,磁力计为LIS2MDL。
DataLogFusion
这里参考ST提供的DataLogFusion程序,DataLogFusion示例应用展示了如何使用STMicroelectronics开发的MotionFX中间件库进行实时运动传感器数据融合。
DataLogFusion的主要执行流程包括初始化硬件和传感器、中间件库(MotionFX)的配置与初始化、传感器数据的采集、实时数据融合以及结果的输出。
磁力计校准过程
MotionFX库的磁力计校准库用于补偿硬铁失真。磁力计校准可以以比传感器融合输出数据速率更慢的频率进行(例如25 Hz)。
● 初始化磁力计校准库:
● 调用 MotionFX_MagCal_init 或 MotionFX_CM0P_MagCal_init 函数。
● 定期调用校准函数:
● 调用 MotionFX_MagCal_run 或 MotionFX_CM0P_MagCal_run 函数,直到校准成功完成。
● 检查校准是否成功:
● 调用 MotionFX_MagCal_getParams 或 MotionFX_CM0P_MagCal_getParams 函数。如果函数返回 mag_data_out.cal_quality = MFX_MAGCALGOOD 或 MFX_CM0P_CALQSTATUSBEST,则校准成功。
初始化磁力计
调用 MotionFX_MagCal_init 或 MotionFX_CM0P_MagCal_init 函数。这里通过调用 MotionFX_MagCal_init,确保磁力计校准模块处于准备就绪状态,能够正确处理和校准磁力计数据。
通过初始化磁力计校准库,并定期调用校准函数,可以确保磁力计数据的准确性,从而提高姿态估计的精度。
频率定义。
#define ALGO_FREQ 100U /* Algorithm frequency 100Hz */
#define ALGO_PERIOD (1000U / ALGO_FREQ) /* Algorithm period [ms] */
添加到初始化中进行调用。
文档中提到的磁力计数据要除以50,这是因为MotionFX库使用的单位是微特斯拉(µT)/50。
在lsm6ds3tr-c_app.h中添加定义。
#define FROM_MGAUSS_TO_UT50 (0.1f/50.0f)
MFX_Arithmetic_Init
MFX_Arithmetic_Init 的作用是初始化 MotionFX 算法库,并进行相关参数设置和配置。
由于现在是9轴解析,需要新定义用于保存 MotionFX 算法状态的数组。
static uint8_t mfxstate_9x[FX_STATE_SIZE];
函数功能:
● 初始化 MotionFX 算法库。
● 配置传感器的偏置值和方向。
● 设置输出参考模式。
● 启用或禁用 6 轴和 9 轴 MotionFX 引擎。
void MFX_Arithmetic_Init(void)
{
MFX_knobs_t iKnobs;
MFX_knobs_t *ipKnobs = &iKnobs;
/* 初始化 MotionFX 算法库,参考自 AlgoBuilded 生成代码 */
/* 初始化 MotionFX 引擎 */
MotionFX_initialize((MFXState_t *)mfxstate_9x);
/* 获取当前的内部结构参数 */
MotionFX_getKnobs(mfxstate_9x, ipKnobs);
/* 设置传感器 */
ipKnobs- >gbias_acc_th_sc = GBIAS_ACC_TH_SC_9X;
ipKnobs- >gbias_gyro_th_sc = GBIAS_GYRO_TH_SC_9X;
ipKnobs- >gbias_mag_th_sc = GBIAS_MAG_TH_SC_9X;
/* 未知作用操作,数据定向? */
ipKnobs- >acc_orientation[0] = ACC_ORIENTATION_X;
ipKnobs- >acc_orientation[1] = ACC_ORIENTATION_Y;
ipKnobs- >acc_orientation[2] = ACC_ORIENTATION_Z;
ipKnobs- >gyro_orientation[0] = GYR_ORIENTATION_X;
ipKnobs- >gyro_orientation[1] = GYR_ORIENTATION_Y;
ipKnobs- >gyro_orientation[2] = GYR_ORIENTATION_Z;
ipKnobs- >mag_orientation[0] = MAG_ORIENTATION_X;
ipKnobs- >mag_orientation[1] = MAG_ORIENTATION_Y;
ipKnobs- >mag_orientation[2] = MAG_ORIENTATION_Z;
/* 设置输出参考模式,数据参考系 */
ipKnobs- >output_type = MFX_ENGINE_OUTPUT_ENU;
ipKnobs- >LMode = 1;
/* modx 代表 MotionFX_update 函数调用频率,
* modx = 1,每调用 MotionFX_propagate 函数一次,可调用 MotionFX_update 函数一次,适用于STM32F4系列处理器,
* modx = 2,每调用 MotionFX_propagate 函数两次,可调用 MotionFX_update 函数一次,适用于STM32F1系列处理器。
*/
ipKnobs- >modx = 1;
/* 设置内部结构参数 */
MotionFX_setKnobs(mfxstate_9x, ipKnobs);
/* 使能6轴 MotionFX 引擎*/
MotionFX_enable_6X(mfxstate_9x, MFX_ENGINE_DISABLE);
/* 关闭9轴 MotionFX 引擎*/
MotionFX_enable_9X(mfxstate_9x, MFX_ENGINE_ENABLE);
}
卡尔曼滤波算法
运行卡尔曼滤波传播算法MotionFX_propagate。 根据需要更新卡尔曼滤波器MotionFX_update。 需要注意的是这2各算法非常吃资源,需要注意MCU算力分配。
函数结构如下所示。
演示
和指南针一个方向。
偏移90度。
偏移180度。
偏移270度。
审核编辑 黄宇
-
陀螺仪
+关注
关注
44文章
768浏览量
98127 -
磁力计
+关注
关注
1文章
71浏览量
20734 -
姿态解算
+关注
关注
0文章
49浏览量
8217
发布评论请先 登录
相关推荐
陀螺仪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开发(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)
陀螺仪LSM6DSOW开发(5)----MotionFX库解析空间坐标
![<b class='flag-5'>陀螺仪</b><b class='flag-5'>LSM6DSOW</b><b class='flag-5'>开发</b>(5)----MotionFX库解析空间坐标](https://file1.elecfans.com/web2/M00/FE/0D/wKgaomaYgkSAUZwMAAlOsGN2SpE275.png)
LSM9DS0陀螺仪和磁力计有哪些?
请问通常使用的磁力计、加速度计、陀螺仪等传感器在安装之前为什么要先校准?
MicroStrain 3DM-GX5-25陀螺仪
匿名飞控如何实现姿态解算呢
lsm6dsox的陀螺仪数据有很多尖峰是什么原因?如何解决?
陀螺仪LSM6DSOW开发(4)----中断获取FIFO数据
![<b class='flag-5'>陀螺仪</b><b class='flag-5'>LSM6DSOW</b><b class='flag-5'>开发</b>(4)----中断获取FIFO数据](https://file1.elecfans.com/web2/M00/FE/0D/wKgaomaYgkSAUZwMAAlOsGN2SpE275.png)
评论