陀螺仪LSM6DSOW开发(8)-

2024-08-20

概述

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

本文将介绍如何使用ST提供的MotionFX库在嵌入式系统中实现磁力计校准, 通过本文的介绍,读者将能够理解磁力计校准的基本概念,掌握使用MotionFX库进行校准的步骤和方法,并学会如何在实际项目中实现磁力计的校准,以获得高精度的磁场测量数据。

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

陀螺仪LSM6DSOW开发(8)- (https://ic.work/) 技术资料 第1张

视频教学

[https://www.bilibili.com/video/BV1Mi421a7QF/]

样品申请

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

源码下载

[https://download.csdn.net/download/qq_24312945/89614581](

硬件准备

首先需要准备一个开发板,这里我准备的是自己绘制的开发板,需要的可以进行申请。

主控为STM32H503CB,陀螺仪为LSM6DSOW,磁力计为LIS2MDL。

陀螺仪LSM6DSOW开发(8)- (https://ic.work/) 技术资料 第2张

DataLogFusion

这里参考ST提供的DataLogFusion程序,DataLogFusion示例应用展示了如何使用STMicroelectronics开发的MotionFX中间件库进行实时运动传感器数据融合。

DataLogFusion的主要执行流程包括初始化硬件和传感器、中间件库(MotionFX)的配置与初始化、传感器数据的采集、实时数据融合以及结果的输出。

陀螺仪LSM6DSOW开发(8)- (https://ic.work/) 技术资料 第3张

磁力计校准过程

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

陀螺仪LSM6DSOW开发(8)- (https://ic.work/) 技术资料 第4张

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

  1. 检查传感器是否启用:函数首先检查是否启用了磁力计传感器。如果传感器未启用,则函数不会执行进一步操作。
  2. 获取磁力计数据:

    ○ 如果使用离线数据(即在调试或仿真模式下),函数会从预定义的离线数据数组中读取磁力计数据。

    ○ 如果使用实时数据,函数会通过调用BSP_SENSOR_MAG_GetAxes函数从实际的磁力计传感器中读取当前的磁力计数据。
  3. 执行磁力计校准:

    ○ 函数检查磁力计是否已经校准。如果尚未校准,则会进行校准过程。

    ○ 将获取的磁力计数据转换为适当的单位,并准备校准输入数据。

    ○ 调用MotionFX_manager_MagCal_run函数运行校准算法。

    ○ 如果校准结果良好(校准质量达到标准),函数会保存校准偏移值,并停止进一步的校准。
  4. 应用校准偏移:将校准偏移应用到磁力计读数中,以补偿硬件偏差和环境干扰。
  5. 序列化并发送数据:将校准后的磁力计数据序列化,并存储到消息结构(Msg)中,以供系统其他部分使用。

陀螺仪LSM6DSOW开发(8)- (https://ic.work/) 技术资料 第5张

MotionFX_manager_MagCal_run函数用于运行磁力计校准算法。该函数接受磁力计输入数据,调用MotionFX库中的校准算法对数据进行处理,并返回校准结果。

MotionFX_MagCal_run函数执行校准算法,对输入数据进行处理,计算出校准所需的参数。

MotionFX_MagCal_getParams函数从校准算法中获取校准后的参数,并存储在输出数据结构data_out中。这些参数包括磁力计的偏移和标度因子等校准信息。

陀螺仪LSM6DSOW开发(8)- (https://ic.work/) 技术资料 第6张

MotionFX_MagCal_getParams

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

陀螺仪LSM6DSOW开发(8)- (https://ic.work/) 技术资料 第7张

调用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。

陀螺仪LSM6DSOW开发(8)- (https://ic.work/) 技术资料 第8张

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

陀螺仪LSM6DSOW开发(8)- (https://ic.work/) 技术资料 第9张

/* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {

    if(fifo_flag)// 如果 FIFO 中断标志被设置
    {        
        uint8_t acc_flag=0 
gyr_flag=0;//加速度角速度标志位 uint8_t deltatime_flag=0;//时间标志位 // printf("fifo_num=%dn",fifo_num); for(int i=0;i< fifo_num;i++)// 遍历 FIFO 数据数组 { // 获取数据指针 datax = (int16_t *)&fifo_data[i][1]; datay = (int16_t *)&fifo_data[i][3]; dataz = (int16_t *)&fifo_data[i][5]; // 根据数据标签处理不同类型的数据 switch (fifo_data[i][0]) { case LSM6DSO_XL_NC_TAG:// 加速度数据 acc_flag=1; acc_x=lsm6dso_from_fs2_to_mg(*datax); acc_y=lsm6dso_from_fs2_to_mg(*datay); acc_z=lsm6dso_from_fs2_to_mg(*dataz); // printf("Acceleration [mg]:%4.2ft%4.2ft%4.2frn", // acc_x, acc_y, acc_z); break; case LSM6DSO_GYRO_NC_TAG:// 角速度数据 gyr_flag=1; gyr_x=lsm6dso_from_fs2000_to_mdps(*datax); gyr_y=lsm6dso_from_fs2000_to_mdps(*datay); gyr_z=lsm6dso_from_fs2000_to_mdps(*dataz); // printf("Angular rate [mdps]:%4.2ft%4.2ft%4.2frn", // gyr_x,gyr_y,gyr_z); break; case LSM6DSO_TIMESTAMP_TAG:// 时间戳数据 deltatime_flag=1; /* 读取时间戳数据 */ uint32_t timestamp=0; timestamp+= fifo_data[i][1]; timestamp+= fifo_data[i][2]< < 8; timestamp+= fifo_data[i][3]< < 16; timestamp+= fifo_data[i][4]< < 24; 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; } // printf("timestamp=%drn",timestamp); break; default: break; } if(i==0) Mag_flag=1; // 如果加速度、角速度和时间戳数据都已获取 if(acc_flag&&gyr_flag&&deltatime_flag) { 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; } magnetic_mG[0] = magnetic_mG[0]-MagOffset[0]; magnetic_mG[1] = magnetic_mG[1]-MagOffset[1]; magnetic_mG[2] = magnetic_mG[2]-MagOffset[2]; lsm6dso_motion_fx_determin();// 调用 MotionFX 处理函数 acc_flag=0; gyr_flag=0; deltatime_flag=0; deltatime_1=deltatime_2; // 更新时间戳 Mag_flag=0; } } // 清除 FIFO 标志和数据量 fifo_flag=0; fifo_num=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=%dn" 
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。

陀螺仪LSM6DSOW开发(8)- (https://ic.work/) 技术资料 第10张

校准成功时为3。

陀螺仪LSM6DSOW开发(8)- (https://ic.work/) 技术资料 第11张

审核编辑 黄宇

文章推荐

相关推荐