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

news2024/11/12 19:54:19

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

概述
视频教学
样品申请
源码下载
硬件准备
DataLogFusion
磁力计校准过程
初始化磁力计
MFX_Arithmetic_Init
卡尔曼滤波算法
演示

概述

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

最近在弄ST和瑞萨RA的课程，需要样片的可以加群申请：615061293 。

在这里插入图片描述

视频教学

样品申请

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

源码下载

硬件准备

首先需要准备一个开发板，这里我准备的是自己绘制的开发板，需要的可以进行申请。
主控为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);
}