概述

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

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

視頻教學

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

樣品申請

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

源碼下載

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

硬件準備

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

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

開啟CRC

串口設置

設置串口速率為2000000。

開啟X-CUBE-MEMS1

速率選擇

陀螺儀數據可以設置25Hz-200Hz。

參考程序

這里參考 IKS01A3_MagnetometerCalibration 。

MX_MEMS_Init

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

獲取并打印庫版本信息

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

獲取并設置校準參數

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

設置初始陀螺儀偏置

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

設置采樣頻率

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

初始化定義

/* 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() 函數在MEMS傳感器應用中主要負責處理傳感器數據并執行與校準相關的操作。在這個上下文中，MX_MEMS_Process() 函數具體執行以下任務：
MX_MEMS_Process() 主要調用了 MX_GyroscopeCalibration_Process() 函數。這是一個私有函數，專門用于處理陀螺儀校準數據的流和處理。
在 MX_GyroscopeCalibration_Process() 函數中，主要執行了 GC_Data_Handler()，它負責從傳感器中讀取數據，運行校準算法，并應用校準結果。

MotionGC_manager_update

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

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

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

這個參數的作用是讓調用者知道在當前的調用中，校準算法是否對陀螺儀的偏置進行了調整。這對于動態監控校準過程和判斷當前校準狀態是否穩定非常有用。

MotionGC_manager_compensate

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

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

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

主程序執行流程

/* 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。

可以看到修正后靜止狀態夏更穩定。

審核編輯 黃宇