概述

在現(xiàn)代電子設(shè)備中，陀螺儀傳感器被廣泛用于測量設(shè)備的旋轉(zhuǎn)運動。然而，由于各種環(huán)境因素和制造工藝的影響，陀螺儀通常會存在零速率偏移（Zero-rate Offset），這會導致測量數(shù)據(jù)的不準確。為了提高測量的精度，需要對陀螺儀進行校準。
MotionGC庫是STMicroelectronics提供的一個專門用于陀螺儀校準的中間件庫。該庫作為X-CUBE-MEMS1軟件擴展包的一部分，運行在STM32微控制器上。它通過計算和補償陀螺儀的角速度偏移，確保陀螺儀輸出的角速度數(shù)據(jù)更加準確和可靠。
MotionGC庫提供了多種API，允許用戶在不同的采樣頻率下執(zhí)行實時校準。庫的核心功能包括檢測設(shè)備的靜止狀態(tài)、計算陀螺儀的偏移量，并應(yīng)用這些偏移量來校正傳感器輸出的數(shù)據(jù)。該庫還允許用戶根據(jù)特定應(yīng)用需求調(diào)整校準參數(shù)，從而優(yōu)化校準效果。

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

視頻教學

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

樣品申請

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

源碼下載

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

硬件準備

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

主控為STM32H503CB，陀螺儀為LSM6DSOW，磁力計為LIS2MDL。

開啟CRC

串口設(shè)置

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

開啟X-CUBE-MEMS1

速率選擇

陀螺儀數(shù)據(jù)可以設(shè)置25Hz-200Hz。

參考程序

這里參考 IKS01A3_MagnetometerCalibration 。

MX_MEMS_Init

MX_MEMS_Init() 函數(shù)是用于初始化MEMS（微機電系統(tǒng)）傳感器組件的，特別是在這個上下文中，主要是針對陀螺儀的校準過程。下面是該函數(shù)在初始化過程中所執(zhí)行的詳細步驟：
調(diào)用 MX_GyroscopeCalibration_Init() 進行陀螺儀校準初始化
這是整個初始化過程的核心部分。該函數(shù)負責為陀螺儀校準準備必要的配置和設(shè)置，包括初始化MotionGC庫。
MotionGC庫初始化
在MX_GyroscopeCalibration_Init()中，首先調(diào)用了MotionGC_manager_init()函數(shù)，這個函數(shù)進一步調(diào)用了MotionGC_Initialize()函數(shù)，用于初始化MotionGC校準引擎。該引擎專門用于處理陀螺儀的偏置校準。
MotionGC_Initialize()接受兩個參數(shù)：
mcu_type：指定所使用的MCU類型，在這里通常是MGC_MCU_STM32。
freq：設(shè)置采樣頻率，以確保校準算法按照正確的時間間隔運行。

獲取并打印庫版本信息

MotionGC_manager_get_version() 函數(shù)用于獲取MotionGC庫的版本信息，并將版本號打印出來。這在調(diào)試和版本控制時非常有用。 MotionGC_GetLibVersion() 會返回版本號的字符串長度，并將版本號保存到一個字符數(shù)組中。

獲取并設(shè)置校準參數(shù)

MotionGC_GetKnobs() 函數(shù)用于獲取當前的校準“旋鈕”（Knobs）設(shè)置，這些設(shè)置包括了加速度計和陀螺儀的閾值、過濾常數(shù)等。 然后通過MotionGC_SetKnobs() 函數(shù)對這些參數(shù)進行調(diào)整。例如，設(shè)置加速度計的靜止狀態(tài)檢測閾值為0.008g，陀螺儀的閾值為0.15dps。這些設(shè)置可以根據(jù)具體應(yīng)用場景進行優(yōu)化。

設(shè)置初始陀螺儀偏置

為了加快校準過程，可以通過MotionGC_manager_set_params() 設(shè)置初始的陀螺儀偏置值，主要執(zhí)行MotionGC_SetCalParams。默認情況下，偏置值通常被設(shè)置為0.0f，表示沒有初始偏置。

設(shè)置采樣頻率

通過MotionGC_manager_set_frequency() 函數(shù)設(shè)置采樣頻率，主要執(zhí)行MotionGC_SetFrequency。采樣頻率是根據(jù)報告間隔計算得出的，確保陀螺儀校準算法在正確的頻率下運行。

初始化定義

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


  stmdev_ctx_t dev_ctx;
  /* Initialize mems driver interface */
  dev_ctx.write_reg = platform_write;
  dev_ctx.read_reg = platform_read;
  dev_ctx.mdelay = platform_delay;
  dev_ctx.handle = &SENSOR_BUS;
  /* Init test platform */
//  platform_init();
  /* Wait sensor boot time */
  platform_delay(BOOT_TIME);
  /* Check device ID */
  lsm6dso_device_id_get(&dev_ctx, &whoamI);
    printf("LSM6DSO_ID=0x%x,whoamI=0x%x",LSM6DSO_ID,whoamI);
  if (whoamI != LSM6DSO_ID)
    while (1);

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

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

  /* Disable I3C interface */
  lsm6dso_i3c_disable_set(&dev_ctx, LSM6DSO_I3C_DISABLE);
  /* Enable Block Data Update */
  lsm6dso_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
  /* Set Output Data Rate */
  lsm6dso_xl_data_rate_set(&dev_ctx, LSM6DSO_XL_ODR_52Hz);
  lsm6dso_gy_data_rate_set(&dev_ctx, LSM6DSO_GY_ODR_52Hz);
  /* Set full scale */
  lsm6dso_xl_full_scale_set(&dev_ctx, LSM6DSO_2g);
  lsm6dso_gy_full_scale_set(&dev_ctx, LSM6DSO_2000dps);
  /* Configure filtering chain(No aux interface)
   * Accelerometer - LPF1 + LPF2 path
   */
