STM32F103实战：MPU6050姿态解算与Arm-2D 3D显示

1. 项目缘起与整体设计思路

1.1 为什么要在STM32F103上做姿态显示

STM32F103这颗芯片在嵌入式圈子里是什么地位，不用我多吹。Cortex-M3内核、72MHz主频、64KB到128KB的Flash、20KB的SRAM，加上丰富的外设资源，让它成了无数人入门嵌入式的第一块板子。但很多人拿它点个灯、跑个串口就结束了，实际上这颗芯片的潜力远不止于此。我这次要聊的，就是用STM32F103做一个完整的空间姿态显示系统——把MPU6050这类六轴传感器的数据读出来，经过姿态解算，最终在屏幕上实时显示一个3D模型或者姿态角数据。

这个项目解决的核心问题是：如何在一块资源有限的MCU上，实现从传感器采集、数据融合、姿态解算到图形显示的完整链路。它适合已经玩过STM32基础外设、想往传感器融合和嵌入式图形方向进阶的朋友。如果你之前只做过串口打印和LED闪烁，这个项目会把你拉到一个全新的层次。

1.2 整体方案选型与架构拆解

整个系统的架构可以分成四层：感知层、解算层、显示层、交互层。

感知层负责读取MPU6050的原始数据。MPU6050通过I2C接口和STM32F103通信，输出三轴加速度计和三轴陀螺仪的原始值。这里有个细节：MPU6050自带DMP（数字运动处理器），可以直接输出四元数，但我不建议一上来就用DMP。原因很简单——DMP的配置复杂，出了问题你根本不知道是传感器的问题还是解算的问题。先用原始数据自己跑一遍互补滤波或者Mahony算法，把整个链路打通，再去考虑用DMP优化。

解算层的核心任务是把加速度计和陀螺仪的数据融合成姿态角。加速度计能提供静态的倾角信息，但对振动敏感；陀螺仪能提供动态的角速度积分，但存在零漂。两者互补，这就是互补滤波的基本逻辑。具体实现上，我选择在STM32F103上跑一个轻量级的Mahony算法或者一阶互补滤波，计算量小，20KB的SRAM完全够用。

显示层有两种方案：一是用0.96寸或1.3寸的OLED屏幕显示姿态角数字和简单的二维指示；二是用TFT LCD配合Arm-2D图形库显示3D模型。Arm-2D是ARM官方推出的一个轻量级2D图形库，专门为Cortex-M系列优化，支持Alpha混合、图层合成、旋转缩放等操作。用它来做姿态显示，可以把一个立方体或者飞机模型的三个轴实时旋转，视觉效果比单纯看数字直观得多。

交互层主要是按键或者串口命令，用来切换显示模式、校准传感器、调整滤波参数。这部分不是必须的，但加上之后调试会方便很多。

1.3 关键器件的选型考量

MPU6050几乎是这个项目的标配。它集成了三轴MEMS陀螺仪和三轴MEMS加速度计，内置16位ADC，通过I2C输出数据。价格便宜，资料丰富，社区支持好。唯一需要注意的是，MPU6050的陀螺仪零漂比较明显，上电后需要静置校准，否则积分出来的角度会一直漂。

STM32F103的选择上，我建议用C8T6或者RCT6。C8T6便宜，64KB Flash、20KB SRAM，跑姿态解算和OLED显示绰绰有余。如果要上TFT LCD和Arm-2D，建议用RCT6，256KB Flash、48KB SRAM，空间更宽裕。最小系统板加上MPU6050模块和一块SPI OLED，总成本控制在50块钱以内。

显示屏方面，SPI接口的OLED最省事，只需要4根线，驱动简单，刷新率也够用。TFT LCD的话，推荐ST7789或者ILI9341驱动的240x240或240x320屏幕，SPI接口版本接线少，配合Arm-2D可以做出很流畅的旋转效果。

2. 核心细节解析与实操要点

2.1 MPU6050的I2C通信与数据读取

