【STM32】玩轉IIC之驅動MPU6050及姿態解算

前言

一.MPU6050模塊介紹

?1.1MPU6050簡介

1.2 MPU6050的引腳定義

1.3MPU6050寄存器解析

二.MPU6050驅動開發

2.1 配置寄存器

2.2對MPU6050寄存器進行讀寫

2.2.1 寫入寄存器

2.2.2讀取寄存器

2.3?初始化MPU6050

2.3.1 設置工作模式

2.3.2 配置采樣率

2.3.3 啟用傳感器

2.4MPU6050數據讀取

2.4.1 讀取加速度數據

2.4.2 讀取陀螺儀數據

2.4.3 數據格式轉換

2.5 驅動代碼實現

2.5.1 初始化函數

2.5.2 數據讀取函數

3.5.2 數據讀取函數

2.6 示例代碼

三. 姿態解算

3.1姿態角與坐標系

3.1.1 姿態角的定義

3.1.2 坐標系的概念

3.2姿態解算算法

3.2.1四元數姿態解算實現

3.2.2四元數姿態解算算法

3.2.2.1初始化四元數

3.2.2.2讀取傳感器數據

3.2.2.3 加速度計歸一化

3.2.2.4計算姿態誤差

3.2.2.5四元數更新

3.2.2.6計算歐拉角

3.2.2.7主函數

?3.3注意事項

四. 應用與調試

4.1 數據可視化

4.2 常見問題與調試技巧

4.3 應用實例

五. 總結

前言

小伙伴們，大家好！已經迫不及待想分享新的技術了！今天，我們來搞點好玩的——MPU6050。這可是個好東西，一個集成了加速度計和陀螺儀的六軸傳感器，能幫你輕松搞定姿態測量和運動檢測。話不多說，直接開搞！！！干貨滿滿，內容比較多，大家可以存下來慢慢看👀

在嵌入式系統和物聯網的世界里，傳感器就像是我們的“眼睛”和“耳朵”，能幫我們感知周圍的一切。而MPU6050，就是這樣一個超級厲害的傳感器。它不僅能測加速度，還能測角速度，簡直是姿態測量的神器！今天，我們就用STM32來驅動它，順便搞搞姿態解算，讓你的項目瞬間高大上起來。

一.MPU6050模塊介紹

?1.1MPU6050簡介

MPU6050是一款高性能的六軸運動傳感器，集成了三軸加速度計和三軸陀螺儀。它能夠測量加速度和角速度，廣泛應用于姿態測量、運動檢測、機器人控制等領域。MPU6050的主要特性如下：

三軸加速度計：測量范圍為±2g/±4g/±8g/±16g（可選單位g為重力加速度）。
三軸陀螺儀：測量范圍為±250°/s/±500°/s/±1000°/s/±2000°/s（可選）。
數字運動處理器（DMP）：支持復雜的運動處理算法，如姿態解算。
IIC接口：支持標準IIC通信協議，易于與微控制器連接。
低功耗：適合電池供電的便攜式設備。
高精度：能夠提供高精度的加速度和角速度數據。

1.2 MPU6050的引腳定義

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

VCC：電源正極（3.3V或5V）。
GND：電源地。
SDA：IIC數據線。
SCL：IIC時鐘線。
INT：中斷輸出引腳（可選）。
AD0：地址選擇引腳，用于設置IIC設備地址。

1.3MPU6050寄存器解析

MPU6050通過IIC接口與微控制器通信，數據存儲在內部寄存器中。下面將介紹我們完成功能所主要用寄存器：

PWR_MGMT_1：電源管理寄存器，用于設置傳感器的工作模式。
SMPLRT_DIV：采樣率分頻寄存器，用于設置數據采樣率。
CONFIG：配置寄存器，用于設置數字低通濾波器（DLPF）。
GYRO_CONFIG：陀螺儀配置寄存器，用于設置測量范圍。
ACCEL_CONFIG：加速度計配置寄存器，用于設置測量范圍。
ACCEL_XOUT_H/L、ACCEL_YOUT_H/L、ACCEL_ZOUT_H/L：加速度數據寄存器。
GYRO_XOUT_H/L、GYRO_YOUT_H/L、GYRO_ZOUT_H/L：陀螺儀數據寄存器。

