概述

磁力計(jì)測量結(jié)果容易受到周圍環(huán)境中的硬鐵（Hard Iron）和軟鐵（Soft Iron）效應(yīng)的干擾，從而影響精度。為了解決這一問題，磁力計(jì)校準(zhǔn)變得至關(guān)重要。STMicroelectronics提供的MotionMC庫是一個(gè)高效的中間件解決方案，專門用于實(shí)時(shí)校準(zhǔn)磁力計(jì)數(shù)據(jù)，以消除這些誤差。
MotionMC庫能夠通過測量不同方向的磁場數(shù)據(jù)，自動(dòng)計(jì)算并補(bǔ)償硬鐵和比例因子效應(yīng)。它集成了在嵌入式系統(tǒng)中運(yùn)行的輕量級算法，能夠在系統(tǒng)運(yùn)行期間進(jìn)行動(dòng)態(tài)校準(zhǔn)，確保磁力計(jì)的輸出數(shù)據(jù)始終準(zhǔn)確可靠。
在本文中，將介紹如何使用LIS2MDL磁力計(jì)與MotionMC庫執(zhí)行磁力計(jì)校準(zhǔn)。我們將探討從傳感器初始化、數(shù)據(jù)采集到最終應(yīng)用校準(zhǔn)參數(shù)的整個(gè)流程，并提供相應(yīng)的代碼示例，以幫助開發(fā)者更好地集成和利用MotionMC庫提升系統(tǒng)的磁力測量精度。

需要樣片的可以加群申請：615061293 。

視頻教學(xué)

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

樣品申請

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

源碼下載

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

硬件準(zhǔn)備

首先需要準(zhǔn)備一個(gè)開發(fā)板，這里我準(zhǔn)備的是自己繪制的開發(fā)板，需要的可以進(jìn)行申請。

主控為STM32H503CB，陀螺儀為LSM6DS3TR-C，磁力計(jì)為LIS2MDL。

開啟CRC

串口設(shè)置

設(shè)置串口速率為2000000。

開啟X-CUBE-MEMS1

速率選擇

磁力計(jì)數(shù)據(jù)最大可以設(shè)置100Hz。

參考程序

這里參考 IKS01A3_MagnetometerCalibration 。

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

MotionMC 是一個(gè)用于校準(zhǔn)磁力計(jì)傳感器的庫。校準(zhǔn)過程旨在消除硬鐵效應(yīng)（由于設(shè)備內(nèi)部或附近的磁性材料引起的誤差）和軟鐵效應(yīng)（由于設(shè)備內(nèi)部或附近的導(dǎo)電材料引起的誤差），從而提高磁力計(jì)的精度。

建議在三維空間中緩慢旋轉(zhuǎn)。這個(gè)動(dòng)作的重點(diǎn)在于它不是一個(gè)簡單的平面運(yùn)動(dòng)，而是需要在不同的空間角度進(jìn)行傾斜和旋轉(zhuǎn)，以覆蓋盡可能多的三維空間位置。

初始化定義

/* USER CODE BEGIN 2 */
    printf("HELLO!n");
  HAL_GPIO_WritePin(CS1_GPIO_Port, CS1_Pin, GPIO_PIN_SET);
  HAL_GPIO_WritePin(CS2_GPIO_Port, CS2_Pin, GPIO_PIN_SET);
    HAL_Delay(100);


  /* Initialize mems driver interface */
  stmdev_ctx_t dev_ctx;
  dev_ctx.write_reg = platform_write;
  dev_ctx.read_reg = platform_read;
  dev_ctx.mdelay = platform_delay;
  dev_ctx.handle = &SENSOR_BUS;
  /* Initialize platform specific hardware */