MPU6050的I2C地址是0x68（AD0接地）或0x69（AD0接VCC）。STM32F103的硬件I2C一直被人诟病，主要是早期标准库的I2C驱动有bug，容易卡死。我的建议是：用HAL库的硬件I2C，或者干脆用软件模拟I2C。软件模拟I2C的好处是引脚随便选，时序自己控制，不容易卡死，缺点是速度慢一点。对于MPU6050这种数据量不大的传感器，软件I2C完全够用。

读取数据的流程是这样的：先初始化MPU6050，配置电源管理寄存器、陀螺仪量程、加速度计量程、数字低通滤波器。然后在一个定时器中断里以固定频率（比如1kHz）读取原始数据。读取的时候，加速度计和陀螺仪各有6个寄存器，共12个字节，可以一次性连续读取。

// 软件I2C读取MPU6050原始数据 void MPU6050_ReadRaw(int16_t *accel, int16_t *gyro) { uint8_t buf[14]; I2C_ReadBytes(MPU6050_ADDR, MPU6050_ACCEL_XOUT_H, buf, 14); accel[0] = (int16_t)((buf[0] << 8) | buf[1]); accel[1] = (int16_t)((buf[2] << 8) | buf[3]); accel[2] = (int16_t)((buf[4] << 8) | buf[5]); // buf[6]和buf[7]是温度，跳过 gyro[0] = (int16_t)((buf[8] << 8) | buf[9]); gyro[1] = (int16_t)((buf[10] << 8) | buf[11]); gyro[2] = (int16_t)((buf[12] << 8) | buf[13]); }

这里有个坑：MPU6050的寄存器地址是8位的，但I2C传输的时候需要左移一位加上读写位。很多新手在这里搞混，读出来的数据全是0xFF或者0x00。另外，MPU6050上电后需要等待一段时间才能稳定，建议延时100ms再开始配置。

2.2 姿态解算算法的选择与实现

姿态解算的核心目标是把加速度计和陀螺仪的数据融合成三个姿态角：横滚角（Roll）、俯仰角（Pitch）、偏航角（Yaw）。加速度计可以算出Roll和Pitch，但Yaw算不出来，因为重力矢量在水平面没有分量。陀螺仪积分可以算出三个角，但零漂会导致误差累积。

一阶互补滤波的思路很简单：高频用陀螺仪，低频用加速度计。具体公式是：

angle = alpha * (angle + gyro * dt) + (1 - alpha) * accel_angle

其中alpha通常取0.95到0.98之间。这个方法的优点是计算量极小，在STM32F103上跑毫无压力。缺点是Yaw角只能靠陀螺仪积分，时间长了还是会漂。

Mahony算法稍微复杂一点，它用四元数表示姿态，通过加速度计的测量值来修正陀螺仪的积分误差。核心思想是用加速度计的实际测量值和四元数推算出的重力方向做叉积，得到误差，然后用这个误差去修正陀螺仪的角速度。这个算法在STM32F103上也能跑，计算量大概在几百个浮点运算，72MHz的主频完全扛得住。

// Mahony算法核心修正步骤 void Mahony_Update(float gx, float gy, float gz, float ax, float ay, float az, float dt) { float recipNorm; float halfvx, halfvy, halfvz; float halfex, halfey, halfez; float qa, qb, qc; // 归一化加速度计数据 recipNorm = 1.0f / sqrtf(ax*ax + ay*ay + az*az); ax *= recipNorm; ay *= recipNorm; az *= recipNorm; // 用四元数推算重力方向 halfvx = q1 * q3 - q0 * q2; halfvy = q0 * q1 + q2 * q3; halfvz = q0 * q0 - 0.5f + q3 * q3; // 叉积得到误差 halfex = (ay * halfvz - az * halfvy); halfey = (az * halfvx - ax * halfvz); halfez = (ax * halfvy - ay * halfvx); // 积分误差修正陀螺仪 if (twoKi > 0.0f) { integralFBx += twoKi * halfex * dt; integralFBy += twoKi * halfey * dt; integralFBz += twoKi * halfez * dt; gx += integralFBx; gy += integralFBy; gz += integralFBz; } gx += twoKp * halfex; gy += twoKp * halfey; gz += twoKp * halfez; // 四元数积分 gx *= (0.5f * dt); gy *= (0.5f * dt); gz *= (0.5f * dt); qa = q0; qb = q1; qc = q2; q0 += (-qb * gx - qc * gy - q3 * gz); q1 += (qa * gx + qc * gz - q3 * gy); q2 += (qa * gy - qb * gz + q3 * gx); q3 += (qa * gz + qb * gy - qc * gx); // 归一化四元数 recipNorm = 1.0f / sqrtf(q0*q0 + q1*q1 + q2*q2 + q3*q3); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm; }