寄存器地址	功能描述
0x6B	PWR_MGMT_1：電源管理寄存器，用于設備復位和時鐘源設置。
0x6C	PWR_MGMT_2：電源管理寄存器，用于設備睡眠模式控制。
0x1B	GYRO_CONFIG：陀螺儀配置寄存器，用于設置陀螺儀的量程范圍。
0x1C	ACCEL_CONFIG：加速度計配置寄存器，用于設置加速度計的量程范圍。
0x1A	CONFIG：配置寄存器，用于設置數字低通濾波器（DLPF）。
0x19	SMPLRT_DIV：采樣率分頻寄存器，用于設置數據采樣率。
0x3B-0x40	加速度計和陀螺儀數據寄存器，分別存儲加速度和角速度的原始數據。

通過配置這些寄存器，可以實現對MPU6050的初始化和數據讀取。

二.MPU6050驅動開發

2.1 配置寄存器

配置寄存器用于設置MPU6050的工作模式、采樣率、濾波器等參數。以下是幾個常用配置寄存器的詳細說明：

PWR_MGMT_1（0x6B）：
- 位7：設備復位位，寫入0x80復位設備。
- 位0-2：時鐘源選擇，通常設置為0x00，使用內部時鐘。
GYRO_CONFIG（0x1B）：
- 位3-0：陀螺儀量程選擇，可選范圍為±250°/s、±500°/s、±1000°/s、±2000°/s。
ACCEL_CONFIG（0x1C）：
- 位3-0：加速度計量程選擇，可選范圍為±2g、±4g、±8g、±16g。
SMPLRT_DIV（0x19）：
- 位7-0：采樣率分頻值，設置為0時，采樣率最高。

對于寄存器這里我另外寫了一個頭文件包含了對MPU6050所有的寄存器地址進行聲明，方便調用，簡單明了！

#ifndef __MPU6050_REG_H
#define __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#endif

2.2對MPU6050寄存器進行讀寫

2.2.1 寫入寄存器

通過IIC接口向MPU6050的寄存器寫入數據，可以配置傳感器的工作模式、量程、采樣率等參數。以下是寫入寄存器的代碼實現：

#define MPU6050_Addr 0xD0void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data) {IIC_Start();IIC_SendByte(MPU6050_Addr); // 發送設備地址（寫操作）IIC_ReceiveAck();IIC_SendByte(RegAddress); // 發送寄存器地址IIC_ReceiveAck();IIC_SendByte(Data); // 發送數據IIC_ReceiveAck();IIC_Stop();
}

2.2.2讀取寄存器

通過IIC接口從MPU6050的寄存器讀取數據，可以獲取傳感器的配置狀態或測量數據。以下是讀取寄存器的代碼實現：

uint8_t MPU6050_ReadReg(uint8_t RegAddress) {uint8_t Data;IIC_Start();IIC_SendByte(MPU6050_Addr); // 發送設備地址（寫操作）IIC_ReceiveAck();IIC_SendByte(RegAddress); // 發送寄存器地址IIC_ReceiveAck();IIC_Start();IIC_SendByte(MPU6050_Addr | 0x01); // 發送設備地址（讀操作）IIC_ReceiveAck();Data = IIC_ReceiveByte(); // 讀取數據IIC_SendAck(1); // 發送NACK，結束讀取IIC_Stop();return Data;
}

2.3?初始化MPU6050

2.3.1 設置工作模式

通過PWR_MGMT_1寄存器設置MPU6050的工作模式。通常在初始化時，先復位設備，然后將其設置為正常工作模式。

void MPU6050_SetPowerMode(uint8_t mode) 
{MPU6050_WriteReg(MPU6050_PWR_MGMT_1, mode);
}

2.3.2 配置采樣率

通過SMPLRT_DIV寄存器設置采樣率分頻值，從而控制數據的采樣頻率。

