概述

磁力計(jì)校準(zhǔn)是確保傳感器數(shù)據(jù)準(zhǔn)確性和可靠性的關(guān)鍵步驟。磁力計(jì)用于測(cè)量地球磁場(chǎng)，并在導(dǎo)航、定位、姿態(tài)測(cè)量等應(yīng)用中起到重要作用。然而，磁力計(jì)在使用過(guò)程中會(huì)受到環(huán)境磁場(chǎng)、硬件偏差、安裝誤差等因素的影響，從而導(dǎo)致測(cè)量數(shù)據(jù)出現(xiàn)偏差。因此，校準(zhǔn)磁力計(jì)以消除這些影響，是獲得精確測(cè)量數(shù)據(jù)的必要步驟。

本文將介紹如何使用ST提供的MotionFX庫(kù)在嵌入式系統(tǒng)中實(shí)現(xiàn)磁力計(jì)校準(zhǔn)， 通過(guò)本文的介紹，讀者將能夠理解磁力計(jì)校準(zhǔn)的基本概念，掌握使用MotionFX庫(kù)進(jìn)行校準(zhǔn)的步驟和方法，并學(xué)會(huì)如何在實(shí)際項(xiàng)目中實(shí)現(xiàn)磁力計(jì)的校準(zhǔn)，以獲得高精度的磁場(chǎng)測(cè)量數(shù)據(jù)。

需要樣片的可以加群申請(qǐng)：615061293 。

視頻教學(xué)

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

樣品申請(qǐng)

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

源碼下載

[https://download.csdn.net/download/qq_24312945/89614587]

硬件準(zhǔn)備

首先需要準(zhǔn)備一個(gè)開(kāi)發(fā)板，這里我準(zhǔn)備的是自己繪制的開(kāi)發(fā)板，需要的可以進(jìn)行申請(qǐng)。
主控為STM32H503CB，陀螺儀為L(zhǎng)SM6DS3TR-C，磁力計(jì)為L(zhǎng)IS2MDL。

DataLogFusion

這里參考ST提供的DataLogFusion程序，DataLogFusion示例應(yīng)用展示了如何使用STMicroelectronics開(kāi)發(fā)的MotionFX中間件庫(kù)進(jìn)行實(shí)時(shí)運(yùn)動(dòng)傳感器數(shù)據(jù)融合。

DataLogFusion的主要執(zhí)行流程包括初始化硬件和傳感器、中間件庫(kù)（MotionFX）的配置與初始化、傳感器數(shù)據(jù)的采集、實(shí)時(shí)數(shù)據(jù)融合以及結(jié)果的輸出。

磁力計(jì)校準(zhǔn)過(guò)程

MotionFX庫(kù)的磁力計(jì)校準(zhǔn)庫(kù)用于補(bǔ)償硬鐵失真。磁力計(jì)校準(zhǔn)可以以比傳感器融合輸出數(shù)據(jù)速率更慢的頻率進(jìn)行（例如25 Hz）。
● 初始化磁力計(jì)校準(zhǔn)庫(kù)：
● 調(diào)用 MotionFX_MagCal_init 或 MotionFX_CM0P_MagCal_init 函數(shù)。
● 定期調(diào)用校準(zhǔn)函數(shù)：
● 調(diào)用 MotionFX_MagCal_run 或 MotionFX_CM0P_MagCal_run 函數(shù)，直到校準(zhǔn)成功完成。
● 檢查校準(zhǔn)是否成功：
● 調(diào)用 MotionFX_MagCal_getParams 或 MotionFX_CM0P_MagCal_getParams 函數(shù)。如果函數(shù)返回 mag_data_out.cal_quality = MFX_MAGCALGOOD 或 MFX_CM0P_CALQSTATUSBEST，則校準(zhǔn)成功。

在app_mems.c中的Magneto_Sensor_Handler函數(shù)負(fù)責(zé)處理磁力計(jì)（磁傳感器）數(shù)據(jù)的獲取、校準(zhǔn)和發(fā)送。該函數(shù)的主要功能是從磁力計(jì)傳感器獲取數(shù)據(jù)，進(jìn)行必要的校準(zhǔn)，然后將處理后的數(shù)據(jù)傳輸給其他部分使用。具體包括以下步驟：

1. 檢查傳感器是否啟用：函數(shù)首先檢查是否啟用了磁力計(jì)傳感器。如果傳感器未啟用，則函數(shù)不會(huì)執(zhí)行進(jìn)一步操作。
2. 獲取磁力計(jì)數(shù)據(jù)：
   ○ 如果使用離線數(shù)據(jù)（即在調(diào)試或仿真模式下），函數(shù)會(huì)從預(yù)定義的離線數(shù)據(jù)數(shù)組中讀取磁力計(jì)數(shù)據(jù)。
   ○ 如果使用實(shí)時(shí)數(shù)據(jù)，函數(shù)會(huì)通過(guò)調(diào)用BSP_SENSOR_MAG_GetAxes函數(shù)從實(shí)際的磁力計(jì)傳感器中讀取當(dāng)前的磁力計(jì)數(shù)據(jù)。
3. 執(zhí)行磁力計(jì)校準(zhǔn)：
   ○ 函數(shù)檢查磁力計(jì)是否已經(jīng)校準(zhǔn)。如果尚未校準(zhǔn)，則會(huì)進(jìn)行校準(zhǔn)過(guò)程。
   ○ 將獲取的磁力計(jì)數(shù)據(jù)轉(zhuǎn)換為適當(dāng)?shù)膯挝?，并?zhǔn)備校準(zhǔn)輸入數(shù)據(jù)。
   ○ 調(diào)用MotionFX_manager_MagCal_run函數(shù)運(yùn)行校準(zhǔn)算法。
   ○ 如果校準(zhǔn)結(jié)果良好（校準(zhǔn)質(zhì)量達(dá)到標(biāo)準(zhǔn)），函數(shù)會(huì)保存校準(zhǔn)偏移值，并停止進(jìn)一步的校準(zhǔn)。