Kp和Ki两个参数需要根据实际效果调。Kp控制加速度计修正的强度，太大会导致对振动敏感，太小则收敛慢。Ki用来消除陀螺仪的零漂，一般取很小的值。我的经验是Kp从2.0开始试，Ki从0.005开始试，根据实际效果微调。

2.3 Arm-2D图形库的移植与配置

Arm-2D是ARM官方推出的2D图形库，专门为Cortex-M系列MCU优化。它的核心优势是硬件无关和轻量级。你只需要提供一个像素填充函数和一个显示刷新函数，剩下的图层合成、Alpha混合、旋转缩放都由Arm-2D帮你搞定。

移植Arm-2D的步骤大致如下：首先从官方仓库下载源码，把必要的源文件添加到工程里。然后实现几个底层接口：__pfb_render_region用来把渲染好的像素块刷到屏幕上，__pfb_request_next用来请求下一块缓冲区。Arm-2D支持PFB（Partial Frame Buffer）模式，也就是只分配一小块内存作为渲染缓冲区，分块渲染整个屏幕。对于STM32F103这种SRAM只有20KB的芯片，PFB模式是必须的。

// Arm-2D的PFB配置示例 #define PFB_BLOCK_WIDTH 240 #define PFB_BLOCK_HEIGHT 8 static uint16_t pfb_buffer[PFB_BLOCK_WIDTH * PFB_BLOCK_HEIGHT]; void __pfb_render_region(const arm_2d_region_t *ptRegion, const uint16_t *pBuffer) { // 把pBuffer里的像素刷到屏幕的指定区域 LCD_SetWindow(ptRegion->tLocation.iX, ptRegion->tLocation.iY, ptRegion->tSize.iWidth, ptRegion->tSize.iHeight); LCD_WritePixels(pBuffer, ptRegion->tSize.iWidth * ptRegion->tSize.iHeight); }

Arm-2D的旋转功能是通过arm_2d_tile_transform或者arm_2d_tile_transform_with_opacity实现的。你可以把一个立方体的六个面分别作为独立的Tile，然后根据姿态角计算每个面的旋转矩阵，最后合成到一起。这个过程听起来复杂，但Arm-2D已经把矩阵运算封装好了，你只需要提供旋转角度和缩放系数。

2.4 显示刷新与数据更新的时序配合

姿态解算和显示刷新是两个不同频率的任务。姿态解算需要高频运行，一般1kHz左右，保证积分精度。显示刷新只需要30Hz到60Hz，人眼就感觉流畅了。如果让显示刷新也跑1kHz，CPU根本忙不过来，而且屏幕也响应不了那么快。

我的做法是用一个定时器产生1kHz的中断，在中断里读取MPU6050数据并调用姿态解算函数。然后在主循环里以固定间隔（比如每33ms）更新一次显示。两个任务之间通过全局变量传递姿态角数据。这里要注意数据一致性：姿态角是浮点数，在32位MCU上读写是原子的，不需要额外加锁。但如果传递的是结构体或者数组，就要考虑在中断里关中断保护一下。

注意：MPU6050的I2C读取不要在中断里做，I2C传输耗时较长，放在中断里会阻塞其他中断。正确的做法是在中断里置一个标志位，在主循环里检测到标志位后再去读取I2C数据。

3. 实操过程与核心环节实现

3.1 硬件连接与最小系统搭建

先说一下硬件连接。STM32F103C8T6最小系统板一块，MPU6050模块一个，SPI OLED或TFT LCD一块，杜邦线若干。