void MPU6050_SetSampleRate(uint8_t div) 
{MPU6050_WriteReg(MPU6050_SMPLRT_DIV, div);
}

2.3.3 啟用傳感器

通過PWR_MGMT_1寄存器啟用加速度計和陀螺儀。

void MPU6050_EnableSensors(void) 
{MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x00); // 啟用設備
}

2.4MPU6050數據讀取

2.4.1 讀取加速度數據

加速度數據存儲在寄存器0x3B到0x40中，分別表示加速度計的X、Y、Z軸數據。

void MPU6050_ReadAccel(int16_t *ax, int16_t *ay, int16_t *az) 
{uint8_t buffer[6]; // 創建一個6字節的緩沖區，用于存儲從MPU6050讀取的加速度數據// 逐個讀取加速度計的X、Y、Z軸的高字節和低字節數據buffer[0] = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H); // X軸加速度數據的高字節buffer[1] = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L); // X軸加速度數據的低字節buffer[2] = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_H); // Y軸加速度數據的高字節buffer[3] = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_L); // Y軸加速度數據的低字節buffer[4] = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_H); // Z軸加速度數據的高字節buffer[5] = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_L); // Z軸加速度數據的低字節// 將高字節和低字節組合成完整的16位加速度數據*ax = (buffer[0] << 8) | buffer[1]; // 將X軸的高字節左移8位后與低字節進行按位或操作，得到完整的X軸加速度數據*ay = (buffer[2] << 8) | buffer[3]; // 同理，得到Y軸加速度數據*az = (buffer[4] << 8) | buffer[5]; // 同理，得到Z軸加速度數據
}

2.4.2 讀取陀螺儀數據

陀螺儀數據存儲在寄存器0x43到0x48中，分別表示陀螺儀的X、Y、Z軸數據。

void MPU6050_ReadGyro(int16_t *gx, int16_t *gy, int16_t *gz) 
{uint8_t buffer[6]; // 創建一個6字節的緩沖區，用于存儲從MPU6050讀取的陀螺儀數據// 逐個讀取陀螺儀的X、Y、Z軸的高字節和低字節數據buffer[0] = MPU6050_ReadReg(MPU6050_GYRO_XOUT_H); // X軸陀螺儀數據的高字節buffer[1] = MPU6050_ReadReg(MPU6050_GYRO_XOUT_L); // X軸陀螺儀數據的低字節buffer[2] = MPU6050_ReadReg(MPU6050_GYRO_YOUT_H); // Y軸陀螺儀數據的高字節buffer[3] = MPU6050_ReadReg(MPU6050_GYRO_YOUT_L); // Y軸陀螺儀數據的低字節buffer[4] = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H); // Z軸陀螺儀數據的高字節buffer[5] = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L); // Z軸陀螺儀數據的低字節// 將高字節和低字節組合成完整的16位陀螺儀數據*gx = (buffer[0] << 8) | buffer[1]; // 將X軸的高字節左移8位后與低字節進行按位或操作，得到完整的X軸陀螺儀數據*gy = (buffer[2] << 8) | buffer[3]; // 同理，得到Y軸陀螺儀數據*gz = (buffer[4] << 8) | buffer[5]; // 同理，得到Z軸陀螺儀數據
}

2.4.3 數據格式轉換

將讀取到的原始數據轉換為實際的物理量。

float MPU6050_ConvertAccel(int16_t value, uint8_t range) 
{float factor; // 用于存儲轉換因子// 根據加速度計的量程選擇合適的轉換因子switch (range) {case 0x00: factor = 16384.0; break; // ±2g，轉換因子為16384case 0x08: factor = 8192.0; break;  // ±4g，轉換因子為8192case 0x10: factor = 4096.0; break;  // ±8g，轉換因子為4096case 0x18: factor = 1365.3; break;  // ±16g，轉換因子為1365.3default: factor = 16384.0; break;    // 默認量程為±2g}// 將原始加速度值轉換為g單位return value / factor;
}float MPU6050_ConvertGyro(int16_t value, uint8_t range) 
{float factor; // 用于存儲轉換因子// 根據陀螺儀的量程選擇合適的轉換因子switch (range) {case 0x00: factor = 131.0; break;  // ±250°/s，轉換因子為131case 0x08: factor = 65.5; break;   // ±500°/s，轉換因子為65.5case 0x10: factor = 32.8; break;   // ±1000°/s，轉換因子為32.8case 0x18: factor = 16.4; break;   // ±2000°/s，轉換因子為16.4default: factor = 131.0; break;     // 默認量程為±250°/s}// 將原始陀螺儀值轉換為°/s單位return value / factor;
}

