本文介绍了基于 STM32 通过 IIC 接口驱动 MPU6050 六轴运动传感器的方法，包括寄存器配置、数据读取及物理量转换。重点阐述了利用加速度计和陀螺仪数据进行四元数姿态解算的算法实现，涵盖初始化、误差计算、四元数更新及欧拉角提取。提供了完整的 C 语言代码示例，并总结了调试注意事项与应用场景。
在嵌入式系统和物联网的世界里，传感器就像是我们的'眼睛'和'耳朵'，能帮我们感知周围的一切。而 MPU6050，就是这样一个超级厉害的传感器。它不仅能测加速度，还能测角速度，简直是姿态测量的神器！今天，我们就用 STM32 来驱动它，顺便搞搞姿态解算。
MPU6050 是一款高性能的六轴运动传感器，集成了三轴加速度计和三轴陀螺仪。它能够测量加速度和角速度，广泛应用于姿态测量、运动检测、机器人控制等领域。MPU6050 的主要特性如下：
MPU6050 模块通常具有以下引脚：
MPU6050 通过 IIC 接口与微控制器通信，数据存储在内部寄存器中。下面将介绍我们完成功能所主要用寄存器：
| 寄存器地址 | 功能描述 |
|---|---|
| 0x6B | PWR_MGMT_1：电源管理寄存器，用于设备复位和时钟源设置。 |
| 0x6C | PWR_MGMT_2：电源管理寄存器，用于设备睡眠模式控制。 |
| 0x1B | GYRO_CONFIG：陀螺仪配置寄存器，用于设置陀螺仪的量程范围。 |
| 0x1C | ACCEL_CONFIG：加速度计配置寄存器，用于设置加速度计的量程范围。 |
| 0x1A | CONFIG：配置寄存器，用于设置数字低通滤波器（DLPF）。 |
| 0x19 | SMPLRT_DIV：采样率分频寄存器，用于设置数据采样率。 |
| 0x3B-0x40 | 加速度计和陀螺仪数据寄存器，分别存储加速度和角速度的原始数据。 |
通过配置这些寄存器，可以实现对 MPU6050 的初始化和数据读取。
配置寄存器用于设置 MPU6050 的工作模式、采样率、滤波器等参数。以下是几个常用配置寄存器的详细说明：
0x80 复位设备。0x00，使用内部时钟。对于寄存器这里我另外写了一个头文件包含了对 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
通过 IIC 接口向 MPU6050 的寄存器写入数据，可以配置传感器的工作模式、量程、采样率等参数。以下是写入寄存器的代码实现：
#define MPU6050_Addr 0xD0
void 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();
}
通过 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;
}
通过 PWR_MGMT_1 寄存器设置 MPU6050 的工作模式。通常在初始化时，先复位设备，然后将其设置为正常工作模式。
void MPU6050_SetPowerMode(uint8_t mode) {
MPU6050_WriteReg(MPU6050_PWR_MGMT_1, mode);
}
通过 SMPLRT_DIV 寄存器设置采样率分频值，从而控制数据的采样频率。
void MPU6050_SetSampleRate(uint8_t div) {
MPU6050_WriteReg(MPU6050_SMPLRT_DIV, div);
}
通过 PWR_MGMT_1 寄存器启用加速度计和陀螺仪。
void MPU6050_EnableSensors(void) {
MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x00); // 启用设备
}
加速度数据存储在寄存器 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 轴加速度数据
}
陀螺仪数据存储在寄存器 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 轴陀螺仪数据
}
将读取到的原始数据转换为实际的物理量。
float MPU6050_ConvertAccel(int16_t value, uint8_t range) {
float factor; // 用于存储转换因子
// 根据加速度计的量程选择合适的转换因子
switch (range) {
case 0x00: factor = 16384.0; break; // ±2g，转换因子为 16384
case 0x08: factor = 8192.0; break; // ±4g，转换因子为 8192
case 0x10: factor = 4096.0; break; // ±8g，转换因子为 4096
case 0x18: factor = 2048.0; break; // ±16g，转换因子为 2048
default: 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，转换因子为 131
case 0x08: factor = 65.5; break; // ±500°/s，转换因子为 65.5
case 0x10: factor = 32.8; break; // ±1000°/s，转换因子为 32.8
case 0x18: factor = 16.4; break; // ±2000°/s，转换因子为 16.4
default: factor = 131.0; break; // 默认量程范围为±250°/s
}
// 将原始陀螺仪值转换为°/s 单位
return value / factor;
}
初始化 MPU6050，设置工作模式、采样率和传感器量程。
#define Gyro_Range 0x18
#define Accel_Range 0x18
// 初始化硬件 MPU6050
void MPU6050_Init(void) {
IIC_Init(); // 初始化 IIC
MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x81); // 复位选用陀螺仪时钟作为时钟源
MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00); // 使能 3 轴加速度和 3 轴陀螺仪
MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09); // 设置采样分频器为 10
MPU6050_WriteReg(MPU6050_CONFIG, 0x06); // 设置数字低通滤波器
MPU6050_WriteReg(MPU6050_GYRO_CONFIG, Gyro_Range); // 配置陀螺仪满量程 +2000dps
MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, Accel_Range); // 配置加速度计满量程±16g
Delay_ms(50);
}
读取加速度和陀螺仪数据，并转换为实际的物理量。
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, Accel_Range); // ±2g 量程
*ay = MPU6050_ConvertAccel(raw_ay, Accel_Range); // ±2g 量程
*az = MPU6050_ConvertAccel(raw_az, Accel_Range); // ±2g 量程
// 将陀螺仪的原始数据转换为浮点数形式（单位：°/s）
*gx = MPU6050_ConvertGyro(raw_gx, Gyro_Range); // ±250°/s 量程
*gy = MPU6050_ConvertGyro(raw_gy, Gyro_Range); // ±250°/s 量程
*gz = MPU6050_ConvertGyro(raw_gz, Gyro_Range); // ±250°/s 量程
}
以下是一个完整的示例代码，展示如何初始化 MPU6050 并读取加速度和陀螺仪数据：
#include "stm32f10x.h" // Device header
#include "Delay.h"
#include "MPU6050.h"
#include "MPU6050_Reg.h"
#include "Usart.h"
int main(void) {
// 初始化 MPU6050
MPU6050_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 设备等领域。
姿态角描述了设备的机体坐标系相对于惯性坐标系的旋转关系（以我图中的画的坐标系作为参考）
俯仰角（Pitch）：表示设备绕 X 轴的旋转角度，范围为**-90°到 +90°**。俯仰角描述了设备相对于水平面的上下倾斜程度。
横滚角（Roll）：表示设备绕 Y 轴的旋转角度，范围为**-180°到 +180°**。横滚角描述了设备相对于水平面的左右倾斜程度。
偏航角（Yaw）：表示设备绕 Z 轴的旋转角度，范围为**-180°到 +180°**。偏航角描述了设备在水平面上的旋转方向。
**注：**在姿态解算中，姿态角的定义通常是以惯性坐标系（世界坐标系）作为参考的。惯性坐标系是一个固定在空间中的参考系，通常以地球的重力方向为基准。
在姿态解算中，坐标系的选择至关重要。通常使用以下两种坐标系：
姿态角描述了机体坐标系相对于惯性坐标系的旋转关系。通过姿态解算，可以将机体坐标系下的测量数据转换到惯性坐标系下，从而实现对设备姿态的描述。抽象来说：姿态解算是'机体坐标系'与'惯性坐标系'之间的转换关系！
在姿态解算中，通过传感器（如加速度计、陀螺仪和磁力计）获取的数据，可以计算出这些欧拉角。这些角度通常用于描述飞行器、机器人或其他设备的姿态。
**仅使用加速度计和陀螺仪的数据也可以求出欧拉角，但会有一定的局限性和误差。**加速度计可以提供关于重力方向的信息，从而帮助确定俯仰角和横滚角，而陀螺仪可以提供角速度信息，通过积分可以得到姿态变化。然而，由于陀螺仪的积分误差和加速度计在动态情况下的不准确性，仅使用这两个传感器的姿态解算通常不如加入磁力计的数据准确。
在姿态解算中，姿态可以通过多种方式表示，常见的有欧拉角、四元数和旋转矩阵。每种表示方法都有其特点和应用场景，且每个姿态表示方法可以相互转换。
在姿态解算中，常见的算法包括：
关于上面的所提到的每一个姿态表示方法和姿态解算算法并没有逐个的展开讲解，如果大家感兴趣的话可以自行查阅，这里我们主要讲四元数法应用！
四元数是一种用于表示三维旋转的数学工具，由四个分量组成：q0, q1, q2, q3。其中，q0 是实部，q1, q2, q3 是虚部。四元数可以表示为：q=q0+q1i+q2j+q3k
相比欧拉角，它具有更好的数值稳定性和抗奇异性的特点，避免欧拉角中的'万向锁'问题。以下是四元数姿态解算的基本步骤：
初始化四元数为单位四元数，表示初始姿态为零。
// 定义一个关于四元数的结构体变量
typedef struct {
float q0, q1, q2, q3;
} Quaternion;
Quaternion q = {1.0f, 0.0f, 0.0f, 0.0f}; // 初始化四元数 表示初始姿态为水平。
从 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, Accel_Range); // ±2g
*ay = MPU6050_ConvertAccel(raw_ay, Accel_Range);
*az = MPU6050_ConvertAccel(raw_az, Accel_Range);
*gx = MPU6050_ConvertGyro(raw_gx, Gyro_Range) * AtR; // ±250°/s
*gy = MPU6050_ConvertGyro(raw_gy, Gyro_Range) * AtR;
*gz = MPU6050_ConvertGyro(raw_gz, Gyro_Range) * AtR;
}
将加速度计数据归一化，提取重力方向
void NormalizeAccel(float *ax, float *ay, float *az) {
float norm = sqrt(*ax * *ax + *ay * *ay + *az * *az);
*ax /= norm;
*ay /= norm;
*az /= norm;
}
通过加速度计和四元数推导出的重力方向的差异，计算姿态误差。
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);
}
结合陀螺仪数据，通过微分方程更新四元数，并进行归一化处理，避免数值误差会越积越大。
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;
}
将四元数转换为欧拉角，得到俯仰角、横滚角和偏航角。
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;
}
这些公式是基于四元数到欧拉角的转换关系，具体推导可以参考相关数学文献
将上述步骤整合到主函数中，实现完整的姿态解算。
#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) {
// 初始化 MPU6050
MPU6050_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);
}
}
实现姿态解算时需注意以下几点：
Kp）很重要，得根据你的实际需求调整，不然解算出来的姿态可能会'飘'。通过这篇文章的介绍，你就可以掌握了如何使用 STM32 通过 IIC 接口驱动 MPU6050，并实现基于四元数的姿态解算。姿态解算的结果可以通过多种方式可视化，并应用于实际项目中。在调试过程中，注意数据异常处理和精度优化，确保姿态解算的准确性和稳定性。
最后，姿态解算还有很多可以深挖的。比如，要是再加个磁力计，精度说不定还能再上个台阶。或者，把解算结果用到无人机、机器人上，让它们动起来更稳当。这些，就留给大家去探索啦！
微信公众号「极客日志」，在微信中扫描左侧二维码关注。展示文案：极客日志 zeeklog
使用加密算法（如AES、TripleDES、Rabbit或RC4）加密和解密文本明文。 在线工具，加密/解密文本在线工具，online
将字符串编码和解码为其 Base64 格式表示形式即可。 在线工具，Base64 字符串编码/解码在线工具，online
将字符串、文件或图像转换为其 Base64 表示形式。 在线工具，Base64 文件转换器在线工具，online
将 Markdown（GFM）转为 HTML 片段，浏览器内 marked 解析；与 HTML转Markdown 互为补充。 在线工具，Markdown转HTML在线工具，online
将 HTML 片段转为 GitHub Flavored Markdown，支持标题、列表、链接、代码块与表格等；浏览器内处理，可链接预填。 在线工具，HTML转Markdown在线工具，online
通过删除不必要的空白来缩小和压缩JSON。 在线工具，JSON 压缩在线工具，online