MPU6050的接线：VCC接3.3V，GND接GND，SCL接PB6，SDA接PB7。如果你用软件I2C，这两个引脚可以随便换，只要在代码里改一下宏定义就行。INT引脚可以不接，我们用轮询方式读取数据。

OLED的接线（以SPI为例）：VCC接3.3V，GND接GND，SCK接PA5，MOSI接PA7，RES接PB0，DC接PB1，CS接PA4。如果你用TFT LCD，接线类似，只是多几个引脚。

这里有个实际经验：MPU6050模块上的上拉电阻。很多模块自带了4.7k的上拉电阻，但有些便宜的模块没有。如果没有上拉电阻，I2C通信会不稳定，读出来的数据时好时坏。你可以用万用表量一下SCL和SDA对VCC的电阻，如果是4.7k左右就说明有上拉。没有的话，自己在SCL和SDA上各接一个4.7k电阻到3.3V。

3.2 工程搭建与CubeMX配置

我用STM32CubeMX来生成工程框架，省去手动配置时钟和外设的麻烦。具体配置如下：

系统时钟：HSE选择外部晶振，PLL倍频到72MHz。STM32F103的外部时钟电路是8MHz晶振加上两个20pF的负载电容。复位电路是10k电阻上拉到3.3V，加上100nF电容到地。

I2C1：如果要用硬件I2C，配置为标准模式或快速模式，时钟频率400kHz。但我建议用软件I2C，所以这里不配置I2C外设，直接用GPIO模拟。

SPI1：配置为全双工主模式，时钟极性低，时钟相位第一个边沿，预分频到18MHz左右。OLED和TFT LCD都支持这个速度。

TIM2：配置为1kHz中断，用来触发姿态解算。预分频器设为71，自动重装载值设为999，这样72MHz / 72 / 1000 = 1kHz。

USART1：配置为115200波特率，用来输出调试信息。这个不是必须的，但强烈建议加上，调试的时候能省很多事。

CubeMX生成代码后，你需要把Arm-2D的源码和MPU6050的驱动代码添加到工程里。MPU6050的驱动可以自己写，也可以用现成的开源库。我建议自己写一遍，这样出了问题你知道去哪里找。

3.3 姿态解算的完整实现流程

姿态解算的完整流程是这样的：

第一步，初始化MPU6050。配置寄存器0x6B为0x00，解除休眠。配置0x1B为0x00，陀螺仪量程±250°/s。配置0x1C为0x00，加速度计量程±2g。配置0x1A为0x03，数字低通滤波器带宽44Hz。

第二步，陀螺仪零漂校准。上电后静置传感器，采集1000个陀螺仪样本，求平均值作为零漂值。后续读取的陀螺仪数据都要减去这个零漂。

// 陀螺仪零漂校准 void Gyro_Calibrate(void) { int32_t sum[3] = {0, 0, 0}; int16_t gyro[3]; for (int i = 0; i < 1000; i++) { MPU6050_ReadRaw(NULL, gyro); sum[0] += gyro[0]; sum[1] += gyro[1]; sum[2] += gyro[2]; HAL_Delay(2); } gyro_offset[0] = sum[0] / 1000.0f; gyro_offset[1] = sum[1] / 1000.0f; gyro_offset[2] = sum[2] / 1000.0f; }

第三步，在1kHz中断里读取数据并解算。把原始数据转换成物理量：加速度计除以16384得到g值，陀螺仪除以131得到°/s。然后调用Mahony算法更新四元数，最后把四元数转换成欧拉角。

// 四元数转欧拉角 void Quaternion_ToEuler(float q0, float q1, float q2, float q3, float *roll, float *pitch, float *yaw) { *roll = atan2f(2.0f * (q0 * q1 + q2 * q3), 1.0f - 2.0f * (q1 * q1 + q2 * q2)) * 57.29578f; *pitch = asinf(2.0f * (q0 * q2 - q3 * q1)) * 57.29578f; *yaw = atan2f(2.0f * (q0 * q3 + q1 * q2), 1.0f - 2.0f * (q2 * q2 + q3 * q3)) * 57.29578f; }