2.5 驅動代碼實現

2.5.1 初始化函數

初始化MPU6050，設置工作模式、采樣率和傳感器量程。

void MPU6050_Init(void) 
{// 復位MPU6050MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x80);MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x00);// 設置陀螺計量程為±250°/sMPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x00);// 設置加速度計量程為±2gMPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x00);// 設置采樣率分頻值為0MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x00);
}

2.5.2 數據讀取函數

讀取加速度和陀螺儀數據，并轉換為實際的物理量。

void MPU6050_ReadSensors(float *ax, float *ay, float *az, float *gx, float *gy, float *gz) 
{int16_t raw_ax, raw_ay, raw_az;  // 用于存儲原始加速度數據int16_t raw_gx, raw_gy, raw_gz;  // 用于存儲原始陀螺儀數據// 讀取加速度計的原始數據MPU6050_ReadAccel(&raw_ax, &raw_ay, &raw_az);// 讀取陀螺儀的原始數據MPU6050_ReadGyro(&raw_gx, &raw_gy, &raw_gz);// 將加速度計的原始數據轉換為浮點數形式（單位：g）*ax = MPU6050_ConvertAccel(raw_ax, 0x00); // ±2g量程*ay = MPU6050_ConvertAccel(raw_ay, 0x00); // ±2g量程*az = MPU6050_ConvertAccel(raw_az, 0x00); // ±2g量程// 將陀螺儀的原始數據轉換為浮點數形式（單位：°/s）*gx = MPU6050_ConvertGyro(raw_gx, 0x00); // ±250°/s量程*gy = MPU6050_ConvertGyro(raw_gy, 0x00); // ±250°/s量程*gz = MPU6050_ConvertGyro(raw_gz, 0x00); // ±250°/s量程
}

2.6 示例代碼

以下是一個完整的示例代碼，展示如何初始化MPU6050并讀取加速度和陀螺儀數據：

#include "mpu6050.h"
#include "iic.h"int main(void) {// 初始化IIC接口IIC_Init();// 初始化MPU6050MPU6050_Init();while (1) {float ax, ay, az;float gx, gy, gz;// 讀取傳感器數據MPU6050_ReadSensors(&ax, &ay, &az, &gx, &gy, &gz);// 打印數據printf("Accel: X=%.2fg, Y=%.2fg, Z=%.2fg\r\n", ax, ay, az);printf("Gyro: X=%.2f°/s, Y=%.2f°/s, Z=%.2f°/s\r\n", gx, gy, gz);// 延時delay_ms(1000);}
}

三. 姿態解算

為什么用姿態解算計算設備姿態角👀因為計算設備姿態角上一般所選用的慣性傳感器，都是mems器件，精度相對較差，同時陀螺儀、加速度計、地磁計單個傳感器無法得到滿意的姿態角信息，所以需要一些融合算法，進行姿態估計。

姿態解算的目標是通過融合MPU6050的加速度計和陀螺儀數據，得到一個準確的姿態估計的👉即計算出設備在三維空間（坐標系）中的姿態角（如俯仰角、橫滾角和偏航角）。姿態角是描述物體在空間中旋轉狀態的三個角度，通常用于飛控、機器人、VR設備等領域。

3.1姿態角與坐標系

3.1.1 姿態角的定義

姿態角描述了設備的機體坐標系相對于慣性坐標系的旋轉關系。

