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建议用RCT6256KB 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地址是0x68AD0接地或0x69AD0接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支持PFBPartial 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人眼就感觉流畅了。如果让显示刷新也跑1kHzCPU根本忙不过来而且屏幕也响应不了那么快。我的做法是用一个定时器产生1kHz的中断在中断里读取MPU6050数据并调用姿态解算函数。然后在主循环里以固定间隔比如每33ms更新一次显示。两个任务之间通过全局变量传递姿态角数据。这里要注意数据一致性姿态角是浮点数在32位MCU上读写是原子的不需要额外加锁。但如果传递的是结构体或者数组就要考虑在中断里关中断保护一下。注意MPU6050的I2C读取不要在中断里做I2C传输耗时较长放在中断里会阻塞其他中断。正确的做法是在中断里置一个标志位在主循环里检测到标志位后再去读取I2C数据。3. 实操过程与核心环节实现3.1 硬件连接与最小系统搭建先说一下硬件连接。STM32F103C8T6最小系统板一块MPU6050模块一个SPI OLED或TFT LCD一块杜邦线若干。MPU6050的接线VCC接3.3VGND接GNDSCL接PB6SDA接PB7。如果你用软件I2C这两个引脚可以随便换只要在代码里改一下宏定义就行。INT引脚可以不接我们用轮询方式读取数据。OLED的接线以SPI为例VCC接3.3VGND接GNDSCK接PA5MOSI接PA7RES接PB0DC接PB1CS接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读不出数据是最常见的问题。排查步骤我整理了一个速查表现象可能原因排查方法读出来全是0xFFI2C地址错误确认AD0引脚电平0x68或0x69读出来全是0x00传感器未初始化检查0x6B寄存器是否写入了0x00数据偶尔跳变I2C上拉电阻缺失测量SCL/SDA对VCC电阻应为4.7k数据完全不变I2C通信失败用逻辑分析仪抓I2C波形温度值正常但加速度计为0量程配置错误检查0x1C寄存器配置我踩过最坑的一次是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 SRAM240x8的PFB需要240823840字节已经占了近20%。如果不够用可以降低屏幕分辨率或者用RGB565格式减少内存占用。解决撕裂的方法是双缓冲。Arm-2D支持双PFB缓冲一个用于渲染一个用于刷屏。渲染完一帧后交换缓冲区这样刷屏的时候不会影响到正在渲染的帧。不过双缓冲会占用双倍内存STM32F103C8T6可能吃不消建议用RCT6。4.4 浮点运算性能优化的几个技巧STM32F103没有硬件浮点单元所有浮点运算都是软件模拟速度比较慢。Mahony算法里有大量的浮点乘法和三角函数如果不优化1kHz的中断里可能跑不完。第一个优化技巧用查表法代替三角函数。atan2f和asinf在每次解算后都要调用这两个函数很耗时。可以预先计算一个正弦表用查表加插值的方式代替实时计算。第二个优化技巧用定点数代替浮点数。把四元数和角速度都放大一定倍数用整数表示乘法用64位整数运算。这样速度能快好几倍但精度会有所下降。对于姿态显示来说精度要求不高定点数完全够用。第三个优化技巧降低解算频率。1kHz不是必须的500Hz甚至200Hz也能得到不错的效果。把定时器中断频率降到500HzCPU负载直接减半。我实测下来72MHz的STM32F103跑Mahony算法加Arm-2D显示解算频率500Hz显示刷新30HzCPU占用率大概在60%左右。如果优化得当还能再降。4.5 从C8T6移植到RCT6的注意事项很多人一开始用C8T6后来发现SRAM不够用想换RCT6。移植的时候要注意几点第一启动文件要换。C8T6用的是startup_stm32f103xb.sRCT6用的是startup_stm32f103xe.s。在CubeMX里重新生成工程的时候会自动处理但如果是手动移植别忘了改。第二Flash算法要换。C8T6是64KB FlashRCT6是256KB。烧录的时候如果还用C8T6的算法只能烧进去64KB后面的代码全丢了。第三时钟配置不变。STM32F103系列都是72MHz主频时钟配置不用改。第四外设引脚可能不同。RCT6的引脚比C8T6多但常用外设的引脚位置基本一致。如果用了C8T6上没有的外设需要重新配置。提示如果你用的是HAL库从C8T6移植到RCT6只需要在CubeMX里改一下芯片型号重新生成代码就行。但如果你用的是标准库就需要手动改启动文件和链接脚本。4.6 常见问题速查表问题排查方向解决方法姿态角静止时漂移陀螺仪零漂上电校准增大Ki姿态角运动后不回零加速度计振动开启低通滤波滑动平均显示画面闪烁PFB缓冲区太小增大PFB高度降低分辨率显示画面撕裂渲染刷屏不同步双缓冲或降低刷新率解算频率上不去浮点运算太慢查表法定点数降频I2C通信失败上拉电阻缺失外接4.7k上拉电阻烧录后不运行启动文件错误检查启动文件和Flash算法屏幕花屏SPI时序不对降低SPI时钟检查DC/CS引脚这个项目我从头到尾做了大概两周中间踩了不少坑但也学到了很多东西。STM32F103虽然老但用来做姿态显示这种中等复杂度的项目性能完全够用。关键是你要理解每个环节的原理知道出了问题去哪里找原因。Arm-2D这个库我是第一次用刚开始觉得挺复杂但用熟了之后发现它的API设计很合理移植也方便。如果你也在做类似的项目希望这些经验能帮你少走点弯路。