第四步，在主循环里更新显示。如果是OLED，直接显示三个角度值。如果是TFT LCD加Arm-2D，就根据姿态角计算旋转矩阵，渲染一个3D立方体。

3.4 Arm-2D显示3D姿态的实操细节

用Arm-2D显示3D姿态，核心思路是：把3D模型拆成多个2D面片，根据姿态角计算每个面片的旋转和位置，然后用Arm-2D的Tile Transform功能渲染出来。

以立方体为例，它有6个面，每个面是一个矩形Tile。在3D空间中，每个面的四个顶点坐标是固定的。根据当前的姿态角（Roll、Pitch、Yaw），计算旋转矩阵，把3D坐标投影到2D屏幕坐标。然后根据投影后的四个顶点，计算Tile的旋转角度和缩放系数，调用Arm-2D的变换函数渲染。

// 3D点旋转并投影到2D void Project3DTo2D(float x, float y, float z, float roll, float pitch, float yaw, int16_t *screen_x, int16_t *screen_y) { float cx = cosf(roll), sx = sinf(roll); float cy = cosf(pitch), sy = sinf(pitch); float cz = cosf(yaw), sz = sinf(yaw); // 绕X轴旋转 float y1 = y * cx - z * sx; float z1 = y * sx + z * cx; // 绕Y轴旋转 float x2 = x * cy + z1 * sy; float z2 = -x * sy + z1 * cy; // 绕Z轴旋转 float x3 = x2 * cz - y1 * sz; float y3 = x2 * sz + y1 * cz; // 透视投影 float scale = 200.0f / (200.0f + z2); *screen_x = (int16_t)(x3 * scale) + 120; *screen_y = (int16_t)(y3 * scale) + 120; }

这个投影函数在每个显示刷新周期里对立方体的8个顶点各调用一次，得到8个2D坐标。然后根据这些坐标确定每个面的位置和大小，用Arm-2D的arm_2d_tile_transform渲染。实际测试下来，240x240的屏幕，30Hz刷新率，STM32F103跑起来毫无压力。

提示：Arm-2D的Tile Transform需要指定一个源Tile和一个目标区域。源Tile就是你的面片图像，目标区域是根据投影坐标计算出来的矩形。旋转角度可以通过arm_2d_location_t和arm_2d_region_t来指定。

4. 常见问题与排查技巧实录

4.1 MPU6050数据读取失败的排查思路

MPU6050读不出数据，是最常见的问题。排查步骤我整理了一个速查表：

我踩过最坑的一次是：MPU6050模块的VCC接了5V，结果I2C电平不匹配，STM32的3.3V引脚识别不了5V电平，读出来的数据全是乱的。后来改成3.3V供电就正常了。所以一定要确认模块的供电电压和I2C电平匹配。

4.2 姿态角漂移与振动的处理经验

姿态角漂移是另一个高频问题。主要表现为：静止时Yaw角缓慢变化，或者运动后Roll和Pitch角回不到零位。

Yaw角漂移的根本原因是陀螺仪零漂。解决方法有两个：一是上电时做零漂校准，二是在Mahony算法里加入Ki积分项。零漂校准只能消除固定偏差，温度变化引起的漂移还需要Ki来动态修正。我的经验是Ki取0.001到0.01之间，太小了修正慢，太大了会引起振荡。

Roll和Pitch角回不到零位，通常是加速度计振动引起的。加速度计对振动非常敏感，电机转动或者手部抖动都会让加速度计输出噪声。解决方法是在MPU6050内部开启数字低通滤波器，把带宽降到20Hz左右。或者在软件里对加速度计数据做滑动平均滤波。

// 加速度计滑动平均滤波 #define FILTER_SIZE 8 float accel_buf[3][FILTER_SIZE]; int filter_index = 0; void Accel_Filter(float *ax, float *ay, float *az) { accel_buf[0][filter_index] = *ax; accel_buf[1][filter_index] = *ay; accel_buf[2][filter_index] = *az; filter_index = (filter_index + 1) % FILTER_SIZE; float sum[3] = {0, 0, 0}; for (int i = 0; i < FILTER_SIZE; i++) { sum[0] += accel_buf[0][i]; sum[1] += accel_buf[1][i]; sum[2] += accel_buf[2][i]; } *ax = sum[0] / FILTER_SIZE; *ay = sum[1] / FILTER_SIZE; *az = sum[2] / FILTER_SIZE; }

