引言

隨著物聯(lián)網(wǎng)、可穿戴設(shè)備、工業(yè)傾角檢測(cè)等場(chǎng)景的普及，低成本、低功耗的姿態(tài)檢測(cè)方案成為嵌入式領(lǐng)域的研究熱點(diǎn)。MPU6050 作為集成三軸加速度計(jì)和三軸陀螺儀的六軸傳感器，憑借低成本、小體積的優(yōu)勢(shì)被廣泛應(yīng)用，但單一傳感器存在明顯缺陷：加速度計(jì)易受運(yùn)動(dòng)加速度干擾（動(dòng)態(tài)精度差），陀螺儀存在零漂累積誤差（靜態(tài)精度差）。

CW32L012 是武漢芯源半導(dǎo)體推出的 32 位 ARM Cortex-M0 + 內(nèi)核 MCU，具備超低功耗（睡眠模式電流 μA 級(jí)）、高性?xún)r(jià)比、豐富的外設(shè)接口（I2C、UART 等）的特點(diǎn)，非常適合邊緣端低資源、低功耗的傳感融合場(chǎng)景。

本文基于 CW32L012 主控，實(shí)現(xiàn) MPU6050 的傳感器數(shù)據(jù)讀取、基礎(chǔ)姿態(tài)解算，并通過(guò)卡爾曼濾波完成加速度計(jì)與陀螺儀的數(shù)據(jù)融合，最終輸出高精度、高魯棒性的姿態(tài)角。

意義

基于 CW32L012 的 MPU6050 數(shù)據(jù)讀取、姿態(tài)解算與卡爾曼數(shù)據(jù)融合方案，一方面針對(duì)性破解了 MPU6050 單一傳感器加速度計(jì)易受運(yùn)動(dòng)加速度干擾、陀螺儀存在零漂累積誤差的精度短板，依托卡爾曼濾波實(shí)現(xiàn)的多源數(shù)據(jù)融合，在 CW32L012 μA 級(jí)低功耗與 96MHz 主頻兼顧運(yùn)算效率的硬件支撐下，在邊緣端低資源、低功耗的約束中實(shí)現(xiàn)了姿態(tài)檢測(cè)靜態(tài)與動(dòng)態(tài)精度的同步提升，完美適配物聯(lián)網(wǎng)、可穿戴設(shè)備、工業(yè)傾角檢測(cè)等場(chǎng)景對(duì) “低成本 + 低功耗 + 高精度” 姿態(tài)檢測(cè)的需求；

另一方面該方案以國(guó)產(chǎn) CW32L012 為核心載體，驗(yàn)證了國(guó)產(chǎn) Cortex-M0 + 內(nèi)核 MCU 在傳感數(shù)據(jù)處理、輕量化算法運(yùn)算場(chǎng)景下的性能潛力，打破了低功耗傳感融合領(lǐng)域?qū)M(jìn)口 MCU 的依賴(lài)，同時(shí)這套覆蓋傳感器讀取、解算、融合全流程的輕量化方案，也為嵌入式開(kāi)發(fā)者提供了可直接移植的參考模板，降低了中小廠(chǎng)商的研發(fā)門(mén)檻，助力國(guó)產(chǎn)嵌入式芯片在感知層細(xì)分領(lǐng)域的產(chǎn)業(yè)化落地。

CW32L012C8T6介紹

CW32L012 是武漢芯源半導(dǎo)體 (CW) 推出的 32 位低功耗 MCU，基于 ARM Cortex-M0 + 內(nèi)核，主頻高達(dá) 96MHz，集成64KB Flash和8KB RAM，專(zhuān)為需要高性?xún)r(jià)比和低功耗的應(yīng)用設(shè)計(jì)。

一、核心規(guī)格：

二、關(guān)鍵特性

高性能計(jì)算單元

擴(kuò)展算術(shù)運(yùn)算單元 (EAU)：硬件加速除法和開(kāi)方運(yùn)算，大幅提升數(shù)學(xué)計(jì)算效率

CORDIC 硬件單元：支持三角函數(shù)、矢量旋轉(zhuǎn)等復(fù)雜運(yùn)算

雙 12 位 ADC：最高 1M SPS 轉(zhuǎn)換速度，內(nèi)置 1.2V 基準(zhǔn)電壓，支持單端 / 差分模式