俯仰角（Pitch）：表示設備繞X軸的旋轉角度，范圍為-90°到+90°。俯仰角描述了設備相對于水平面的上下傾斜程度。
橫滾角（Roll）：表示設備繞Y軸的旋轉角度，范圍為-180°到+180°。橫滾角描述了設備相對于水平面的左右傾斜程度。
偏航角（Yaw）：表示設備繞Z軸的旋轉角度，范圍為-180°到+180°。偏航角描述了設備在水平面上的旋轉方向。

注：在姿態解算中，姿態角的定義通常是以慣性坐標系（世界坐標系）作為參考的。慣性坐標系是一個固定在空間中的參考系，通常以地球的重力方向為基準。

3.1.2 坐標系的概念

在姿態解算中，坐標系的選擇至關重要。通常使用以下兩種坐標系：

慣性坐標系（世界坐標系）：固定在空間中的坐標系，通常以地球的重力方向為Z軸，水平面為XY平面。
機體坐標系（傳感器坐標系）：固定在設備上的坐標系，通常以設備的幾何中心為原點，X軸指向設備的前方向，Y軸指向設備的右方向，Z軸指向設備的下方向。

姿態角描述了機體坐標系相對于慣性坐標系的旋轉關系。通過姿態解算，可以將機體坐標系下的測量數據轉換到慣性坐標系下，從而實現對設備姿態的描述。??抽象來說：姿態解算是“機體坐標系”與“慣性坐標系”之間的轉換關系！

3.2姿態解算算法

在姿態解算中，姿態可以通過多種方式表示，常見的有歐拉角、四元數和旋轉矩陣。每種表示方法都有其特點和應用場景，且每個姿態表示方法可以相互轉換。

在姿態解算中，常見的算法包括：

四元數法：通過四元數融合加速度計和陀螺儀數據，計算出設備的姿態。
互補濾波法：結合加速度計和陀螺儀數據，通過互補濾波器修正姿態角。
卡爾曼濾波法：一種更高級的濾波算法，能夠更精確地融合傳感器數據，適用于復雜的姿態解算

關于上面的所提到的每一個姿態表示方法和姿態解算算法并沒有逐個的展開講解，如果大家感興趣的話可以自行查閱，這里我們主要講四元數法應用！

3.2.1四元數姿態解算實現

四元數是一種用于表示三維旋轉的數學工具，由四個分量組成：q0?,q1?,q2?,q3?。其中，q0? 是實部，q1?,q2?,q3? 是虛部。四元數可以表示為： q=q0?+q1?i+q2?j+q3?k

相比歐拉角，它具有更好的數值穩定性和抗奇異性的特點，避免歐拉角中的“萬向鎖”問題。以下是四元數姿態解算的基本步驟：

初始化四元數：初始狀態下，四元數設為單位四元數，即 q0?=1,q1?=q2?=q3?=0。
讀取傳感器數據：從MPU6050讀取加速度計和陀螺儀的原始數據，并將陀螺儀數據轉換為弧度。
加速度計歸一化：將加速度計數據歸一化，以提取重力方向。
計算姿態誤差：通過加速度計和四元數推導出的重力方向的差異，計算姿態誤差。
互補濾波：將姿態誤差反饋到四元數更新中，結合陀螺儀數據，通過互補濾波器修正四元數。
四元數更新：通過微分方程更新四元數，并進行歸一化處理。
計算歐拉角：將四元數轉換為歐拉角，得到俯仰角、橫滾角和偏航角。

3.2.2四元數姿態解算算法

3.2.2.1初始化四元數

初始化四元數為單位四元數，表示初始姿態為零。

//定義一個關于四元數的結構體變量
typedef struct 
{float q0, q1, q2, q3;
} Quaternion;Quaternion q = {1.0f, 0.0f, 0.0f, 0.0f};  // 初始化四元數

3.2.2.2讀取傳感器數據

從MPU6050讀取加速度計和陀螺儀的原始數據，并將陀螺儀數據轉換為弧度。

