概述

MotionFX庫包含用于校準(zhǔn)陀螺儀、加速度計(jì)和磁力計(jì)傳感器的例程。 將磁力計(jì)的數(shù)據(jù)與加速度計(jì)和陀螺儀的數(shù)據(jù)融合，可以大幅提高姿態(tài)估計(jì)的精度。三軸加速度計(jì)提供設(shè)備的傾斜信息，陀螺儀提供角速度信息，而磁力計(jì)提供方位信息，三者結(jié)合可以提供更加準(zhǔn)確和穩(wěn)定的三維方向和姿態(tài)信息。

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

視頻教學(xué)

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

樣品申請

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

源碼下載

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

硬件準(zhǔn)備

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

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

DataLogFusion

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

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

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

MotionFX庫的磁力計(jì)校準(zhǔn)庫用于補(bǔ)償硬鐵失真。磁力計(jì)校準(zhǔn)可以以比傳感器融合輸出數(shù)據(jù)速率更慢的頻率進(jìn)行（例如25 Hz）。
● 初始化磁力計(jì)校準(zhǔn)庫：
● 調(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)成功。

初始化磁力計(jì)

調(diào)用 MotionFX_MagCal_init 或 MotionFX_CM0P_MagCal_init 函數(shù)。這里通過調(diào)用 MotionFX_MagCal_init，確保磁力計(jì)校準(zhǔn)模塊處于準(zhǔn)備就緒狀態(tài)，能夠正確處理和校準(zhǔn)磁力計(jì)數(shù)據(jù)。
通過初始化磁力計(jì)校準(zhǔn)庫，并定期調(diào)用校準(zhǔn)函數(shù)，可以確保磁力計(jì)數(shù)據(jù)的準(zhǔn)確性，從而提高姿態(tài)估計(jì)的精度。

頻率定義。

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

添加到初始化中進(jìn)行調(diào)用。

文檔中提到的磁力計(jì)數(shù)據(jù)要除以50，這是因?yàn)镸otionFX庫使用的單位是微特斯拉（μT）/50。

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

#define FROM_MGAUSS_TO_UT50     (0.1f/50.0f)

MFX_Arithmetic_Init

MFX_Arithmetic_Init 的作用是初始化 MotionFX 算法庫，并進(jìn)行相關(guān)參數(shù)設(shè)置和配置。

由于現(xiàn)在是9軸解析，需要新定義用于保存 MotionFX 算法狀態(tài)的數(shù)組。

static uint8_t mfxstate_9x[FX_STATE_SIZE];

函數(shù)功能：
● 初始化 MotionFX 算法庫。
● 配置傳感器的偏置值和方向。
● 設(shè)置輸出參考模式。
● 啟用或禁用 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);

    /* 獲取當(dāng)前的內(nèi)部結(jié)構(gòu)參數(shù) */
    MotionFX_getKnobs(mfxstate_9x, ipKnobs);

    /* 設(shè)置傳感器 */
    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;

    /* 未知作用操作，數(shù)據(jù)定向？ */
    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;  

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

    /* 設(shè)置內(nèi)部結(jié)構(gòu)參數(shù) */
    MotionFX_setKnobs(mfxstate_9x, ipKnobs);

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

卡爾曼濾波算法

運(yùn)行卡爾曼濾波傳播算法MotionFX_propagate。

根據(jù)需要更新卡爾曼濾波器MotionFX_update。

需要注意的是這2各算法非常吃資源，需要注意MCU算力分配。

函數(shù)結(jié)構(gòu)如下所示。

演示

和指南針一個(gè)方向。

偏移90度。

偏移180度。

偏移270度。

審核編輯 黃宇