雙 12 位 DAC：高精度模擬輸出，支持 DMA 傳輸

豐富外設(shè)資源

模擬外設(shè)：4 路電壓比較器、雙路軌到軌運(yùn)算放大器

通信接口：3 路 UART (支持 LIN)、2 路 I2C (支持 SMBus)、3 路 SPI (最高 24Mbps)

定時(shí)器陣列：高級(jí)定時(shí)器 (6 路 PWM)、4 個(gè)通用定時(shí)器、低功耗定時(shí)器、霍爾傳感器專(zhuān)用定時(shí)器等

40 路 GPIO：全部支持中斷和喚醒功能，推挽 / 開(kāi)漏輸出

超低功耗設(shè)計(jì)

多種低功耗模式：Sleep、DeepSleep

靈活時(shí)鐘管理：可獨(dú)立關(guān)斷各外設(shè)時(shí)鐘，降低功耗

低功耗定時(shí)器：支持低功耗模式下的定時(shí)喚醒

可編程 LVD：低電壓檢測(cè)，防止異常斷電

三、應(yīng)用場(chǎng)景

電機(jī)控制領(lǐng)域

無(wú)刷直流電機(jī) (BLDC) 控制 (如智能水泵)

家電電機(jī)控制 (風(fēng)扇、空調(diào))

工業(yè)自動(dòng)化設(shè)備

測(cè)量與監(jiān)測(cè)設(shè)備

充電器控制

電池管理系統(tǒng) (BMS)

智能插座、電表

消費(fèi)電子

智能家居控制器

便攜式設(shè)備 (MP3 播放器)

智能玩具 (如 "哈基汪" 智能小車(chē))

MPU6050介紹與開(kāi)發(fā)

一、MPU6050介紹

1.1MPU6050簡(jiǎn)介

MPU6050是一款高性能的六軸運(yùn)動(dòng)傳感器，集成了三軸加速度計(jì)和三軸陀螺儀。它能夠測(cè)量加速度和角速度，廣泛應(yīng)用于姿態(tài)測(cè)量、運(yùn)動(dòng)檢測(cè)、機(jī)器人控制等領(lǐng)域。MPU6050的主要特性如下：

三軸加速度計(jì)：測(cè)量范圍為±2g/±4g/±8g/±16g（可選 單位g為重力加速度）。

三軸陀螺儀：測(cè)量范圍為±250°/s/±500°/s/±1000°/s/±2000°/s（可選）。

數(shù)字運(yùn)動(dòng)處理器（DMP）：支持復(fù)雜的運(yùn)動(dòng)處理算法，如姿態(tài)解算。

IIC接口：支持標(biāo)準(zhǔn)IIC通信協(xié)議，易于與微控制器連接。

低功耗：適合電池供電的便攜式設(shè)備。

高精度：能夠提供高精度的加速度和角速度數(shù)據(jù)。

1.2 MPU6050的引腳定義

MPU6050模塊通常具有以下引腳：

VCC：電源正極（3.3V或5V）。

GND：電源地。

SDA：IIC數(shù)據(jù)線(xiàn)。

SCL：IIC時(shí)鐘線(xiàn)。

INT：中斷輸出引腳（可選）。

AD0：地址選擇引腳，用于設(shè)置IIC設(shè)備地址。

1.3MPU6050寄存器解析

MPU6050通過(guò)IIC接口與微控制器通信，數(shù)據(jù)存儲(chǔ)在內(nèi)部寄存器中。下面將介紹我們完成功能所主要用寄存器：

PWR_MGMT_1：電源管理寄存器，用于設(shè)置傳感器的工作模式。

SMPLRT_DIV：采樣率分頻寄存器，用于設(shè)置數(shù)據(jù)采樣率。

CONFIG：配置寄存器，用于設(shè)置數(shù)字低通濾波器（DLPF）。

GYRO_CONFIG：陀螺儀配置寄存器，用于設(shè)置測(cè)量范圍。

ACCEL_CONFIG：加速度計(jì)配置寄存器，用于設(shè)置測(cè)量范圍。

ACCEL_XOUT_H/L、ACCEL_YOUT_H/L、ACCEL_ZOUT_H/L：加速度數(shù)據(jù)寄存器。