//  platform_init();
  /* Wait sensor boot time */
  platform_delay(BOOT_TIME);

  /* Check device ID */
  lis2mdl_device_id_get(&dev_ctx, &whoamI);
    printf("LIS2MDL_ID=0x%x,whoamI=0x%xn",LIS2MDL_ID,whoamI);
  if (whoamI != LIS2MDL_ID)
    while (1) {
      /* manage here device not found */
    }

  /* Restore default configuration */
  lis2mdl_reset_set(&dev_ctx, PROPERTY_ENABLE);

  do {
    lis2mdl_reset_get(&dev_ctx, &rst);
  } while (rst);

  /* Enable Block Data Update */
  lis2mdl_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
  /* Set Output Data Rate */
  lis2mdl_data_rate_set(&dev_ctx, LIS2MDL_ODR_50Hz);
  /* Set / Reset sensor mode */
  lis2mdl_set_rst_mode_set(&dev_ctx, LIS2MDL_SENS_OFF_CANC_EVERY_ODR);
  /* Enable temperature compensation */
  lis2mdl_offset_temp_comp_set(&dev_ctx, PROPERTY_ENABLE);
  /* Set device in continuous mode */
  lis2mdl_operating_mode_set(&dev_ctx, LIS2MDL_CONTINUOUS_MODE);    


    MX_MEMS_Init();

  /* USER CODE END 2 */

MotionMC文件

主要包含app_mems.c和app_mems.h， 它們提供了一些與MEMS傳感器相關(guān)的初始化、處理和管理函數(shù)。這些文件在磁力計(jì)校準(zhǔn)、數(shù)據(jù)處理以及傳感器初始化等方面發(fā)揮著重要作用。
● MX_MEMS_Init(void) 和 MX_MEMS_Process(void)：這些函數(shù)用于初始化和處理MEMS傳感器的操作。
● MotionMC_manager_init(int sampletime, unsigned short int enable)：初始化MotionMC庫。
● MotionMC_manager_update(MMC_Input_t *data_in)：使用新的傳感器數(shù)據(jù)更新MotionMC庫。
● MotionMC_manager_get_params(MMC_Output_t *data_out)：獲取校準(zhǔn)后的參數(shù)。
● MotionMC_manager_compensate(MOTION_SENSOR_Axes_t *data_raw, MOTION_SENSOR_Axes_t *data_comp)：對傳感器數(shù)據(jù)應(yīng)用校準(zhǔn)補(bǔ)償。

MotionMC_Initialize

MotionMC_manager_init中主要執(zhí)行MotionMC_Initialize，MotionMC_Initialize用途主要初始化MotionMC庫并設(shè)置內(nèi)部機(jī)制。這個(gè)函數(shù)在使用磁力計(jì)校準(zhǔn)庫之前必須調(diào)用。
參數(shù):
sampletime: 設(shè)置更新函數(shù)調(diào)用之間的時(shí)間間隔（以毫秒為單位）。
enable: 啟用（1）或禁用（0）該庫。

MotionMC_manager_get_version

MotionMC_manager_get_version中主要執(zhí)行MotionMC_GetLibVersion，MotionMC_GetLibVersion用途主要是檢索庫的版本信息。
參數(shù):
version: 一個(gè)指向字符數(shù)組的指針，用于存儲版本字符串。
返回值: 版本字符串中的字符數(shù)量。

MotionMC_manager_update

MotionMC_manager_update中主要執(zhí)行MotionMC_Update，MotionMC_Update用途主要是運(yùn)行磁力計(jì)校準(zhǔn)算法。這個(gè)函數(shù)需要周期性地調(diào)用，其周期與初始化函數(shù)中設(shè)置的sampletime參數(shù)相同。
參數(shù):
data_in: 指向包含輸入數(shù)據(jù)的結(jié)構(gòu)體的指針。這個(gè)結(jié)構(gòu)體包含了當(dāng)前的磁力計(jì)傳感器數(shù)據(jù)和時(shí)間戳。

MotionMC_manager_get_params

MotionMC_manager_get_params中主要執(zhí)行MotionMC_GetCalParams，MotionMC_GetCalParams用途主要是獲取磁力計(jì)的硬鐵（HI）和比例因子（SF）校準(zhǔn)系數(shù)。
參數(shù):
data_out: 指向包含輸出數(shù)據(jù)的結(jié)構(gòu)體的指針。這個(gè)結(jié)構(gòu)體包括了HI偏置、SF矩陣和校準(zhǔn)質(zhì)量因子。

CalQuality = 0：校準(zhǔn)參數(shù)的準(zhǔn)確性未知。
CalQuality = 1：校準(zhǔn)參數(shù)的準(zhǔn)確性較差，不能被信任。
CalQuality = 2：校準(zhǔn)參數(shù)的準(zhǔn)確性尚可。
CalQuality = 3：校準(zhǔn)參數(shù)的準(zhǔn)確性良好。

