驱动LSM6DS3TR-C实现高效运动检测与数据采集(10)----融合磁力计进行姿态解算

概述

MotionFX库包含用于校准陀螺仪、加速度计和磁力计传感器的例程。 将磁力计的数据与加速度计和陀螺仪的数据融合，可以大幅提高姿态估计的精度。三轴加速度计提供设备的倾斜信息，陀螺仪提供角速度信息，而磁力计提供方位信息，三者结合可以提供更加准确和稳定的三维方向和姿态信息。

需要样片的可以加群申请：615061293 。

视频教学

https://www.bilibili.com/video/BV1V4421S7Xs/

驱动LSM6DS3TR-C实现高效运动检测与数据采集(10)----融合磁力计进行姿态解算

样品申请

https://www.wjx.top/vm/OhcKxJk.aspx#

源码下载

https://download.csdn.net/download/qq_24312945/89563161

硬件准备

首先需要准备一个开发板，这里我准备的是自己绘制的开发板，需要的可以进行申请。
主控为STM32H503CB，陀螺仪为LSM6DS3TR-C，磁力计为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度。