void MPU6050_ReadSensors(float *ax, float *ay, float *az, float *gx, float *gy, float *gz)
{int16_t raw_ax, raw_ay, raw_az;int16_t raw_gx, raw_gy, raw_gz;MPU6050_ReadAccel(&raw_ax, &raw_ay, &raw_az);MPU6050_ReadGyro(&raw_gx, &raw_gy, &raw_gz);*ax = MPU6050_ConvertAccel(raw_ax, 0x00);  // ±2g*ay = MPU6050_ConvertAccel(raw_ay, 0x00);*az = MPU6050_ConvertAccel(raw_az, 0x00);*gx = MPU6050_ConvertGyro(raw_gx, 0x00);  // ±250°/s*gy = MPU6050_ConvertGyro(raw_gy, 0x00);*gz = MPU6050_ConvertGyro(raw_gz, 0x00);
}

3.2.2.3 加速度計歸一化

將加速度計數據歸一化，提取重力方向

void NormalizeAccel(float *ax, float *ay, float *az) 
{float norm = sqrt(*ax * *ax + *ay * *ay + *az * *az);*ax /= norm;*ay /= norm;*az /= norm;
}

3.2.2.4計算姿態誤差

通過加速度計和四元數推導出的重力方向的差異，計算姿態誤差。

void ComputeError(float ax, float ay, float az, float *error_x, float *error_y, float *error_z) 
{float gravity_x = 2 * (q.q1 * q.q3 - q.q0 * q.q2);float gravity_y = 2 * (q.q0 * q.q1 + q.q2 * q.q3);float gravity_z = 1 - 2 * (q.q1 * q.q1 + q.q2 * q.q2);*error_x = (ay * gravity_z - az * gravity_y);*error_y = (az * gravity_x - ax * gravity_z);*error_z = (ax * gravity_y - ay * gravity_x);
}

3.2.2.5四元數更新

結合陀螺儀數據，通過微分方程更新四元數，并進行歸一化處理，避免數值誤差會越積越大。

void UpdateQuaternion(float gx, float gy, float gz, float error_x, float error_y, float error_z, float dt) 
{float Kp = 0.5f;  // 互補濾波系數float q0_dot = -q.q1 * gx - q.q2 * gy - q.q3 * gz;float q1_dot = q.q0 * gx - q.q3 * gy + q.q2 * gz;float q2_dot = q.q3 * gx + q.q0 * gy - q.q1 * gz;float q3_dot = -q.q2 * gx + q.q1 * gy + q.q0 * gz;q.q0 += (q0_dot + Kp * error_x) * dt;q.q1 += (q1_dot + Kp * error_y) * dt;q.q2 += (q2_dot + Kp * error_z) * dt;q.q3 += (q3_dot) * dt;// 歸一化四元數，避免數值誤差會越積越大float norm = sqrt(q.q0 * q.q0 + q.q1 * q.q1 + q.q2 * q.q2 + q.q3 * q.q3);q.q0 /= norm;q.q1 /= norm;q.q2 /= norm;q.q3 /= norm;
}

3.2.2.6計算歐拉角

將四元數轉換為歐拉角，得到俯仰角、橫滾角和偏航角。

void ComputeEulerAngles(float *pitch, float *roll, float *yaw) 
{*roll = atan2(2 * (q.q2 * q.q3 + q.q0 * q.q1), q.q0 * q.q0 - q.q1 * q.q1 - q.q2 * q.q2 + q.q3 * q.q3);*pitch = asin(-2 * (q.q1 * q.q3 - q.q0 * q.q2));*yaw = atan2(2 * (q.q1 * q.q2 + q.q0 * q.q3), q.q0 * q.q0 + q.q1 * q.q1 - q.q2 * q.q2 - q.q3 * q.q3);*roll *= RtA;  // 弧度轉角度*pitch *= RtA;*yaw *= RtA;
}

3.2.2.7主函數

將上述步驟整合到主函數中，實現完整的姿態解算。