4. 應(yīng)用校準(zhǔn)偏移：將校準(zhǔn)偏移應(yīng)用到磁力計(jì)讀數(shù)中，以補(bǔ)償硬件偏差和環(huán)境干擾。
5. 序列化并發(fā)送數(shù)據(jù)：將校準(zhǔn)后的磁力計(jì)數(shù)據(jù)序列化，并存儲(chǔ)到消息結(jié)構(gòu)（Msg）中，以供系統(tǒng)其他部分使用。

MotionFX_manager_MagCal_run函數(shù)用于運(yùn)行磁力計(jì)校準(zhǔn)算法。該函數(shù)接受磁力計(jì)輸入數(shù)據(jù)，調(diào)用MotionFX庫(kù)中的校準(zhǔn)算法對(duì)數(shù)據(jù)進(jìn)行處理，并返回校準(zhǔn)結(jié)果。
MotionFX_MagCal_run函數(shù)執(zhí)行校準(zhǔn)算法，對(duì)輸入數(shù)據(jù)進(jìn)行處理，計(jì)算出校準(zhǔn)所需的參數(shù)。
MotionFX_MagCal_getParams函數(shù)從校準(zhǔn)算法中獲取校準(zhǔn)后的參數(shù)，并存儲(chǔ)在輸出數(shù)據(jù)結(jié)構(gòu)data_out中。這些參數(shù)包括磁力計(jì)的偏移和標(biāo)度因子等校準(zhǔn)信息。

MotionFX_MagCal_getParams

MotionFX_MagCal_getParams函數(shù)用于獲取磁力計(jì)的校準(zhǔn)參數(shù)。這些參數(shù)包括校準(zhǔn)后的硬鐵偏移量和校準(zhǔn)質(zhì)量指標(biāo)。該函數(shù)通過(guò)傳入一個(gè)指向輸出數(shù)據(jù)結(jié)構(gòu)的指針，返回校準(zhǔn)結(jié)果的詳細(xì)信息。