//  lsm6dso_xl_hp_path_on_out_set(&dev_ctx, LSM6DSO_LP_ODR_DIV_100);
//  lsm6dso_xl_filter_lp2_set(&dev_ctx, PROPERTY_ENABLE);

    MX_MEMS_Init();

    uint8_t acc_flag=0,gy_flag=0;
  /* USER CODE END 2 */

MX_MEMS_Process

MX_MEMS_Process() 函數(shù)在MEMS傳感器應(yīng)用中主要負責處理傳感器數(shù)據(jù)并執(zhí)行與校準相關(guān)的操作。在這個上下文中，MX_MEMS_Process() 函數(shù)具體執(zhí)行以下任務(wù)：
MX_MEMS_Process() 主要調(diào)用了 MX_GyroscopeCalibration_Process() 函數(shù)。這是一個私有函數(shù)，專門用于處理陀螺儀校準數(shù)據(jù)的流和處理。
在 MX_GyroscopeCalibration_Process() 函數(shù)中，主要執(zhí)行了 GC_Data_Handler()，它負責從傳感器中讀取數(shù)據(jù)，運行校準算法，并應(yīng)用校準結(jié)果。

MotionGC_manager_update

GC_Data_Handler() 函數(shù)從加速度計和陀螺儀讀取當前的傳感器數(shù)據(jù)，具體包括：
● 將加速度計數(shù)據(jù)從毫重力單位（mg）轉(zhuǎn)換為重力單位（g）。
● 將陀螺儀數(shù)據(jù)從毫度每秒（mdps）轉(zhuǎn)換為度每秒（dps）。
然后調(diào)用 MotionGC_manager_update() 函數(shù)來運行陀螺儀校準算法，更新偏置值，并通過 MotionGC_manager_compensate() 函數(shù)對原始數(shù)據(jù)進行補償，得到校準后的角速度數(shù)據(jù)，MotionGC_Update()。

● 當調(diào)用 MotionGC_Update() 函數(shù)后，如果陀螺儀的偏置（gyro bias）被更新了，那么 bias_update 所指向的整數(shù)值會被設(shè)置為 1，表示偏置已更新。

● 如果陀螺儀的偏置沒有變化，則 bias_update 所指向的整數(shù)值將保持為 0，表示偏置沒有更新。

這個參數(shù)的作用是讓調(diào)用者知道在當前的調(diào)用中，校準算法是否對陀螺儀的偏置進行了調(diào)整。這對于動態(tài)監(jiān)控校準過程和判斷當前校準狀態(tài)是否穩(wěn)定非常有用。

MotionGC_manager_compensate

MotionGC_manager_compensate() 函數(shù)對原始數(shù)據(jù)進行補償，得到校準后的角速度數(shù)據(jù)，主要執(zhí)行MotionGC_GetCalParams。

● 這個函數(shù)用于獲取當前陀螺儀的補償參數(shù)，也就是當前的陀螺儀角速度零點偏置（angular zero-rate level）。

● 參數(shù) *gyro_bias 是一個指向結(jié)構(gòu)體的指針，這個結(jié)構(gòu)體包含了陀螺儀在X、Y、Z軸上的零速率偏置值。調(diào)用這個函數(shù)后，gyro_bias 結(jié)構(gòu)體將被填充當前的校準參數(shù)。

主程序執(zhí)行流程

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

    uint8_t reg;
    /* Read output only if new xl value is available */
    lsm6dso_xl_flag_data_ready_get(&dev_ctx, ®);

    if (reg) {
      /* Read acceleration field data */
      memset(data_raw_acceleration, 0x00, 3 * sizeof(int16_t));
      lsm6dso_acceleration_raw_get(&dev_ctx, data_raw_acceleration);
      acceleration_mg[0] =
        lsm6dso_from_fs2_to_mg(data_raw_acceleration[0]);
      acceleration_mg[1] =
        lsm6dso_from_fs2_to_mg(data_raw_acceleration[1]);
      acceleration_mg[2] =
        lsm6dso_from_fs2_to_mg(data_raw_acceleration[2]);
//      printf( "Acceleration [mg]:%4.2ft%4.2ft%4.2frn",
//              acceleration_mg[0], acceleration_mg[1], acceleration_mg[2]);
            acc_flag=1;

    }

    lsm6dso_gy_flag_data_ready_get(&dev_ctx, ®);

    if (reg) {
      /* Read angular rate field data */
      memset(data_raw_angular_rate, 0x00, 3 * sizeof(int16_t));
      lsm6dso_angular_rate_raw_get(&dev_ctx, data_raw_angular_rate);
      angular_rate_mdps[0] =
        lsm6dso_from_fs2000_to_mdps(data_raw_angular_rate[0]);
      angular_rate_mdps[1] =
        lsm6dso_from_fs2000_to_mdps(data_raw_angular_rate[1]);
      angular_rate_mdps[2] =
        lsm6dso_from_fs2000_to_mdps(data_raw_angular_rate[2]);
//      printf("Angular rate [mdps]:%4.2ft%4.2ft%4.2frn",
//              angular_rate_mdps[0], angular_rate_mdps[1], angular_rate_mdps[2]);
            gy_flag=1;


    }
        if(acc_flag && gy_flag)
        {
            MX_MEMS_Process();        
            gy_flag=0;
            acc_flag=0;

        }

    /* USER CODE END WHILE */

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

演示

陀螺儀的偏置未被更新為0。

陀螺儀的偏置被更新為1。

可以看到修正后靜止狀態(tài)夏更穩(wěn)定。

審核編輯 黃宇