驱动LSM6DS3TR-C实现高效运动检测与数据采集(11)----磁力计校准

记帖

于 2024-08-05 01:59:31 发布

阅读量443

点赞数 12

分类专栏： stm32cube 单片机传感器文章标签： mems 陀螺仪磁力计 LIS2MDL LSM6DS3TR-C MOTIONFX 欧拉角

本文链接：https://blog.csdn.net/qq_24312945/article/details/140915220

版权

单片机同时被 3 个专栏收录

256 篇文章 155 订阅

订阅专栏

stm32cube

136 篇文章 59 订阅

订阅专栏

传感器

92 篇文章 3 订阅

订阅专栏

驱动LSM6DS3TR-C实现高效运动检测与数据采集.11--磁力计校准

概述
视频教学
样品申请
源码下载
硬件准备
DataLogFusion
磁力计校准过程
MotionFX_MagCal_getParams
校准移植
演示

概述

磁力计校准是确保传感器数据准确性和可靠性的关键步骤。磁力计用于测量地球磁场，并在导航、定位、姿态测量等应用中起到重要作用。然而，磁力计在使用过程中会受到环境磁场、硬件偏差、安装误差等因素的影响，从而导致测量数据出现偏差。因此，校准磁力计以消除这些影响，是获得精确测量数据的必要步骤。
本文将介绍如何使用ST提供的MotionFX库在嵌入式系统中实现磁力计校准，通过本文的介绍，读者将能够理解磁力计校准的基本概念，掌握使用MotionFX库进行校准的步骤和方法，并学会如何在实际项目中实现磁力计的校准，以获得高精度的磁场测量数据。

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

在这里插入图片描述

视频教学

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

驱动LSM6DS3TR-C实现高效运动检测与数据采集(11)----磁力计校准

样品申请

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，则校准成功。

在这里插入图片描述

在app_mems.c中的Magneto_Sensor_Handler函数负责处理磁力计（磁传感器）数据的获取、校准和发送。该函数的主要功能是从磁力计传感器获取数据，进行必要的校准，然后将处理后的数据传输给其他部分使用。具体包括以下步骤：

检查传感器是否启用：函数首先检查是否启用了磁力计传感器。如果传感器未启用，则函数不会执行进一步操作。
获取磁力计数据：
○ 如果使用离线数据（即在调试或仿真模式下），函数会从预定义的离线数据数组中读取磁力计数据。
○ 如果使用实时数据，函数会通过调用BSP_SENSOR_MAG_GetAxes函数从实际的磁力计传感器中读取当前的磁力计数据。
执行磁力计校准：
○ 函数检查磁力计是否已经校准。如果尚未校准，则会进行校准过程。
○ 将获取的磁力计数据转换为适当的单位，并准备校准输入数据。
○ 调用MotionFX_manager_MagCal_run函数运行校准算法。
○ 如果校准结果良好（校准质量达到标准），函数会保存校准偏移值，并停止进一步的校准。
应用校准偏移：将校准偏移应用到磁力计读数中，以补偿硬件偏差和环境干扰。
序列化并发送数据：将校准后的磁力计数据序列化，并存储到消息结构（Msg）中，以供系统其他部分使用。

在这里插入图片描述

MotionFX_manager_MagCal_run函数用于运行磁力计校准算法。该函数接受磁力计输入数据，调用MotionFX库中的校准算法对数据进行处理，并返回校准结果。
MotionFX_MagCal_run函数执行校准算法，对输入数据进行处理，计算出校准所需的参数。
MotionFX_MagCal_getParams函数从校准算法中获取校准后的参数，并存储在输出数据结构data_out中。这些参数包括磁力计的偏移和标度因子等校准信息。

在这里插入图片描述

MotionFX_MagCal_getParams

MotionFX_MagCal_getParams函数用于获取磁力计的校准参数。这些参数包括校准后的硬铁偏移量和校准质量指标。该函数通过传入一个指向输出数据结构的指针，返回校准结果的详细信息。

在这里插入图片描述

调用MotionFX_MagCal_getParams函数后，可以通过检查data_out结构体中的参数来评估校准结果的质量，并应用偏移量来调整磁力计数据。
cal_quality：校准质量因子，指示校准结果的准确性。具体值包括：
MFX_MAGCALUNKNOWN = 0：校准参数的准确性未知。
MFX_MAGCALPOOR = 1：校准参数的准确性较差，不能被信任。
MFX_MAGCALOK = 2：校准参数的准确性尚可。
MFX_MAGCALGOOD = 3：校准参数的准确性良好。