#include "mpu6050.h"
#include "math.h"#define PI 3.1415926535f
#define RtA 57.2957795f  // 弧度轉角度
#define AtR 0.0174532925f  // 角度轉弧度typedef struct 
{float q0, q1, q2, q3;
} Quaternion;Quaternion q = {1.0f, 0.0f, 0.0f, 0.0f};  // 初始化四元數void GetAngles(float *pitch, float *roll, float *yaw, float dt) 
{float ax, ay, az, gx, gy, gz;MPU6050_ReadSensors(&ax, &ay, &az, &gx, &gy, &gz);NormalizeAccel(&ax, &ay, &az);float error_x, error_y, error_z;ComputeError(ax, ay, az, &error_x, &error_y, &error_z);UpdateQuaternion(gx, gy, gz, error_x, error_y, error_z, dt);ComputeEulerAngles(pitch, roll, yaw);
}int main(void) 
{// 初始化MPU6050MPU6050_Init();while (1) {float pitch, roll, yaw;GetAngles(&pitch, &roll, &yaw, 0.01f);  // 假設采樣間隔為10ms，保證一定的實時性// 打印姿態角printf("Pitch: %.2f, Roll: %.2f, Yaw: %.2f\r\n", pitch, roll, yaw);// 延時delay_ms(10);}
}

?3.3注意事項

小伙伴們，在動手實現姿態解算的時候，有幾個要點得留心：

硬件連接要穩：MPU6050和STM32的IIC通信線路（SDA和SCL）一定要接對，不然通信可能會出問題。
傳感器得校準：加速度計和陀螺儀用之前最好校準一下，不然數據可能會不準。靜止狀態下多讀幾次數據，算個平均值，這樣能更準。
數據別急躁：加速度計和陀螺儀的數據可能會有點“毛躁”，用低通濾波器或者滑動平均濾波器給它“梳梳毛”，數據會更平滑。
算法參數要調：四元數解算里的互補濾波器增益（比如Kp）很重要，得根據你的實際需求調整，不然解算出來的姿態可能會“飄”。
代碼要細心：四元數更新后一定要歸一化，不然數值誤差會越積越大。還有，采樣頻率別太低，不然實時性會受影響。
調試別慌：遇到數據跳變或者通信失敗，別急，先檢查硬件連接，再看看代碼邏輯。慢慢來，總能找到問題所在。

總之，技術這事兒，耐心和細心是關鍵。希望這些小提示能幫到你，祝你順利搞定姿態解算！

這里我只是帶大家簡單理解如何操作實現的，如果大家有不理解的可以來問，歡迎大家評論，后面我可能會為大家再出一篇關于姿態解算詳細講解😄

四. 應用與調試

4.1 數據可視化

串口打印：通過串口打印姿態角（俯仰、橫滾、偏航），方便實時觀察。
上位機軟件：用LabVIEW、MATLAB等工具接收串口數據，繪制姿態變化曲線。
顯示屏：用LCD或LED顯示姿態角，直觀展示當前姿態。

4.2 常見問題與調試技巧

數據跳變：可能是傳感器干擾或算法問題。檢查硬件連接，增加濾波算法。
數據不準確：可能是傳感器未校準或參數設置不當。校準傳感器，調整算法參數。
通信失敗：可能是硬件連接或IIC配置錯誤。檢查線路，確保通信參數正確。

4.3 應用實例

姿態測量：用于無人機、機器人等設備的姿態控制。
運動檢測：實現計步器、手勢識別等功能。
VR設備：跟蹤頭部運動，提供沉浸式體驗。

五. 總結

不知不覺，我們已經走到了文章的尾聲，說實話，這過程里，我也是邊寫邊學，收獲滿滿。通過這篇文章的介紹，你就可以掌握了如何使用STM32通過IIC接口驅動MPU6050，并實現基于四元數的姿態解算。姿態解算的結果可以通過多種方式可視化，并應用于實際項目中。在調試過程中，注意數據異常處理和精度優化，確保姿態解算的準確性和穩定性。

最后，姿態解算這事兒，還有很多可以深挖的。比如，要是再加個磁力計，精度說不定還能再上個臺階。或者，把解算結果用到無人機、機器人上，讓它們動起來更穩當。這些，就留給大家去探索啦！希望這篇文章能給你帶來些啟發，要是覺得有用，別忘了點贊哦！下次見！