注意：滑动平均滤波会引入相位延迟，FILTER_SIZE越大延迟越明显。对于姿态显示这种应用，8个样本的延迟可以接受。如果做飞行控制，延迟就不能太大，建议用更复杂的卡尔曼滤波。

4.3 Arm-2D显示闪烁与撕裂的解决

用Arm-2D做显示的时候，如果刷新率设置不当，会出现画面闪烁或者撕裂。闪烁的原因是PFB缓冲区太小，分块渲染的时候前后两帧不一致。撕裂的原因是渲染和刷屏没有同步。

解决闪烁的方法是增大PFB缓冲区。如果SRAM够用，把PFB的高度从8行增加到16行或者32行。STM32F103C8T6只有20KB SRAM，240x8的PFB需要24082=3840字节，已经占了近20%。如果不够用，可以降低屏幕分辨率，或者用RGB565格式减少内存占用。

解决撕裂的方法是双缓冲。Arm-2D支持双PFB缓冲，一个用于渲染，一个用于刷屏。渲染完一帧后交换缓冲区，这样刷屏的时候不会影响到正在渲染的帧。不过双缓冲会占用双倍内存，STM32F103C8T6可能吃不消，建议用RCT6。

4.4 浮点运算性能优化的几个技巧

STM32F103没有硬件浮点单元，所有浮点运算都是软件模拟，速度比较慢。Mahony算法里有大量的浮点乘法和三角函数，如果不优化，1kHz的中断里可能跑不完。

第一个优化技巧：用查表法代替三角函数。atan2f和asinf在每次解算后都要调用，这两个函数很耗时。可以预先计算一个正弦表，用查表加插值的方式代替实时计算。

第二个优化技巧：用定点数代替浮点数。把四元数和角速度都放大一定倍数用整数表示，乘法用64位整数运算。这样速度能快好几倍，但精度会有所下降。对于姿态显示来说，精度要求不高，定点数完全够用。

第三个优化技巧：降低解算频率。1kHz不是必须的，500Hz甚至200Hz也能得到不错的效果。把定时器中断频率降到500Hz，CPU负载直接减半。

我实测下来，72MHz的STM32F103跑Mahony算法加Arm-2D显示，解算频率500Hz，显示刷新30Hz，CPU占用率大概在60%左右。如果优化得当，还能再降。

4.5 从C8T6移植到RCT6的注意事项

很多人一开始用C8T6，后来发现SRAM不够用，想换RCT6。移植的时候要注意几点：

第一，启动文件要换。C8T6用的是startup_stm32f103xb.s，RCT6用的是startup_stm32f103xe.s。在CubeMX里重新生成工程的时候会自动处理，但如果是手动移植，别忘了改。

第二，Flash算法要换。C8T6是64KB Flash，RCT6是256KB。烧录的时候如果还用C8T6的算法，只能烧进去64KB，后面的代码全丢了。

第三，时钟配置不变。STM32F103系列都是72MHz主频，时钟配置不用改。

第四，外设引脚可能不同。RCT6的引脚比C8T6多，但常用外设的引脚位置基本一致。如果用了C8T6上没有的外设，需要重新配置。

提示：如果你用的是HAL库，从C8T6移植到RCT6只需要在CubeMX里改一下芯片型号，重新生成代码就行。但如果你用的是标准库，就需要手动改启动文件和链接脚本。

4.6 常见问题速查表

这个项目我从头到尾做了大概两周，中间踩了不少坑，但也学到了很多东西。STM32F103虽然老，但用来做姿态显示这种中等复杂度的项目，性能完全够用。关键是你要理解每个环节的原理，知道出了问题去哪里找原因。Arm-2D这个库我是第一次用，刚开始觉得挺复杂，但用熟了之后发现它的API设计很合理，移植也方便。如果你也在做类似的项目，希望这些经验能帮你少走点弯路。