校准移植

在main.c中添加变量定义。

/* USER CODE BEGIN 0 */
float MagOffset[3]={0.0f,0.0f,0.0f};//磁力计偏差
int Mag_TimeStamp,Mag_TimeStamp_1,Mag_TimeStamp_2;//磁力计时间戳

uint8_t Mag_flag=0;
/* USER CODE END 0 */

磁力计数据官方文档推荐20/40HZ。

在这里插入图片描述

在mian.c中添加磁力计校准执行函数这里陀螺仪数据为416Hz，单次循环执行10次，所以让磁力计在单次循环中只执行一次，频率则为40Hz左右。
在这里插入图片描述

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
		
	if(fifo_flag)
	{
		for(int i=0;i<fifo_num;i++)// 遍历 FIFO 数据数组
		{	
			
			int16_t gyr;				
			gyr=(gyr_fifo[i][1]<<8) + gyr_fifo[i][0];
			gyr_x =lsm6ds3tr_c_from_fs2000dps_to_mdps(gyr);
			gyr=(gyr_fifo[i][3]<<8) + gyr_fifo[i][2];				
			gyr_y =lsm6ds3tr_c_from_fs2000dps_to_mdps(gyr);
			gyr=(gyr_fifo[i][5]<<8) + gyr_fifo[i][4];				
			gyr_z =lsm6ds3tr_c_from_fs2000dps_to_mdps(gyr);
//			printf(
//							"gyr_x:%4.2f\t%4.2f\t%4.2f\r\n",
//							gyr_x, gyr_y, gyr_z);			
			int16_t acc;
			acc=(acc_fifo[i][1]<<8) + acc_fifo[i][0];
			acc_x =lsm6ds3tr_c_from_fs4g_to_mg(acc);
			acc=(acc_fifo[i][3]<<8) + acc_fifo[i][2];				
			acc_y =lsm6ds3tr_c_from_fs4g_to_mg(acc);
			acc=(acc_fifo[i][5]<<8) + acc_fifo[i][4];				
			acc_z =lsm6ds3tr_c_from_fs4g_to_mg(acc);
			
//			printf(
//							"acc_x:%4.2f\t%4.2f\t%4.2f\r\n",
//							acc_x, acc_y, acc_z);	
							
		 /* 读取时间戳数据 */
			uint32_t timestamp=0;
			timestamp=(timestamp_fifo[i][1]<<16)|(timestamp_fifo[i][0]<<8)
				|(timestamp_fifo[i][3]);

			if(i==0)
				Mag_flag=1;		
			if(deltatime_first==0)//第一次
				{
					deltatime_1=timestamp;
					deltatime_2=deltatime_1;
					deltatime_first=1;
					Mag_TimeStamp_1=timestamp;
					Mag_TimeStamp_2=timestamp;  
				}
				else
				{
					deltatime_2=timestamp;
					Mag_TimeStamp_2=timestamp;
				}
      memset(data_raw_magnetic, 0x00, 3 * sizeof(int16_t));
      lis2mdl_magnetic_raw_get(&lis2mdl_dev_ctx, data_raw_magnetic);
      magnetic_mG[0] = lis2mdl_from_lsb_to_mgauss(data_raw_magnetic[0]);
      magnetic_mG[1] = lis2mdl_from_lsb_to_mgauss(data_raw_magnetic[1]);
      magnetic_mG[2] = lis2mdl_from_lsb_to_mgauss(data_raw_magnetic[2]);
			if(Mag_flag)
			{
				Mag_TimeStamp=Mag_TimeStamp_2-Mag_TimeStamp_1;
				if(Mag_TimeStamp_2>Mag_TimeStamp_1)
					Mag_TimeStamp=(int)((Mag_TimeStamp_2-Mag_TimeStamp_1)*25.0f/1000);
				else if(Mag_TimeStamp_1>Mag_TimeStamp_2)
					Mag_TimeStamp=(int)((0xffffffff-Mag_TimeStamp_2+Mag_TimeStamp_1)*25.0f/1000);
				else if(Mag_TimeStamp_1==Mag_TimeStamp_2)
					Mag_TimeStamp=0;
				Magneto_Sensor_Handler();
				Mag_TimeStamp_1=Mag_TimeStamp_2;
				Mag_flag=0;
			}	
			magnetic_mG[0] = magnetic_mG[0]-MagOffset[0];
			magnetic_mG[1] = magnetic_mG[1]-MagOffset[1];
			magnetic_mG[2] = magnetic_mG[2]-MagOffset[2];	
				
					lsm6ds3tr_c_motion_fx_determin();	
				deltatime_1=deltatime_2;
				
			}
			fifo_flag=0;				
		}
		
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */

