简介：本资源是一套基于STM32F103与MPU6500的嵌入式姿态解算完整工程，面向嵌入式开发初学者及无人机、机器人姿态控制方向的进阶学习者，解决六轴IMU数据采集、SPI高速通信、四元数姿态解算与CAN总线输出等核心问题。压缩包共206个文件，含37个C源文件（含stm32f10x_can.c、stm32f10x_tim.c等外设驱动）、36个头文件（.h）、39个编译中间文件（.o/.d），以及uvprojx工程配置、hex可执行镜像、map/axf调试信息等，完整覆盖从底层寄存器配置、SPI时序实现、四元数更新算法到CAN帧封装的全链路代码，包体大小为7.36MB。已有409人学习下载，工程结构规范，模块划分清晰——含传感器驱动层、四元数滤波核心（含时间积分与归一化）、姿态角转换及CAN发送模块，可直接编译烧录运行，是理解IMU姿态解算原理与STM32工业级通信实践的优质参考范例。 手里拿到一份“MPU6500研发代码(四元数).zip”的代码包，里面有STM32F103的SPI驱动、MPU6500寄存器配置、四元数姿态解算这几个关键词，基本就能猜到这是一个什么项目：用STM32F103通过SPI接口读MPU6500（六轴加速度计+陀螺仪），再用四元数完成姿态解算。这套组合在平衡车、四轴、机械臂末端姿态检测、云台稳定器这些场景里特别常见。如果你正准备做这类项目，或者正卡在“MPU6500读出来的数据怎么变成姿态角”这个环节，这篇博客就是给你写的。
我这次会把整个代码包拆开揉碎，从SPI初始化、寄存器坑点、原始数据读取，到四元数姿态解算的数学原理和C代码实现，再到实际调试中我踩过的坑，全部过一遍。不论你是刚接触STM32的新手，还是从MPU6050迁移过来的老手，按这套思路都能少走很多弯路。
1. 项目整体设计与方案选型思路
1.1 为什么是MPU6500而不是MPU6050
很多人的第一反应是：MPU6050资料那么多，为什么要换MPU6500？我最初也是从MPU6050起步的，直到有一次项目中需要更高的数据刷新率，才发现MPU6050的I2C接口跑在400kHz已经是极限，而且I2C的时序受线长和干扰影响很大，稍微布线不好就出现丢数据。MPU6500最大的变化就是原生支持SPI接口，时钟最高可以跑到20MHz左右，读一次完整的加速度计+陀螺仪数据（14个字节）在1MHz速率下也就一百多微秒，这个吞吐量对需要高频控制循环的场景来讲非常关键。
另外一个实际区别是封装和功耗。MPU6500是QFN封装，体积比MPU6050小一圈，工作电流也更低，这对电池供电的可穿戴设备、小型无人机来说很友好。但代价是QFN封装手工焊接难度高一些，建议直接画个小转接板，或者买现成的模块，省得折腾引脚间距。
功能上两者都是六轴（3轴加速度计+3轴陀螺仪），量程配置也几乎一致。但MPU6500的数据寄存器布局和I2C地址都变了，寄存器映射表不能完全照搬MPU6050，这是很多移植代码出错的地方。下面这张表是我整理的关键差异。
| 芯片 | 通信接口 | 最大SPI速率 | I2C地址 | 封装 | 数据寄存器地址 |
|---|---|---|---|---|---|
| MPU6050 | I2C/SPI | 1MHz（实测） | 0x68/0x69 | LGA | ACCEL_XOUT_H=0x3B |
| MPU6500 | I2C/SPI | 20MHz（理论） | 0x68/0x69 | QFN | ACCEL_XOUT_H=0x3B（基本一致） |
注意，虽然数据寄存器地址看起来一样，但配置寄存器的复位行为、部分保留位处理是不同的。比如MPU6500的PWR_MGMT_1寄存器里，bit7还是设备复位，bit6是睡眠模式，这些和MPU6050相同，但有些保留位必须保持默认值，用MPU6050的初始化代码直接跑，偶尔会出现读WHO_AM_I正常、读数据却异常的情况。我这次给出的代码全部按照MPU6500数据手册重新核对过，可以直接用。
1.2 SPI和I2C怎么选，为什么这次用SPI
I2C的优势是引脚少（只需要SDA和SCL两根线），但MPU6500作为从机挂在I2C总线上，速率上限不高，而且主机读数据时需要先发寄存器地址、再重启总线读数据，一帧数据要拆成多次传输。对于50Hz、100Hz的姿态更新率，I2C勉强够用，但如果你想把姿态解算频率提到500Hz甚至1kHz，I2C就成了瓶颈。
SPI是四线制（SCLK、MOSI、MISO、CS），多两根线，但全双工、速率高、时序简单，stm32的硬件SPI外设可以直接接管，不占用CPU轮询时间。我在这个项目里实际配的是1MHz，读取14字节数据一次事务搞定，实测在72MHz主频下，主循环里姿态更新能做到1kHz左右。如果跑200Hz，CPU占用率不到10%，留出了大量的余量给控制算法、通信协议栈。
再补充一个经验：SPI虽然快，但硬件上要注意CS片选。MPU6500的CS引脚必须由主机明确控制，不能直接接地。因为MPU6500上电后会自动检测CS引脚的电平来决定通信模式——CS拉高是I2C模式，CS拉低才是SPI模式。我第一次就因为这个吃了亏，把CS直接接地，结果芯片老是进不了SPI模式，读WHO_AM_I全返回0xFF。后来改成GPIO控制，先拉高再拉低，问题才解决。
1.3 代码包的整体模块划分
这份代码包的核心思路是三层分离：
- 底层：SPI读写函数封装，直接操作STM32F103的SPI1外设，提供8位读写接口。
- 驱动层：MPU6500寄存器读写、初始化、加速度计/陀螺仪原始数据读取。
- 解算层：四元数姿态解算算法（互补滤波/Mahony），输出四元数和欧拉角。
这种分层的好处是每一层都可以单独替换。比如以后换一颗IMU芯片，只需要重写驱动层，解算层完全不用动；或者把底层从硬件SPI换成软件模拟SPI，也只是替换底层而已。
代码入口的初始化逻辑顺序我是这么设计的：
- 初始化硬件SPI GPIO、SPI外设。
- 拉低CS，读WHO_AM_I（0x75）确认设备ID为0x70。
- 复位MPU6500（PWR_MGMT_1写0x80），延时100ms。
- 唤醒设备（PWR_MGMT_1写0x00），延时10ms。
- 配置采样率分频、DLPF低通滤波、陀螺仪量程、加速度计量程。
- 采集静态校准数据，计算陀螺零偏。
- 初始四元数设为[1,0,0,0]，进入解算主循环。
这个顺序是标准流程，但中间有几个坑点，比如复位之后必须等足够时间再操作寄存器，否则后续写入会失败。我建议至少延时100ms，有些MPU6500模块需要的复位时间更长，我后面在常见问题里专门说。
2. MPU6500关键寄存器配置与SPI初始化实操
2.1 六针SPI硬件连接与引脚分配
我用的MCU是STM32F103C8T6，SPI1外设的引脚固定，复用映射如下：
| SPI1信号 | STM32F103引脚 |
|---|---|
| SCLK | PA5 |
| MISO | PA6 |
| MOSI | PA7 |
| NSS（软件控制） | PA4 |
MPU6500模块的排针从左到右一般依次是VCC、GND、SCL（SCLK）、SDA（MOSI）、ADO（MISO）、CS。注意这里的SDA标签沿用了I2C的叫法，在SPI模式下实际是MOSI，别接反了。接线就按上面的表格一一对应即可，VCC接3.3V，GND接GND。
有一个硬件细节特别值得说：SPI时钟线SCLK上串联一个22-33欧姆的电阻，可以有效抑制振铃和过冲，尤其当杜邦线长度超过10cm时。我第一版电路没加这个电阻，SPI速率提到4MHz后读回来的数据偶尔出现bit错误，用示波器看SCLK波形，上升沿有很明显的过冲。加上22欧姆电阻后，波形干净很多。这份代码默认支持到4MHz，如果你只跑1MHz，串不串电阻都行，但建议养成好习惯。
2.2 SPI工作模式与参数配置
MPU6500的SPI时序支持Mode 3（CPOL=1，CPHA=1），即时钟空闲时为高电平，第二个边沿采样。我用STM32的硬件SPI一般配置成：
SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex; SPI_InitStructure.SPI_Mode = SPI_Mode_Master; SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b; SPI_InitStructure.SPI_CPOL = SPI_CPOL_High; SPI_InitStructure.SPI_CPHA = SPI_CPHA_2Edge; SPI_InitStructure.SPI_NSS = SPI_NSS_Soft; SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_72; // 1MHz SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;注意SPI_BaudRatePrescaler_72表示72MHz/72=1MHz。这个速率怎么选？MPU6500带长排线的时候，高速容易产生信号质量问题；速率太低又浪费。我的经验是调试阶段用1MHz最稳，初始化成功后可以慢慢提升到4MHz，再往上需要检查波形。另外SPI的CPOL和CPHA如果配错，通常读WHO_AM_I会返回0xFF或者读出很多个字节，因为字节对齐完全乱了。可以写一个小函数反复切换模式去测试，能稳定读到0x70的那个模式就是对的。
在GPIO配置上，SCLK和MOSI、CS都配置为复用推挽输出（GPIO_Mode_AF_PP），MISO配置为上拉输入（GPIO_Mode_IPU）。MISO加上拉是为了防止空闲时浮空导致误码，这个很多人容易忽略。
2.3 初始化流程与寄存器配置详解
MPU6500初始化我用的是标准寄存器序列，每步都加了必要的延时，下面贴上核心代码（标准外设库版本）：
void MPU6500_Init(void) { MPU6500_CS_HIGH(); // CS先拉高，确保识别为SPI模式 // 1. 复位设备 MPU6500_WriteReg(MPU6500_PWR_MGMT_1, 0x80); delay_ms(100); // 2. 唤醒并选择时钟源 MPU6500_WriteReg(MPU6500_PWR_MGMT_1, 0x01); // 用PLL X轴陀螺作为时钟源 delay_ms(10); // 3. 配置采样率分频：采样率 = 陀螺输出速率/(1+SMPLRT_DIV) MPU6500_WriteReg(MPU6500_SMPLRT_DIV, 0x04); // 配合DLPF，得到约200Hz // 4. 配置DLPF低通滤波 MPU6500_WriteReg(MPU6500_CONFIG, 0x03); // 加速度计和陀螺仪都过44Hz低通 // 5. 陀螺仪量程：±2000dps MPU6500_WriteReg(MPU6500_GYRO_CONFIG, 0x18); // 0x18 = 二进制11000，即±2000 // 6. 加速度计量程：±8g MPU6500_WriteReg(MPU6500_ACCEL_CONFIG, 0x10); // 0x10 = 二进制10000，即±8g // 7. 关中断，关I2C主接口 MPU6500_WriteReg(MPU6500_INT_PIN_CFG, 0x02); MPU6500_WriteReg(MPU6500_INT_ENABLE, 0x00); }这里的寄存器地址我在代码里做了宏定义，关键几个如下：
- MPU6500_PWR_MGMT_1 = 0x6B
- MPU6500_SMPLRT_DIV = 0x19
- MPU6500_CONFIG = 0x1A
- MPU6500_GYRO_CONFIG = 0x1B
- MPU6500_ACCEL_CONFIG = 0x1C
- MPU6500_ACCEL_XOUT_H = 0x3B
- MPU6500_WHO_AM_I = 0x75
关于DLPF设置，我需要多说一句。DLPF（数字低通滤波器）的截止频率直接影响姿态解算的平滑度。截止频率太低，响应慢，姿态滞后明显；太高，又会有高频噪声串进来。我做平衡车项目的时候把DLPF设为0x03，对应44Hz截止频率，配合200Hz采样率，效果比较均衡。如果你做的是动作捕捉这类对延迟敏感的应用，可以选更高的截止频率，比如0x02对应的94Hz，然后在软件层面再做一次滑动滤波。
2.4 SPI读写设备寄存器的陷阱
SPI读写MPU6500时，寄存器地址的最高位有特别含义：最高位为1表示读操作，为0表示写操作。比如要读ACCEL_XOUT_H（地址0x3B），发送的第一个字节必须是0x3B | 0x80 = 0xBB；如果忘了加0x80，芯片会把这次操作当成写操作，读回来的数据就是错的。这个细节我在移植代码时反反复复出现过，其实是SPI协议的基本规则，但特别容易忽略。
读多字节数据时，CS要保持低电平，连续发送地址之后，主机时钟每来一个边沿，从机就输出一个字节数据。以读加速度计和陀螺仪全部6轴数据为例：
uint8_t buf[14]; MPU6500_CS_LOW(); SPI_ReadWriteByte(0x3B | 0x80); // 发送起始寄存器地址（读操作） for (int i = 0; i < 14; i++) { buf[i] = SPI_ReadWriteByte(0x00); // 主机发0x00提供时钟，读回从机数据 } MPU6500_CS_HIGH();这个14字节依次是ACCEL_XOUT_H、ACCEL_XOUT_L、ACCEL_YOUT_H、ACCEL_YOUT_L、ACCEL_ZOUT_H、ACCEL_ZOUT_L、TEMP_OUT_H、TEMP_OUT_L、GYRO_XOUT_H、GYRO_XOUT_L、GYRO_YOUT_H、GYRO_YOUT_L、GYRO_ZOUT_H、GYRO_ZOUT_L。它的排列顺序是加速度计XYZ、温度、陀螺仪XYZ，特别注意陀螺仪在温度之后，不是加速度计之后直接接陀螺仪，很多人取数据时容易错位。
CS片选操作里也有讲究：每次读操作结束后，CS必须拉高，否则MPU6500会认为传输还没结束，下次操作就会错乱。如果用了硬件NSS，容易遇到NSS控制时机不对的问题，所以在初始化里把NSS配置成软件模式（SPI_NSS_Soft），然后用GPIO手动控制CS。
3. 原始数据读取、单位换算与校准预处理
3.1 量程选择与原始数据的单位换算
初始化里我配的陀螺仪量程是±2000dps，加速度计量程是±8g。为什么选这两个量程？±2000dps适合大动态场景，比如四轴飞行器的快速翻滚；±8g能覆盖大多数运动场景，同时分辨率也还行。如果你做的是静态倾角测量，建议加速度计量程改成±2g，分辨率更高，因为静态姿态下加速度值通常就在±1g附近，±2g量程下每LSB对应的加速度更小，读数更精细。
MPU6500的原始数据是16位有符号数，对应的换算公式是：
- 陀螺仪角速度（dps）= 原始值 / 灵敏度
- 加速度（g）= 原始值 / 灵敏度
不同量程下灵敏度如下：
| 陀螺仪量程(dps) | 灵敏度(LSB/dps) | 加速度计量程(g) | 灵敏度(LSB/g) |
|---|---|---|---|
| ±250 | 131 | ±2 | 16384 |
| ±500 | 65.5 | ±4 | 8192 |
| ±1000 | 32.8 | ±8 | 4096 |
| ±2000 | 16.4 | ±16 | 2048 |
拿±2000dps来说，灵敏度是16.4 LSB/dps，原始值如果是1640，那角速度就是1640/16.4=100dps。代码里我直接用浮点除法，牺牲了一点点速度换来了可读性。如果你对性能要求非常高，可以换定点数，但在F103上跑浮点运算其实也够快，姿态解算整体在1kHz更新率下CPU占用依然不高。
3.2 陀螺零偏校准的实践方法
陀螺仪零偏是姿态解算里最让人头疼的问题。所谓零偏，就是静止时陀螺仪输出不为0，比如X轴静止时输出可能稳定在-20左右，对应-1.2dps。如果不去除零偏，积分几分钟后，姿态角就飘得没边了。MPU6500有个内部自动校准机制，但实际效果一般，我通常在代码里做软件校准。
校准方法很简单：设备上电后保持静止，采集N个样本（我取500个，大约2.5秒），去掉前50个数据不统计（等待启动稳定），然后求平均值。这个平均值就是零偏：
float gyro_offset[3] = {0, 0, 0}; for (int i = 0; i < 500; i++) { MPU6500_ReadGyro(gyro_raw); if (i > 50) { gyro_offset[0] += gyro_raw[0]; gyro_offset[1] += gyro_raw[1]; gyro_offset[2] += gyro_raw[2]; } } gyro_offset[0] /= 450; gyro_offset[1] /= 450; gyro_offset[2] /= 450;校准要特别注意两点：一是设备必须真正水平静止，不能有人为晃动；二是校准过程中如果板子有电机振动，零偏会测不准。我第二次做四轴的时候没注意，直接把四轴放在桌上校准，电机还没转，但是桨叶有点晃动，导致零偏里混入了低频振动分量，悬停时姿态飘得厉害。
3.3 数据预处理顺序
拿到原始数据之后，不是直接扔进姿态解算。我习惯的预处理顺序是：
- 读取原始数据。
- 判断数据有效性（可选的，检查传感器状态寄存器）。
- 去掉陀螺仪零偏。
- 单位换算成物理量（dps和g）。
- 对加速度计做一阶低通滤波，对陀螺仪不做额外滤波（DLPF已经在硬件上做了）。
加速度计为什么还要软件滤波？因为加速度计对运动加速度很敏感，而姿态解算里加速度计是提供重力方向参考的，如果运动加速度太大，重力向量会被干扰，姿态角就会跳变。平衡车原地快速前后晃动时，加速度计输出里除了重力分量，还有很大的运动加速度分量，这会让roll/pitch角度瞬间跳好几度。我常用的简单一阶滤波：
acc_lpf = acc_lpf * 0.9f + acc_new * 0.1f;这个滤波系数不是固定的，系数越小越平滑，但滞后越大。平衡车我用的0.9，如果你调参觉得角度滞后明显，可以试试0.8或者0.7，找到噪声和滞后之间的平衡点。
4. 四元数姿态解算核心实现
4.1 为什么要用四元数表示姿态
姿态解算输出的是物体相对大地坐标系的旋转关系，最直观的表达方式是欧拉角（roll、pitch、yaw）。但欧拉角有个致命缺陷——万向锁。当pitch接近±90°时，roll和yaw的旋转轴重合，系统丢失一个自由度，表现为角度跳变。四元数用4个数字表示旋转，没有万向锁问题，而且计算时只需要乘法和加法，不需要三角函数，非常适合嵌入式。
四元数的形式是q = w + xi + yj + zk，也可以写成(w, x, y, z)。其中w是标量部分，表示旋转角的一半的余弦，x、y、z是向量部分，表示旋转轴。初始状态我们认为设备水平放置，姿态四元数就是q = (1, 0, 0, 0)。最终把四元数转成欧拉角时，用下面的公式：
roll = atan2f(2.0f * (w*x + y*z), 1.0f - 2.0f * (x*x + y*y)); pitch = asinf(2.0f * (w*y - z*x)); yaw = atan2f(2.0f * (w*z + x*y), 1.0f - 2.0f * (y*y + z*z));注意，pitch的asinf函数在±90°附近会有数值不稳定问题，这是欧拉角本身的性质决定的，四元数虽然能避免万向锁，但你最终输出欧拉角时还是绕不过去。如果你的应用确认pitch不会接近90°，用这个公式没问题；如果有可能，建议保持四元数输出，在更高层去处理。
4.2 互补滤波姿态解算的数学原理
姿态解算的核心问题：陀螺仪积分漂移，加速度计噪声大且受运动加速度干扰。陀螺仪短期精确但长期漂移，加速度计长期稳定（重力方向恒定）但短期毛刺大。互补滤波的想法就是：用加速度计修正陀螺仪积分的漂移，把两者的优点结合起来。
具体做法是先计算理论重力向量在机体坐标系下的表示。姿态四元数q = (w, x, y, z)把大地坐标系重力向量(0, 0, 1)旋转到机体坐标系，得到理论重力向量：
vx = 2.0f * (x*z - w*y); vy = 2.0f * (w*x + y*z); vz = w*w - x*x - y*y + z*z;这个vx、vy、vz就是"根据当前姿态推算出来的重力方向"。同时加速度计直接测量到的重力向量是ax、ay、az（归一化后）。理论上这两个向量应该一致，如果不一致，它们的叉积就代表姿态误差。叉积的大小正比于角度误差，所以可以用叉积做比例积分修正。
修正量加到陀螺仪角速度上：
omega_x += kp * error_x + ki * error_int_x; omega_y += kp * error_y + ki * error_int_y; omega_z += kp * error_z + ki * error_int_z;然后四元数通过角速度积分更新：
w += 0.5f * (-x*omega_x - y*omega_y - z*omega_z) * dt; x += 0.5f * ( w*omega_x - z*omega_y + y*omega_z) * dt; y += 0.5f * ( z*omega_x + w*omega_y - x*omega_z) * dt; z += 0.5f * (-y*omega_x + x*omega_y + w*omega_z) * dt;最后归一化四元数，防止舍入误差导致模长漂移。
4.3 Mahony算法完整实现
这份代码包里我用的是Mahony互补滤波算法，它代码量小、调参简单、在STM32F103上跑毫无压力。完整的解算函数如下：
void Mahony_Update(float* q, float gx, float gy, float gz, float ax, float ay, float az, float dt) { float norm; float vx, vy, vz; float ex, ey, ez; float error_int_x = 0, error_int_y = 0, error_int_z = 0; static float halfKp = 1.0f; static float halfKi = 0.005f; // 加速度计归一化 norm = sqrtf(ax*ax + ay*ay + az*az); ax /= norm; ay /= norm; az /= norm; // 计算理论重力向量 vx = 2.0f * (q[1]*q[3] - q[0]*q[2]); vy = 2.0f * (q[0]*q[1] + q[2]*q[3]); vz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]; // 叉积求误差 ex = ay * vz - az * vy; ey = az * vx - ax * vz; ez = ax * vy - ay * vx; // 积分项 if (halfKi > 0.0f) { error_int_x += halfKi * ex * dt; error_int_y += halfKi * ey * dt; error_int_z += halfKi * ez * dt; gx += error_int_x; gy += error_int_y; gz += error_int_z; } // 比例项修正陀螺仪 gx += halfKp * ex; gy += halfKp * ey; gz += halfKp * ez; // 四元数更新（一阶龙格-库塔法） float w = q[0], x = q[1], y = q[2], z = q[3]; q[0] = w - 0.5f * (x*gx + y*gy + z*gz) * dt; q[1] = x + 0.5f * (w*gx - z*gy + y*gz) * dt; q[2] = y + 0.5f * (z*gx + w*gy - x*gz) * dt; q[3] = z + 0.5f * (-y*gx + x*gy + w*gz) * dt; // 归一化 norm = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); q[0] /= norm; q[1] /= norm; q[2] /= norm; q[3] /= norm; }两个关键参数kp和ki的调参经验：
- kp越大，算法对加速度计跟随越快，能更快纠正陀螺仪漂移，但太大容易把运动加速度当作姿态误差，导致角度抖动。
- ki主要消除稳态漂移，如果静止时yaw缓慢变化，适当加大ki；但如果加速度计受到持续干扰，ki太大会让姿态持续偏转。
我自己做四轴时的调试顺序：先把ki设为0，只调kp。初始值1.0（代码里的halfKp其实是kp/2），观察静止时的roll和pitch波动。如果波动在±0.5度以内，说明kp合适；波动太大就减小kp。然后从小往大加ki，直到静止时yaw漂移在几分钟内不超过1度为止。这个方法屡试不爽。
4.4 初始四元数和欧拉旋转顺序的坑
很多人在姿态解算的初始条件上栽跟头。代码里初始四元数如果设成(1,0,0,0)，意味着设备静置时的姿态就是欧拉角(0,0,0)。但如果你的设备安装方式是竖起来的，或者电路板不是水平安装，就得先根据初始欧拉角计算初始四元数，再送入解算循环。
欧拉旋转顺序也很关键。同一组欧拉角，按不同的旋转顺序（ZYX、ZXY等）会得到不同的四元数。在姿态解算里，通常默认的旋转顺序是ZYX，也就是先绕Z轴（yaw），再绕Y轴（pitch），最后绕X轴（roll）。我的代码里欧拉角转四元数的公式是按这个顺序来的，如果你的业务层用了别的顺序，角度输出会乱。
提供一个简单的欧拉角转四元数函数，适用于初始姿态校准：
void Euler_To_Quaternion(float roll, float pitch, float yaw, float* q) { float cr = cosf(roll * 0.5f); float sr = sinf(roll * 0.5f); float cp = cosf(pitch * 0.5f); float sp = sinf(pitch * 0.5f); float cy = cosf(yaw * 0.5f); float sy = sinf(yaw * 0.5f); q[0] = cr*cp*cy + sr*sp*sy; q[1] = sr*cp*cy - cr*sp*sy; q[2] = cr*sp*cy + sr*cp*sy; q[3] = cr*cp*sy - sr*sp*cy; }说实话这个场景用得不多，大多数项目都是水平安装，初始化四元数直接(1,0,0,0)就行。但如果做手持设备、或者设备有一个固定安装倾角，这段代码就是救命的。
5. 常见问题与排查技巧实录
5.1 SPI读WHO_AM_I失败：返回0x00或0xFF
这是最多人卡住的地方，也是最容易排查的。如果读WHO_AM_I返回0x00，大概率是CS片选时序不对，或者MPU6500根本没有进入SPI模式。先确认CS引脚是否由GPIO控制，上电时CS要先保持高，初始化SPI后再拉低开始通信。如果CS直接接地了，我前面说过，芯片会直接选择I2C模式，SPI永远不通。
返回0xFF则是SPI完全没读到数据，从这几个方向排查：
- SCLK、MOSI、MISO三根线是否接对。重点检查MISO，因为主机TXD和RXD容易在接线时忽略，很多模块上MISO标成ADO，别以为那是地址引脚。
- SPI模式是否匹配。把CPOL/CPHA四个组合都试一遍，能稳定读到0x70的就是正确模式。
- 检查SPI外设时钟是否使能。STM32F103上SPI1挂在APB2总线（72MHz），SPI2挂在APB1总线（36MHz），使能错误会直接导致SCLK无输出。
- 用示波器量SCLK引脚，确认有波形。如果引脚波形正常但仍读不到数据，再量MISO。
5.2 数据读出来了，但静止时角度漂移很大
角度漂移通常分两类。一类是roll和pitch缓慢漂移，一般是加速度计噪声或者kp太小，无法及时纠正陀螺仪零偏。先检查陀螺仪零偏是否去除干净，特别是温度变化导致零偏变化的情况。MPU6500的零偏受温度影响比较大，如果你从室内拿到室外，温差超过十几度，静态零偏会明显变化。解决方法是开机后自适应校准，或者加温度补偿，第二个方案复杂，我一般用开机自动校准。
另一类是yaw持续漂移。yaw只依靠陀螺仪积分，没有绝对的参考源（除非加了磁力计），所以yaw漂移是六轴传感器的固有问题。互补滤波里yaw的修正能力很弱，只能靠kp、ki来抑制部分漂移。如果你的应用对yaw精度要求高，必须加磁力计做九轴融合，否则无解。
5.3 动态情况下姿态角跳变
场景：用力快速晃动板子，roll/pitch角度突然跳好几度，然后慢悠悠恢复。这是典型的运动加速度干扰问题。加速计在快速加速时，测到的不是重力方向，而是"重力+运动加速度"的合向量，合向量方向当然不等于重力方向，于是算法就受到了误导。
解决方案有三个方向：
- 降低加速度计的权重，也就是减小kp。但kp太小会导致静止时姿态收敛慢。
- 检测到较大运动加速度时，自动减小kp。可以根据加速度计模长判断，如果|√(ax²+ay²+az²)-1g|超过某个阈值，就认为是剧烈运动，此时降低加速度计的修正权重。
- 硬件上优化DLPF滤波参数，进一步滤除高频运动分量。
我自己的平衡车项目里用了方案2，效果很明显。阈值和kp衰减系数需要在实车上调，但思路是通用的。
5.4 STM32F103 PA9/PA10和CAN通信相关小插曲
代码包里除了MPU6500驱动，有些人还提到STM32F103的CAN通讯例程和PA9、PA10的串口问题。我顺手提一句，PA9是USART1_TX，PA10是USART1_RX，这是F103最常用的调试串口。而CAN通讯使用的是PA11（CAN_RX）和PA12（CAN_TX），别和串口搞混了。在做姿态传感器和上位机联调时，经常需要同时使用串口和CAN，如果板子上没有多余串口，建议直接用USART1做调试输出，CAN独立走，这样互不干扰。
5.5 MATLAB上位机联调与数据验证
代码跑通了之后，别急着写控制算法，先验证姿态解算是否正确。我的习惯是把四元数和欧拉角通过串口发到上位机，在电脑上用MATLAB脚本实时绘制姿态变化曲线。具体做法是：STM32每10ms发一帧数据，格式用简单的文本协议，比如roll,pitch,yaw\n，MATLAB那边用serialport读取并画图。
在MATLAB里验证的方法很简单：把板子水平放置，roll/pitch应该在0度附近波动；把板子立起来绕Y轴转90度，pitch应该输出接近90；快速旋转后静态放置，角度应该慢慢回到准确位置。如果这些动作都符合预期，说明姿态解算的框架是可靠的。我后来还写了一个简单的3D姿态显示脚本，利用四元数直接更新3D模型，效果直观得多，也更容易发现解算中的异常跳变。
5.6 从MPU6050代码迁移到MPU6500的注意事项
如果你手里已经有MPU6050的驱动代码，迁移到MPU6500时别只改寄存器地址，还有几个隐藏差异：
- MPU6500上电后默认是I2C模式？不对，上电时MPU6500会检测CS脚电平，CS高则I2C。所以必须把CS脚接给GPIO控制，不能像I2C那版一样直接拉高到VCC。
- MPU6500的WHO_AM_I虽然寄存器地址相同，但返回值不同。MPU6050返回0x68，MPU6500返回0x70。判断ID的时候别写死0x68，我见过很多人因为这个卡住。
- 部分MPU6500模块的I2C上拉电阻默认焊在板上，即使你用SPI，SDA和SCL上的上拉电阻也不会影响SPI工作，可以不用管。
6. 后续扩展与代码优化方向
代码包跑通之后，可以往这些方向继续深挖。如果要做更高频率的姿态更新，可以把SPI速率提升到4MHz或8MHz，同时打开STM32的SPI DMA，把读取和计算完全交给硬件，CPU只负责解算。实测用DMA搬运14字节数据，CPU负载能再降低不少。
如果产品需要长时间运行，建议把姿态解算频率固定下来，不要用延时循环凑时间，改用定时器中断，每5ms读一次数据并解算。固定时间步长对四元数积分的精度有很大影响，时间跳变会引起角速度积分误差。
再进一步，如果项目对yaw精度要求高，可以考虑外接磁力计（比如HMC5883L或者AK8963），把数据融合扩展成九轴版本。算法上可以用Madgwick的九轴更新公式，在Mahony基础上增加磁力计修正项，代码改动量不算大，但能显著改善yaw的长期稳定性。
从这份代码包出发，还能把姿态数据通过CAN总线发给主控制器，配合无线模块做远程状态监控。STM32F103的CAN外设和USART都可以同时工作，整个系统的实时性完全够用。
最后再分享一个我实际操作中的体会：姿态解算这套东西，代码本身并不复杂，真正复杂的是调试。很多时候你觉得算法有问题，实际上是传感器数据没洗干净、SPI时序有毛刺、或者硬件安装有偏差。建议遇到问题时先用串口把最原始的传感器数据打出来，自己眼睛看一遍，确认数据合理了，再去怀疑算法。一步一步排查，远比对着代码瞎改强。
本文还有配套的精品资源，点击获取