MotionMC_manager_compensate

MotionMC_manager_compensate 的主要作用是對磁力計(jì)數(shù)據(jù)進(jìn)行硬鐵（Hard Iron）和軟鐵（Soft Iron）校準(zhǔn)，從而補(bǔ)償磁力計(jì)測量中的誤差。

/**
 * @brief  Do hard & soft iron calibration
 * @param  data_raw  Raw magnetometer data [mGauss]
 * @param  data_comp  Calibrated (compensated) data (hard & soft iron calibration) [mGauss]
 * @retval None
 */
void MotionMC_manager_compensate(MOTION_SENSOR_Axes_t *data_raw, MOTION_SENSOR_Axes_t *data_comp)
{
  MMC_Output_t data_out;
  MotionMC_GetCalParams(&data_out);

  float mag_raw_mG[3];
  float mag_comp_mG[3];

  mag_raw_mG[0] = (float)data_raw- >x;
  mag_raw_mG[1] = (float)data_raw- >y;
  mag_raw_mG[2] = (float)data_raw- >z;

  /* Compensate magnetometer data */
  /* NOTE: Convert hard iron coefficients [uT] to [mGauss] */
  for (int i = 0; i < 3; i++)
  {
    mag_comp_mG[i] = 0.0f;
    for (int j = 0; j < 3; j++)
    {
      mag_comp_mG[i] += (mag_raw_mG[j]  -  data_out.HI_Bias[j] * 10.0f)  *  data_out.SF_Matrix[i][j];
    }

    mag_comp_mG[i] += (mag_comp_mG[i] >= 0.0f) ? 0.5f : -0.5f;
  }

  data_comp- >x = (int32_t)mag_comp_mG[0];
  data_comp- >y = (int32_t)mag_comp_mG[1];
  data_comp- >z = (int32_t)mag_comp_mG[2];
}

● 硬鐵效應(yīng)：由設(shè)備內(nèi)部或附近的永久磁鐵或磁性材料引起的偏移，會導(dǎo)致測量結(jié)果出現(xiàn)恒定的誤差。
● 軟鐵效應(yīng)：由設(shè)備內(nèi)部或附近的導(dǎo)磁材料（如鐵）引起的誤差，會影響測量結(jié)果的方向和幅度。
● 補(bǔ)償計(jì)算:
● 對每個(gè)軸（X, Y, Z）進(jìn)行硬鐵和軟鐵效應(yīng)的補(bǔ)償計(jì)算。計(jì)算過程中：
○ 硬鐵效應(yīng)補(bǔ)償：從原始數(shù)據(jù)中減去硬鐵偏置，單位從微特斯拉（uT）轉(zhuǎn)換為毫高斯（mGauss），即乘以10。
○ 軟鐵效應(yīng)補(bǔ)償：通過乘以校正矩陣 SF_Matrix，對軟鐵效應(yīng)進(jìn)行補(bǔ)償。
● 結(jié)果修正和輸出:
● 補(bǔ)償后的磁力計(jì)數(shù)據(jù) mag_comp_mG 進(jìn)行四舍五入，然后轉(zhuǎn)換為整數(shù)并存儲到 data_comp 中。

主程序執(zhí)行流程

/* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    uint8_t reg;
    /* Read output only if new value is available */
    lis2mdl_mag_data_ready_get(&dev_ctx, ®);

    if (reg) {
      /* Read magnetic field data */
      memset(data_raw_magnetic, 0x00, 3 * sizeof(int16_t));
      lis2mdl_magnetic_raw_get(&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]);
            MX_MEMS_Process();


//      printf("Magnetic field [mG]:%4.2ft%4.2ft%4.2frn",
//              magnetic_mG[0], magnetic_mG[1], magnetic_mG[2]);
//      /* Read temperature data */
//      memset(&data_raw_temperature, 0x00, sizeof(int16_t));
//      lis2mdl_temperature_raw_get(&dev_ctx, &data_raw_temperature);
//      temperature_degC = lis2mdl_from_lsb_to_celsius(data_raw_temperature);
//      printf("Temperature [degC]:%6.2frn",
//              temperature_degC);
    }        


    /* USER CODE END WHILE */

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

演示

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

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

指向北數(shù)據(jù)。

指向南數(shù)據(jù)。

審核編輯 黃宇