在app.h中添加磁力计校准函数定义。

#define FROM_UT50_TO_MGAUSS  500.0f
void Magneto_Sensor_Handler(void);

在app.c中添加磁力计校准函数。

typedef struct
{
  uint8_t hours;
  uint8_t minutes;
  uint8_t seconds;
  uint8_t subsec;
  float pressure;
  float humidity;
  float temperature;
  int32_t acceleration_x_mg;
  int32_t acceleration_y_mg;
  int32_t acceleration_z_mg;
  int32_t angular_rate_x_mdps;
  int32_t angular_rate_y_mdps;
  int32_t angular_rate_z_mdps;
  int32_t magnetic_field_x_mgauss;
  int32_t magnetic_field_y_mgauss;
  int32_t magnetic_field_z_mgauss;
} offline_data_t;

#define OFFLINE_DATA_SIZE  8
uint8_t UseOfflineData = 0;
uint8_t MagCalStatus = 0;
offline_data_t OfflineData[OFFLINE_DATA_SIZE];
extern float MagOffset[3];
extern float magnetic_mG[3];
extern int Mag_TimeStamp;
/**
  * @brief  Handles the MAG axes data getting/sending
  * @param  Msg the MAG part of the stream
  * @retval None
  */
void Magneto_Sensor_Handler(void)
{
  float ans_float;
  MFX_MagCal_input_t mag_data_in;
  MFX_MagCal_output_t mag_data_out;


		mag_data_in.mag[0] = (float)magnetic_mG[0] * FROM_MGAUSS_TO_UT50;
		mag_data_in.mag[1] = (float)magnetic_mG[1] * FROM_MGAUSS_TO_UT50;
		mag_data_in.mag[2] = (float)magnetic_mG[2] * FROM_MGAUSS_TO_UT50;

		mag_data_in.time_stamp = (int)Mag_TimeStamp;
//		Mag_TimeStamp += (uint32_t)ALGO_PERIOD;


		MotionFX_MagCal_run(&mag_data_in);
		MotionFX_MagCal_getParams(&mag_data_out);
		printf("mag_data_out=%d,MFX_MAGCALGOOD=%d\n",mag_data_out.cal_quality,MFX_MAGCALGOOD);
		if (mag_data_out.cal_quality == MFX_MAGCALGOOD)
		{
			
//			MagCalStatus = 1;

			ans_float = (mag_data_out.hi_bias[0] * FROM_UT50_TO_MGAUSS);
			MagOffset[0] = (int32_t)ans_float;
			ans_float = (mag_data_out.hi_bias[1] * FROM_UT50_TO_MGAUSS);
			MagOffset[1] = (int32_t)ans_float;
			ans_float = (mag_data_out.hi_bias[2] * FROM_UT50_TO_MGAUSS);
			MagOffset[2] = (int32_t)ans_float;

//          /* Disable magnetometer calibration */
//          MotionFX_manager_MagCal_stop(ALGO_PERIOD);
		}

}

演示

未校准成功时未0。

在这里插入图片描述

校准成功时为3。

在这里插入图片描述

记帖

关注

12
点赞
踩
10

收藏

觉得还不错? 一键收藏
打赏
0
评论
驱动LSM6DS3TR-C实现高效运动检测与数据采集(11)----磁力计校准

磁力计校准是确保传感器数据准确性和可靠性的关键步骤。磁力计用于测量地球磁场，并在导航、定位、姿态测量等应用中起到重要作用。然而，磁力计在使用过程中会受到环境磁场、硬件偏差、安装误差等因素的影响，从而导致测量数据出现偏差。因此，校准磁力计以消除这些影响，是获得精确测量数据的必要步骤。
复制链接

扫一扫