調(diào)用MotionFX_MagCal_getParams函數(shù)后，可以通過(guò)檢查data_out結(jié)構(gòu)體中的參數(shù)來(lái)評(píng)估校準(zhǔn)結(jié)果的質(zhì)量，并應(yīng)用偏移量來(lái)調(diào)整磁力計(jì)數(shù)據(jù)。
cal_quality：校準(zhǔn)質(zhì)量因子，指示校準(zhǔn)結(jié)果的準(zhǔn)確性。具體值包括：
MFX_MAGCALUNKNOWN = 0：校準(zhǔn)參數(shù)的準(zhǔn)確性未知。
MFX_MAGCALPOOR = 1：校準(zhǔn)參數(shù)的準(zhǔn)確性較差，不能被信任。
MFX_MAGCALOK = 2：校準(zhǔn)參數(shù)的準(zhǔn)確性尚可。
MFX_MAGCALGOOD = 3：校準(zhǔn)參數(shù)的準(zhǔn)確性良好。

校準(zhǔn)移植

在main.c中添加變量定義。

/* USER CODE BEGIN 0 */
float MagOffset[3]={0.0f,0.0f,0.0f};//磁力計(jì)偏差
int Mag_TimeStamp,Mag_TimeStamp_1,Mag_TimeStamp_2;//磁力計(jì)時(shí)間戳

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

磁力計(jì)數(shù)據(jù)官方文檔推薦20/40HZ。

在mian.c中添加磁力計(jì)校準(zhǔn)執(zhí)行函數(shù)這里陀螺儀數(shù)據(jù)為416Hz，單次循環(huán)執(zhí)行10次，所以讓磁力計(jì)在單次循環(huán)中只執(zhí)行一次，頻率則為40Hz左右。

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

    if(fifo_flag)
    {
        for(int i=0;i< fifo_num;i++)// 遍歷 FIFO 數(shù)據(jù)數(shù)組
        {    

            int16_t gyr;                
            gyr=(gyr_fifo[i][1]< 
            gyr_x =lsm6ds3tr_c_from_fs2000dps_to_mdps(gyr);
            gyr=(gyr_fifo[i][3]< 
            gyr_y =lsm6ds3tr_c_from_fs2000dps_to_mdps(gyr);
            gyr=(gyr_fifo[i][5]< 
            gyr_z =lsm6ds3tr_c_from_fs2000dps_to_mdps(gyr);
//            printf(
//                            "gyr_x:%4.2ft%4.2ft%4.2frn",
//                            gyr_x, gyr_y, gyr_z);            
            int16_t acc;
            acc=(acc_fifo[i][1]< 
            acc_x =lsm6ds3tr_c_from_fs4g_to_mg(acc);
            acc=(acc_fifo[i][3]< 
            acc_y =lsm6ds3tr_c_from_fs4g_to_mg(acc);
            acc=(acc_fifo[i][5]< 
            acc_z =lsm6ds3tr_c_from_fs4g_to_mg(acc);

//            printf(
//                            "acc_x:%4.2ft%4.2ft%4.2frn",
//                            acc_x, acc_y, acc_z);    

         /* 讀取時(shí)間戳數(shù)據(jù) */
            uint32_t timestamp=0;
            timestamp=(timestamp_fifo[i][1]< 
                |(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中添加磁力計(jì)校準(zhǔn)函數(shù)定義。

#define FROM_UT50_TO_MGAUSS  500.0f
void Magneto_Sensor_Handler(void);

在app.c中添加磁力計(jì)校準(zhǔn)函數(shù)。

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);
        }

}

演示

未校準(zhǔn)成功時(shí)未0。

校準(zhǔn)成功時(shí)為3。

審核編輯 黃宇