概述

MotionFX庫包含用于校準陀螺儀、加速度計和磁力計傳感器的例程。 將磁力計的數據與加速度計和陀螺儀的數據融合，可以大幅提高姿態估計的精度。三軸加速度計提供設備的傾斜信息，陀螺儀提供角速度信息，而磁力計提供方位信息，三者結合可以提供更加準確和穩定的三維方向和姿態信息。

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

視頻教學

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

樣品申請

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

源碼下載

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

硬件準備

首先需要準備一個開發板，這里我準備的是自己繪制的開發板，需要的可以進行申請。
主控為STM32H503CB，陀螺儀為LSM6DS3TR-C，磁力計為LIS2MDL。

DataLogFusion

這里參考ST提供的DataLogFusion程序，DataLogFusion示例應用展示了如何使用STMicroelectronics開發的MotionFX中間件庫進行實時運動傳感器數據融合。

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

磁力計校準過程

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，則校準成功。

初始化磁力計

調用 MotionFX_MagCal_init 或 MotionFX_CM0P_MagCal_init 函數。這里通過調用 MotionFX_MagCal_init，確保磁力計校準模塊處于準備就緒狀態，能夠正確處理和校準磁力計數據。
通過初始化磁力計校準庫，并定期調用校準函數，可以確保磁力計數據的準確性，從而提高姿態估計的精度。

頻率定義。

#define ALGO_FREQ  100U /* Algorithm frequency 100Hz */
#define ALGO_PERIOD  (1000U / ALGO_FREQ) /* Algorithm period [ms] */

添加到初始化中進行調用。

文檔中提到的磁力計數據要除以50，這是因為MotionFX庫使用的單位是微特斯拉（μT）/50。

在lsm6ds3tr-c_app.h中添加定義。

#define FROM_MGAUSS_TO_UT50     (0.1f/50.0f)

MFX_Arithmetic_Init

MFX_Arithmetic_Init 的作用是初始化 MotionFX 算法庫，并進行相關參數設置和配置。 由于現在是9軸解析，需要新定義用于保存 MotionFX 算法狀態的數組。

static uint8_t mfxstate_9x[FX_STATE_SIZE];

函數功能：
● 初始化 MotionFX 算法庫。
● 配置傳感器的偏置值和方向。
● 設置輸出參考模式。
● 啟用或禁用 6 軸和 9 軸 MotionFX 引擎。

void MFX_Arithmetic_Init(void)
{
    MFX_knobs_t iKnobs;
    MFX_knobs_t *ipKnobs = &iKnobs;


    /* 初始化 MotionFX 算法庫，參考自 AlgoBuilded 生成代碼 */
    /* 初始化 MotionFX 引擎 */
    MotionFX_initialize((MFXState_t *)mfxstate_9x);

    /* 獲取當前的內部結構參數 */
    MotionFX_getKnobs(mfxstate_9x, ipKnobs);

    /* 設置傳感器 */
    ipKnobs- >gbias_acc_th_sc = GBIAS_ACC_TH_SC_9X;
    ipKnobs- >gbias_gyro_th_sc = GBIAS_GYRO_TH_SC_9X;
    ipKnobs- >gbias_mag_th_sc = GBIAS_MAG_TH_SC_9X;

    /* 未知作用操作，數據定向？ */
    ipKnobs- >acc_orientation[0] = ACC_ORIENTATION_X;
    ipKnobs- >acc_orientation[1] = ACC_ORIENTATION_Y;
    ipKnobs- >acc_orientation[2] = ACC_ORIENTATION_Z;

    ipKnobs- >gyro_orientation[0] = GYR_ORIENTATION_X;
    ipKnobs- >gyro_orientation[1] = GYR_ORIENTATION_Y;
    ipKnobs- >gyro_orientation[2] = GYR_ORIENTATION_Z;

    ipKnobs- >mag_orientation[0] = MAG_ORIENTATION_X;
    ipKnobs- >mag_orientation[1] = MAG_ORIENTATION_Y;
    ipKnobs- >mag_orientation[2] = MAG_ORIENTATION_Z;  

    /* 設置輸出參考模式，數據參考系 */
    ipKnobs- >output_type = MFX_ENGINE_OUTPUT_ENU;
    ipKnobs- >LMode = 1;
    /* modx 代表 MotionFX_update 函數調用頻率，
     *    modx = 1，每調用 MotionFX_propagate 函數一次，可調用 MotionFX_update 函數一次，適用于STM32F4系列處理器，
     *    modx = 2，每調用 MotionFX_propagate 函數兩次，可調用 MotionFX_update 函數一次，適用于STM32F1系列處理器。
     */
    ipKnobs- >modx = 1;

    /* 設置內部結構參數 */
    MotionFX_setKnobs(mfxstate_9x, ipKnobs);

    /* 使能6軸 MotionFX 引擎*/
    MotionFX_enable_6X(mfxstate_9x, MFX_ENGINE_DISABLE);
    /* 關閉9軸 MotionFX 引擎*/
    MotionFX_enable_9X(mfxstate_9x, MFX_ENGINE_ENABLE);
}

卡爾曼濾波算法

運行卡爾曼濾波傳播算法MotionFX_propagate。
根據需要更新卡爾曼濾波器MotionFX_update。
需要注意的是這2各算法非常吃資源，需要注意MCU算力分配。

函數結構如下所示。

演示

和指南針一個方向。

偏移90度。

偏移180度。

偏移270度。

審核編輯 黃宇