GYRO_XOUT_H/L、GYRO_YOUT_H/L、GYRO_ZOUT_H/L：陀螺儀數(shù)據(jù)寄存器

二、MPU6050驅(qū)動(dòng)開(kāi)發(fā)

2.1 寄存器描述

配置寄存器用于設(shè)置MPU6050的工作模式、采樣率、濾波器等參數(shù)。以下是幾個(gè)常用配置寄存器的詳細(xì)說(shuō)明：

PWR_MGMT_1（0x6B）：

位7：設(shè)備復(fù)位位，寫(xiě)入0x80復(fù)位設(shè)備。

位0-2：時(shí)鐘源選擇，通常設(shè)置為0x00，使用內(nèi)部時(shí)鐘。

GYRO_CONFIG（0x1B）：

位3-0：陀螺儀量程選擇，可選范圍為±250°/s、±500°/s、±1000°/s、±2000°/s。

ACCEL_CONFIG（0x1C）：

位3-0：加速度計(jì)量程選擇，可選范圍為±2g、±4g、±8g、±16g。

SMPLRT_DIV（0x19）：

位7-0：采樣率分頻值，設(shè)置為0時(shí)，采樣率最高

為了方便，先在MPU6050_Reg.h里面宏定義常用的寄存器：

#define        MPU6050_SMPLRT_DIV          0x19
#define        MPU6050_CONFIG              0x1A
#define        MPU6050_GYRO_CONFIG         0x1B
#define        MPU6050_ACCEL_CONFIG        0x1C
#define        MPU6050_ACCEL_XOUT_H        0x3B
#define        MPU6050_ACCEL_XOUT_L        0x3C
#define        MPU6050_ACCEL_YOUT_H        0x3D
#define        MPU6050_ACCEL_YOUT_L        0x3E
#define        MPU6050_ACCEL_ZOUT_H        0x3F
#define        MPU6050_ACCEL_ZOUT_L        0x40
#define        MPU6050_TEMP_OUT_H          0x41
#define        MPU6050_TEMP_OUT_L          0x42
#define        MPU6050_GYRO_XOUT_H         0x43
#define        MPU6050_GYRO_XOUT_L         0x44
#define        MPU6050_GYRO_YOUT_H         0x45
#define        MPU6050_GYRO_YOUT_L         0x46
#define        MPU6050_GYRO_ZOUT_H         0x47
#define        MPU6050_GYRO_ZOUT_L         0x48
#define        MPU6050_PWR_MGMT_1          0x6B
#define        MPU6050_PWR_MGMT_2          0x6C
#define        MPU6050_WHO_AM_I            0x75

2.2 對(duì)MPU6050寄存器進(jìn)行讀寫(xiě)

2.2.1 寫(xiě)入寄存器

通過(guò)IIC接口向MPU6050的寄存器寫(xiě)入數(shù)據(jù)，可以配置傳感器的工作模式、量程、采樣率等參數(shù)。以下是寫(xiě)入寄存器的代碼實(shí)現(xiàn)：

/**
  * 函    數(shù)：MPU6050寫(xiě)寄存器
  * 參    數(shù)：RegAddress 寄存器地址，范圍：參考MPU6050手冊(cè)的寄存器描述
  * 參    數(shù)：Data 要寫(xiě)入寄存器的數(shù)據(jù)，范圍：0x00~0xFF
  * 返 回 值：無(wú)
  */
void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data)
{
        MyI2C_Start();                                                //I2C起始
        MyI2C_SendByte(MPU6050_ADDRESS);        //發(fā)送從機(jī)地址，讀寫(xiě)位為0，表示即將寫(xiě)入
        MyI2C_ReceiveAck();                                        //接收應(yīng)答
        MyI2C_SendByte(RegAddress);                        //發(fā)送寄存器地址
        MyI2C_ReceiveAck();                                        //接收應(yīng)答
        MyI2C_SendByte(Data);                                //發(fā)送要寫(xiě)入寄存器的數(shù)據(jù)
        MyI2C_ReceiveAck();                                        //接收應(yīng)答
        MyI2C_Stop();                                                //I2C終止
}

2.2.2讀取寄存器

