陀螺仪LSM6DSOW开发.7--融合磁力计进行姿态解算
概述
MotionFX库包含用于校准陀螺仪、加速度计和磁力计传感器的例程。 将磁力计的数据与加速度计和陀螺仪的数据融合，可以大幅提高姿态估计的精度。三轴加速度计提供设备的倾斜信息，陀螺仪提供角速度信息，而磁力计提供方位信息，三者结合可以提供更加准确和稳定的三维方向和姿态信息。
需要样片的可以加群申请：615061293 。
视频教学
https://www.bilibili.com/video/BV194421U7iw/
<iframe allowfullscreen="true" data-mediaembed="bilibili" frameborder="0" id="49NIQz6F-1722579649793" src="https://player.bilibili.com/player.html?aid=1756286467"></iframe>陀螺仪LSM6DSOW开发(7)----融合磁力计进行姿态解算
样品申请
https://www.wjx.top/vm/OhcKxJk.aspx#
源码下载
硬件准备
首先需要准备一个开发板，这里我准备的是自己绘制的开发板，需要的可以进行申请。
主控为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度。