通過(guò)IIC接口從MPU6050的寄存器讀取數(shù)據(jù)，可以獲取傳感器的配置狀態(tài)或測(cè)量數(shù)據(jù)。以下是讀取寄存器的代碼實(shí)現(xiàn)：

/**
  * 函    數(shù)：MPU6050讀寄存器
  * 參    數(shù)：RegAddress 寄存器地址，范圍：參考MPU6050手冊(cè)的寄存器描述
  * 返 回 值：讀取寄存器的數(shù)據(jù)，范圍：0x00~0xFF
  */
uint8_t MPU6050_ReadReg(uint8_t RegAddress)
{
        uint8_t Data;

        MyI2C_Start();                                                //I2C起始
        MyI2C_SendByte(MPU6050_ADDRESS);        //發(fā)送從機(jī)地址，讀寫(xiě)位為0，表示即將寫(xiě)入
        MyI2C_ReceiveAck();                                        //接收應(yīng)答
        MyI2C_SendByte(RegAddress);                        //發(fā)送寄存器地址
        MyI2C_ReceiveAck();                                        //接收應(yīng)答

        MyI2C_Start();                                                //I2C重復(fù)起始
        MyI2C_SendByte(MPU6050_ADDRESS | 0x01);        //發(fā)送從機(jī)地址，讀寫(xiě)位為1，表示即將讀取
        MyI2C_ReceiveAck();                                        //接收應(yīng)答
        Data = MyI2C_ReceiveByte();                        //接收指定寄存器的數(shù)據(jù)
        MyI2C_SendAck(1);                                        //發(fā)送應(yīng)答，給從機(jī)非應(yīng)答，終止從機(jī)的數(shù)據(jù)輸出
        MyI2C_Stop();                                                //I2C終止

        return Data;
}

2.3 初始化MPU6050

根據(jù)上述的讀/寫(xiě)寄存器函數(shù)，對(duì)6050的寄存器進(jìn)行初始化配置：

/**
  * 函    數(shù)：MPU6050初始化
  * 參    數(shù)：無(wú)
  * 返 回 值：無(wú)
  */
void MPU6050_Init(void)
{
        MyI2C_Init();                                                                        //先初始化底層的I2C

        /*MPU6050寄存器初始化，需要對(duì)照MPU6050手冊(cè)的寄存器描述配置，此處僅配置了部分重要的寄存器*/
        MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x01);                //電源管理寄存器1，取消睡眠模式，選擇時(shí)鐘源為X軸陀螺儀
        MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00);                //電源管理寄存器2，保持默認(rèn)值0，所有軸均不待機(jī)
        MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09);                //采樣率分頻寄存器，配置采樣率
        MPU6050_WriteReg(MPU6050_CONFIG, 0x06);                        //配置寄存器，配置DLPF
        MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18);        //陀螺儀配置寄存器，選擇滿(mǎn)量程為±2000°/s
        MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x18);        //加速度計(jì)配置寄存器，選擇滿(mǎn)量程為±16g
}

2.4MPU6050數(shù)據(jù)讀取

根據(jù)上述的讀/寫(xiě)寄存器函數(shù)，將6050的數(shù)據(jù)從對(duì)應(yīng)的寄存器讀出來(lái)：

/**
  * 函    數(shù)：MPU6050獲取數(shù)據(jù)
  * 參    數(shù)：AccX AccY AccZ 加速度計(jì)X、Y、Z軸的數(shù)據(jù)，使用輸出參數(shù)的形式返回，范圍：-32768~32767
  * 參    數(shù)：GyroX GyroY GyroZ 陀螺儀X、Y、Z軸的數(shù)據(jù)，使用輸出參數(shù)的形式返回，范圍：-32768~32767
  * 返 回 值：無(wú)
  */
void MPU6050_GetData(int16_t *AccX, int16_t *AccY, int16_t *AccZ, 
                                                int16_t *GyroX, int16_t *GyroY, int16_t *GyroZ)
{
        uint8_t DataH, DataL;                                                                //定義數(shù)據(jù)高8位和低8位的變量

        DataH = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H);                //讀取加速度計(jì)X軸的高8位數(shù)據(jù)
        DataL = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L);                //讀取加速度計(jì)X軸的低8位數(shù)據(jù)
        *AccX = (DataH