简介面向STM32嵌入式开发与四旋翼无人机入门者的完整项目资料围绕MPU6050姿态解算与ANOTC匿名地面站V5联调展开涵盖飞控基础知识文档、IOC工程配置、外设驱动与核心控制算法适合想从硬件到上位机贯通学习无人机控制系统的开发者。压缩包共1033个文件以c/h源码为主并包含s汇编、o目标文件、uvprojx工程文件、docx基础文档及可直接运行的上位机exe等整体约41.86MB目录结构覆盖Core、MATH、HARDWARE等模块便于按功能快速定位。目前已有1400人浏览学习。资源内除完整工程与驱动外还提供卡尔曼/互补滤波等姿态解算思路、串口通信代码和飞控调参参考可帮助读者理解俯仰、翻滚、偏航角解算流程并借助匿名地面站观测实时状态是一份兼具理论讲解与实战代码的实践案例。1. 从 MPU6050 到 ANOTC 地面站一次把姿态解算链路打通的项目很多人做四旋翼是从“能转”开始的但真正让飞机稳住的是姿态数据。STM32 读 MPU6050 的原始加速度和角速度并不难难在怎么把这些带噪声、带零漂的测量值变成欧拉角再送到地面站可视化。这个标题里的 ANOTC 匿名地面站 V5 是匿名科创开源的 PC 端上位机它的协议格式恰好覆盖了姿态、油门、GPS 等多类数据很多 STM32 飞控项目都用它来做调试和验证。本文会把姿态解算的数学基础、四元数与欧拉角的关系、STM32 的 I2C 读取流程、互补滤波与 Mahony 算法的取舍、以及 ANOTC 地面站的具体通信协议一次讲透并给出可以直接下载到开发板里跑的代码框架。适合刚把 C 语言和 STM32 外设玩熟、准备把第一个自旋翼飞机调稳的人也适合已经飞起来但不清楚姿态数据在 MCU 里如何流转的开发者。2. MPU6050 数据读取与 STM32 底层驱动2.1 为什么主流飞控方案选 MPU6050 而不是独立陀螺仪加加速度计MPU6050 内部集成了一个三轴陀螺仪和一个三轴加速度计采用 I2C 接口输出数据外部只需要 4 根线VCC、GND、SCL、SDA。它自带数字运动处理器 DMP可以直接输出四元数省去主控解算的算力。但实际做飞控时很多人宁愿只拿它当原始传感器用让 STM32 自己跑解算。原因是 DMP 的输出频率固定且无法针对特定机架的震动特性做调参而自研解算可以将陀螺仪数据与加速度计数据按权重融合配合 PID 得到更顺滑的角度环输入。MPU6050 的量程配置也是刚入门容易忽略的点。陀螺仪可配 ±250、±500、±1000、±2000 °/s加速度计可配 ±2g、±4g、±8g、±16g。四轴飞行时自旋角速度一般不会超过 500 °/s但剧烈翻滚或快速偏航时接近 2000 °/s常见做法是把陀螺仪量程设为 ±2000 °/s、加速度计量程设为 ±8g以保证不削顶。量程越大分辨率越低但飞控场景宁可牺牲一点分辨率换取满量程可靠性。2.2 STM32 的 I2C 时序与 MPU6050 寄存器映射把 MPU6050 的 AD0 引脚接地时I2C 从机地址是 0x68接高电平时是 0x69。一个 I2C 总线上可以挂两个 MPU6050这在双 IMU 冗余设计的飞控里很常见。使用 STM32 的硬件 I2C 时需要处理总线空闲检测、ACK 时序和错误恢复而很多人直接用 GPIO 模拟 I2C原因是时序完全可控且代码可移植到任意 MCU。以 STM32F103 标准库为例初始化 MPU6050 的核心步骤是#define MPU6050_ADDR 0xD0 // 左移一位后的地址0x681 #define PWR_MGMT_1 0x6B #define SMPLRT_DIV 0x19 #define CONFIG 0x1A #define GYRO_CONFIG 0x1B #define ACCEL_CONFIG 0x1C #define ACCEL_XOUT_H 0x3B void mpu6050_init(void) { uint8_t temp; // 唤醒芯片清除 SLEEP 位 temp I2C_ReadByte(MPU6050_ADDR, PWR_MGMT_1); temp ~0x40; I2C_WriteByte(MPU6050_ADDR, PWR_MGMT_1, temp); // 采样率 陀螺仪输出频率 / (1 SMPLRT_DIV) I2C_WriteByte(MPU6050_ADDR, SMPLRT_DIV, 0x07); // 数字低通滤波 DLPF 设置为 0x06对应 5Hz 带宽适合静态解算 I2C_WriteByte(MPU6050_ADDR, CONFIG, 0x06); // 陀螺仪 ±2000 °/s满量程对应 16.4 LSB/(°/s) I2C_WriteByte(MPU6050_ADDR, GYRO_CONFIG, 0x18); // 加速度计 ±8g满量程对应 4096 LSB/g I2C_WriteByte(MPU6050_ADDR, ACCEL_CONFIG, 0x10); }这段代码里I2C_WriteByte和I2C_ReadByte是自封装的底层函数。注意 PWR_MGMT_1 寄存器上电默认是 0x40即休眠状态如果初始化后读到的数据全是 0xFF优先检查这个寄存器是否被正确写入。SMPLRT_DIV 设为 7 意味着采样率是 1kHz这个值不是固定死的如果后续解算频率是 500Hz可以设 1如果主控还需要做电机控制、串口通信等任务设 7 可以让 I2C 中断更稀疏。2.2.1 连续读取 14 字节数据块读取姿态数据时建议一次性从 ACCEL_XOUT_H 开始连续读 14 字节而不是逐字节读。MPU6050 的寄存器地址是递增的连续读可以保证同一个采样时刻的加速度和角速度数据是原子性的避免先读加速度、再读角速度时中间发生了新的采样更新。void mpu6050_read_raw(int16_t *accel, int16_t *gyro) { uint8_t buf[14]; I2C_ReadBytes(MPU6050_ADDR, ACCEL_XOUT_H, buf, 14); // 高字节在前大端需要拼成有符号数 accel[0] (int16_t)((buf[0] 8) | buf[1]); // X 轴加速度 accel[1] (int16_t)((buf[2] 8) | buf[3]); // Y 轴加速度 accel[2] (int16_t)((buf[4] 8) | buf[5]); // Z 轴加速度 gyro[0] (int16_t)((buf[8] 8) | buf[9]); // X 轴角速度 gyro[1] (int16_t)((buf[10] 8) | buf[11]); // Y 轴角速度 gyro[2] (int16_t)((buf[12] 8) | buf[13]); // Z 轴角速度 }加速度计的第 3.3 节里提到过温度寄存器在 buf[6] 和 buf[7]这里没用上。如果发现加速度计 Z 轴在水平放置时读数不是 4096 附近而接近 0多半是芯片被斜焊或者坐标系定义改了。其他情况要先检查 I2C 数据线是否加上拉电阻MPU6050 模块通常板载 4.7kΩ 上拉飞控主板上只接 SDA 和 SCL 通信线不要额外再加否则会降低信号边沿的翻转速度。3. 四元数与欧拉角姿态解算的数学基础3.1 为什么用四元数而不是欧拉角直接算欧拉角直观但有个致命问题万向节锁。当俯仰角接近 ±90° 时滚转和偏航的旋转轴重合解算会丢失自由度。四旋翼在正常飞行时俯仰角很少超过 60°看起来不触发锁死但一旦做翻跟头等特技动作欧拉角微分方程会出现除零项导致解算发散。四元数用四个数表示刚体旋转没有奇异性且计算量小。一个单位四元数q [w, x, y, z]可以表示绕任意轴旋转任意角度STM32F103 在 72MHz 主频下跑一次四元数更新加归一化只需几十微秒对控制周期毫无压力。四元数到欧拉角的转换公式是// 从 w,x,y,z 计算 rollpitchyaw单位弧度 roll atan2f(2.0f * (w*x y*z), 1.0f - 2.0f * (x*x y*y)); pitch asinf(2.0f * (w*y - x*z)); yaw atan2f(2.0f * (w*z x*y), 1.0f - 2.0f * (y*y z*z));这里 roll 和 yaw 用atan2f保证值域覆盖 -π 到 π而 pitch 用asin结果始终在 -90° 到 90° 之间。3.1.1 四元数的几何含义把四元数理解为“旋转轴加旋转角”更容易调试。比如绕 Z 轴旋转 90°四元数是[0.7071, 0, 0, 0.7071]四个分量符合“角度一半的余弦 轴分量乘以角度一半的正弦”的规律。如果解算结果在静止时 yaw 不归零而是一直漂移先检查是不是陀螺仪零偏没补偿。四元数本身不会自动纠正偏航漂移因为它缺少像加速度计那样的绝对参考。3.2 互补滤波一阶算法里的经典选择互补滤波的原理是陀螺仪动态响应快但有积分漂移加速度计无长期漂移但动态响应慢且有高频噪声。把两者互补起来就是让陀螺仪通过低通滤波后的加速度计来校正。一阶互补滤波公式是angle 0.98 * (angle gyro_rate * dt) 0.02 * accel_angle系数 0.98 和 0.02 表示信任陀螺仪 98%信任加速度计 2%。这个比例可以调但固定系数在小角度和小加速度场景下够用做大机动时加速度计算出的角度会混入运动加速度导致姿态被拉偏。互补滤波的优点是代码极简单适合没有浮点单元的低端 MCU 快速实现缺点是难以处理陀螺仪轴间耦合动态精度不如 Mahony。3.3 Mahony 姿态解算算法飞控项目的主流方案Mahony 算法属于互补滤波的改进版它不在角度域融合而是在四元数域用加速度计和磁力计的测量值构建误差向量通过 PI 控制器修正陀螺仪漂移。该算法需要两个关键输入归一化后的加速度计向量和当前四元数旋转出的重力参考向量。void mahony_update(float gx, float gy, float gz, float ax, float ay, float az, float dt) { float q0 q[0], q1 q[1], q2 q[2], q3 q[3]; float norm; float vx, vy, vz; float ex, ey, ez; float half_t 0.5f * dt; // 加速度计归一化 norm sqrtf(ax*ax ay*ay az*az); if (norm 1e-6f) return; ax / norm; ay / norm; az / norm; // 由当前四元数计算重力方向参考向量 vx 2.0f * (q1*q3 - q0*q2); vy 2.0f * (q0*q1 q2*q3); vz q0*q0 - q1*q1 - q2*q2 q3*q3; // 测量向量与参考向量的叉积作为误差 ex (ay*vz - az*vy); ey (az*vx - ax*vz); ez (ax*vy - ay*vx); // 误差积分项消除陀螺仪常值漂移 integral_fb_x ex * Ki * half_t; integral_fb_y ey * Ki * half_t; integral_fb_z ez * Ki * half_t; // 陀螺仪角速度补偿 gx ex * Kp integral_fb_x; gy ey * Kp integral_fb_y; gz ez * Kp integral_fb_z; // 四元数一阶龙格库塔更新然后归一化 q0 half_t * (-q1*gx - q2*gy - q3*gz); q1 half_t * ( q0*gx q2*gz - q3*gy); q2 half_t * ( q0*gy - q1*gz q3*gx); q3 half_t * ( q0*gz q1*gy - q2*gx); norm sqrtf(q0*q0 q1*q1 q2*q2 q3*q3); q[0] q0/norm; q[1] q1/norm; q[2] q2/norm; q[3] q3/norm; }这里的Kp决定加速度计修正陀螺仪的速度Ki决定对陀螺仪零漂的长期补偿。从静止启动时Kp 过大容易让姿态在高频抖动Kp 过小则动态跟踪慢。我给的默认值是 Kp1.2、Ki0.05适合普通穿越机机架如果发现悬停时姿态呈波浪状来回摆把 Kp 降到 0.8如果推油门瞬间姿态明显滞后把 Kp 提到 2.0。dt是两次更新之间的间隔秒数必须由定时器精确测量不能直接用固定常量否则受中断影响时算法会出问题。4. 用 ANOTC 匿名地面站 V5 把姿态解算结果可视化4.1 ANOTC 地面站的数据协议格式ANOTC 匿名地面站 V5 的串口协议核心是“帧头 长度 功能字 数据 校验和”。官方文档里常用的帧头是0xAA 0xAA但我见过网上流传的一些版本实际用的是0xAA 0x55所以移植前最好确认接收端源码里的宏定义。最常见的姿态数据帧是 DATA_ID_ATTITUDE功能字0x01数据区 6 个 float依次是 roll、pitch、yaw 和它们的速率。typedef struct { uint8_t head[2]; // 0xAA 0xAA uint8_t len; // 数据长度不包含头、长度、功能字和校验 uint8_t funct; // 功能字1 表示姿态 float roll; float pitch; float yaw; float roll_speed; float pitch_speed; float yaw_speed; uint8_t sum; // 校验和从 head[0] 累加到 yaw_speed 末字节 } __packed anotc_atti_t;打包发送时注意STM32 默认的小端模式正好与地面站解析一致float 可以按数组直接 memcpy。校验和计算要包含帧头两字节、长度、功能字和数据漏掉帧头是最常见的错误表现为地面站偶尔收到一帧但马上断线。发送周期建议 50Hz 到 100Hz即每 10ms 到 20ms 发一次太高的频率会占满串口带宽太低则地面站曲线看起来卡顿。4.2 STM32 串口发送姿态数据到地面站发送函数的关键是把浮点数拆成字节不能直接用printf(%f)转字符串那样既慢又会被串口中断打断浮点格式化。私募函数的实现如下void anotc_send_attitude(float roll, float pitch, float yaw) { uint8_t buf[20]; uint8_t sum 0; int i 0; buf[i] 0xAA; buf[i] 0xAA; buf[i] 16; // 3 个 float12 字节 roll_speed 等暂不发送先发 12 buf[i] 0x01; memcpy(buf[i], roll, 4); i 4; memcpy(buf[i], pitch, 4); i 4; memcpy(buf[i], yaw, 4); i 4; for (int j 0; j i; j) { sum buf[j]; } buf[i] sum; // 串口 DMA 发送或直接阻塞发送 HAL_UART_Transmit(huart1, buf, i, 10); }这里暂未发送角速度长度就填 12。如果地面站端需要角速度做微分前馈可以改成 24 字节并把len改成 24校验和累加范围不变。注意 H3 节提到的“先发什么后发什么”必须与地面站的解析顺序一致否则数据显示出来的 roll 和 pitch 会互换。可以在地面站上做“机头朝上倾斜”的快速测试把飞控绕 X 轴旋转观察图形里的飞机是否同步绕 X 轴转动。4.2.1 用 USB-TTL 模块连接并验证ANOTC 地面站通常通过 USB 转串口模块连接飞控。接线顺序是USB-TTL 的 RX 接飞控 TXTX 接飞控 RXGND 共地。很多人喜欢只用一根 TX 线认为只要发数据就够了但 USB-TTL 模块的 RX 悬空时可能造成干扰导致地面站不断报串口错误所以建议三线连接。波特率统一设 115200地面站里选择对应的串口号后点击连接观察“无效帧”计数是否持续增长。若是先检查校验和代码再把串口助手打开看原始字节流确认每一帧是否以AA AA开头。5. 进阶调参与姿态数据验证方法5.1 陀螺仪零偏标定与数据质量判断姿态解算的效果上限由传感器质量决定。陀螺仪静止时输出不为零长时间积分会累积出几度每秒的漂移。简单标定方法是上电后保持静止 2 秒采样 100 次取平均值存下来作为零偏在运行时刻从原始读数中减去。float gyro_offset[3]; for (int i 0; i 100; i) { mpu6050_read_raw(accel, gyro); gyro_offset[0] gyro[0]; gyro_offset[1] gyro[1]; gyro_offset[2] gyro[2]; delay_ms(2); } gyro_offset[0] / 100; gyro_offset[1] / 100; gyro_offset[2] / 100;但零偏随温度变化起飞前陀螺仪内部温度还未稳定刚上电的零偏和飞行几分钟后的零偏可能差别较大。更稳妥的做法是把 Mahony 算法的 Ki 调到 0.08 到 0.1让积分项持续在线估计零漂。离线标定时注意飞机不要放在强振动源旁边否则加速度计采集到的是振动导致地面站上姿态静止时也在 ±0.5° 内波动。5.2 解算频率与控制频率的匹配我见过很多新手把姿态解算放在主循环里和电机控制混在一起。当电机 PWM 频率高、串口打印频繁时主循环周期不稳定Mahony 的dt跳动姿态解算很容易发散。更合理的做法是用定时器触发 1kHz 中断中断里只做陀螺仪和加速度计读取、四元数更新然后将欧拉角放到共享变量里主循环只负责读取并发送地面站。// 定时器中断服务函数1kHz void TIM1_UP_IRQHandler(void) { int16_t accel[3], gyro[3]; float gx_raw[3], ax_raw[3]; static uint32_t last_time; mpu6050_read_raw(accel, gyro); // 转换为物理单位陀螺仪 /16.4加速度 /4096 ax_raw[0] accel[0] / 4096.0f; ax_raw[1] accel[1] / 4096.0f; ax_raw[2] accel[2] / 4096.0f; gx_raw[0] gyro[0] / 16.4f * DEG2RAD; gx_raw[1] gyro[1] / 16.4f * DEG2RAD; gx_raw[2] gyro[2] / 16.4f * DEG2RAD; float dt (HAL_GetTick() - last_time) / 1000.0f; last_time HAL_GetTick(); mahony_update(gx_raw[0], gx_raw[1], gx_raw[2], ax_raw[0], ax_raw[1], ax_raw[2], dt); // 四元数转欧拉角存给主循环 atti_roll atan2f(...); atti_pitch asinf(...); atti_yaw atan2f(...); }如果定时器中断和主循环都在使用q数组需要加临界区保护否则当主循环正在读四元数而中断写入时可能读到新旧混搭的数据。最简单的方法是只在中断里写主循环读取时临时关闭中断读完再打开。飞行控制任务如果跑在 4kHz 或 8kHz姿态解算频率不必一样高通常 1kHz 解算、控制率 1kHz 或 2kHz 就足够更高的频率只会增加微控制器的负载。5.3 验证姿态解算是否正确的三种方法第一种静态水平。把飞控平放地面站显示的 roll 与 pitch 应在 0° 附近yaw 任意。若 roll 恒偏 2°说明加速度计安装有角度误差可以在代码里做安装角补偿。第二种静态旋转。用手缓慢将飞控绕 X 轴转 90°读数应跟上且稳定在 90°停止后 1 秒内回落到 89° 到 91° 之间偏差大的话检查陀螺仪量程或 Kp 大小。第三种动态回零。快速晃动飞控再静止roll 和 pitch 最终应回到 0°yaw 不能回零属于正常现象因为它没有绝对参考。若回不到 0° 而是停在某个固定角度多半是振动导致加速度计被拉偏或 Mahony 的 Ki 太小无法抑制积分饱和。5.4 从地面站数据反推 PID 的可用技巧ANOTC 地面站不仅能看姿态还能通过自带的数据分析窗口做波形对比。把油门从 0 推到 50%再看 pitch 波形可以判断飞机是否有低头或抬头趋势。具体技巧是在遥控器上一个通道叠加一个 5° 的 pitch 阶跃信号同时记录姿态响应如果响应有超调且来回振荡说明 PID 的 P 太大或 D 太小如果响应迟缓且到不了目标角度说明 P 太小。地面站自带的时域波形比肉眼观察飞机更可靠因为高频抖动看不清波形能直观看到 10Hz 以下的低频振荡。发送数据时还可以加入一个自定义状态量通道比如把电池电压或控制量输出发到地面站的“用户数据”功能字里用它的曲线对照姿态角响应这样可以判断是姿态环振荡还是电机响应慢造成的耦合。不同版本的地面站功能字定义略有不同建议先用官方示例的“示波器”功能字测试一帧确认通路后再加上其他传感器数据。本文还有配套的精品资源点击获取
STM32+MPU6050姿态解算：从四元数到ANOTC地面站全链路实战
简介面向STM32嵌入式开发与四旋翼无人机入门者的完整项目资料围绕MPU6050姿态解算与ANOTC匿名地面站V5联调展开涵盖飞控基础知识文档、IOC工程配置、外设驱动与核心控制算法适合想从硬件到上位机贯通学习无人机控制系统的开发者。压缩包共1033个文件以c/h源码为主并包含s汇编、o目标文件、uvprojx工程文件、docx基础文档及可直接运行的上位机exe等整体约41.86MB目录结构覆盖Core、MATH、HARDWARE等模块便于按功能快速定位。目前已有1400人浏览学习。资源内除完整工程与驱动外还提供卡尔曼/互补滤波等姿态解算思路、串口通信代码和飞控调参参考可帮助读者理解俯仰、翻滚、偏航角解算流程并借助匿名地面站观测实时状态是一份兼具理论讲解与实战代码的实践案例。1. 从 MPU6050 到 ANOTC 地面站一次把姿态解算链路打通的项目很多人做四旋翼是从“能转”开始的但真正让飞机稳住的是姿态数据。STM32 读 MPU6050 的原始加速度和角速度并不难难在怎么把这些带噪声、带零漂的测量值变成欧拉角再送到地面站可视化。这个标题里的 ANOTC 匿名地面站 V5 是匿名科创开源的 PC 端上位机它的协议格式恰好覆盖了姿态、油门、GPS 等多类数据很多 STM32 飞控项目都用它来做调试和验证。本文会把姿态解算的数学基础、四元数与欧拉角的关系、STM32 的 I2C 读取流程、互补滤波与 Mahony 算法的取舍、以及 ANOTC 地面站的具体通信协议一次讲透并给出可以直接下载到开发板里跑的代码框架。适合刚把 C 语言和 STM32 外设玩熟、准备把第一个自旋翼飞机调稳的人也适合已经飞起来但不清楚姿态数据在 MCU 里如何流转的开发者。2. MPU6050 数据读取与 STM32 底层驱动2.1 为什么主流飞控方案选 MPU6050 而不是独立陀螺仪加加速度计MPU6050 内部集成了一个三轴陀螺仪和一个三轴加速度计采用 I2C 接口输出数据外部只需要 4 根线VCC、GND、SCL、SDA。它自带数字运动处理器 DMP可以直接输出四元数省去主控解算的算力。但实际做飞控时很多人宁愿只拿它当原始传感器用让 STM32 自己跑解算。原因是 DMP 的输出频率固定且无法针对特定机架的震动特性做调参而自研解算可以将陀螺仪数据与加速度计数据按权重融合配合 PID 得到更顺滑的角度环输入。MPU6050 的量程配置也是刚入门容易忽略的点。陀螺仪可配 ±250、±500、±1000、±2000 °/s加速度计可配 ±2g、±4g、±8g、±16g。四轴飞行时自旋角速度一般不会超过 500 °/s但剧烈翻滚或快速偏航时接近 2000 °/s常见做法是把陀螺仪量程设为 ±2000 °/s、加速度计量程设为 ±8g以保证不削顶。量程越大分辨率越低但飞控场景宁可牺牲一点分辨率换取满量程可靠性。2.2 STM32 的 I2C 时序与 MPU6050 寄存器映射把 MPU6050 的 AD0 引脚接地时I2C 从机地址是 0x68接高电平时是 0x69。一个 I2C 总线上可以挂两个 MPU6050这在双 IMU 冗余设计的飞控里很常见。使用 STM32 的硬件 I2C 时需要处理总线空闲检测、ACK 时序和错误恢复而很多人直接用 GPIO 模拟 I2C原因是时序完全可控且代码可移植到任意 MCU。以 STM32F103 标准库为例初始化 MPU6050 的核心步骤是#define MPU6050_ADDR 0xD0 // 左移一位后的地址0x681 #define PWR_MGMT_1 0x6B #define SMPLRT_DIV 0x19 #define CONFIG 0x1A #define GYRO_CONFIG 0x1B #define ACCEL_CONFIG 0x1C #define ACCEL_XOUT_H 0x3B void mpu6050_init(void) { uint8_t temp; // 唤醒芯片清除 SLEEP 位 temp I2C_ReadByte(MPU6050_ADDR, PWR_MGMT_1); temp ~0x40; I2C_WriteByte(MPU6050_ADDR, PWR_MGMT_1, temp); // 采样率 陀螺仪输出频率 / (1 SMPLRT_DIV) I2C_WriteByte(MPU6050_ADDR, SMPLRT_DIV, 0x07); // 数字低通滤波 DLPF 设置为 0x06对应 5Hz 带宽适合静态解算 I2C_WriteByte(MPU6050_ADDR, CONFIG, 0x06); // 陀螺仪 ±2000 °/s满量程对应 16.4 LSB/(°/s) I2C_WriteByte(MPU6050_ADDR, GYRO_CONFIG, 0x18); // 加速度计 ±8g满量程对应 4096 LSB/g I2C_WriteByte(MPU6050_ADDR, ACCEL_CONFIG, 0x10); }这段代码里I2C_WriteByte和I2C_ReadByte是自封装的底层函数。注意 PWR_MGMT_1 寄存器上电默认是 0x40即休眠状态如果初始化后读到的数据全是 0xFF优先检查这个寄存器是否被正确写入。SMPLRT_DIV 设为 7 意味着采样率是 1kHz这个值不是固定死的如果后续解算频率是 500Hz可以设 1如果主控还需要做电机控制、串口通信等任务设 7 可以让 I2C 中断更稀疏。2.2.1 连续读取 14 字节数据块读取姿态数据时建议一次性从 ACCEL_XOUT_H 开始连续读 14 字节而不是逐字节读。MPU6050 的寄存器地址是递增的连续读可以保证同一个采样时刻的加速度和角速度数据是原子性的避免先读加速度、再读角速度时中间发生了新的采样更新。void mpu6050_read_raw(int16_t *accel, int16_t *gyro) { uint8_t buf[14]; I2C_ReadBytes(MPU6050_ADDR, ACCEL_XOUT_H, buf, 14); // 高字节在前大端需要拼成有符号数 accel[0] (int16_t)((buf[0] 8) | buf[1]); // X 轴加速度 accel[1] (int16_t)((buf[2] 8) | buf[3]); // Y 轴加速度 accel[2] (int16_t)((buf[4] 8) | buf[5]); // Z 轴加速度 gyro[0] (int16_t)((buf[8] 8) | buf[9]); // X 轴角速度 gyro[1] (int16_t)((buf[10] 8) | buf[11]); // Y 轴角速度 gyro[2] (int16_t)((buf[12] 8) | buf[13]); // Z 轴角速度 }加速度计的第 3.3 节里提到过温度寄存器在 buf[6] 和 buf[7]这里没用上。如果发现加速度计 Z 轴在水平放置时读数不是 4096 附近而接近 0多半是芯片被斜焊或者坐标系定义改了。其他情况要先检查 I2C 数据线是否加上拉电阻MPU6050 模块通常板载 4.7kΩ 上拉飞控主板上只接 SDA 和 SCL 通信线不要额外再加否则会降低信号边沿的翻转速度。3. 四元数与欧拉角姿态解算的数学基础3.1 为什么用四元数而不是欧拉角直接算欧拉角直观但有个致命问题万向节锁。当俯仰角接近 ±90° 时滚转和偏航的旋转轴重合解算会丢失自由度。四旋翼在正常飞行时俯仰角很少超过 60°看起来不触发锁死但一旦做翻跟头等特技动作欧拉角微分方程会出现除零项导致解算发散。四元数用四个数表示刚体旋转没有奇异性且计算量小。一个单位四元数q [w, x, y, z]可以表示绕任意轴旋转任意角度STM32F103 在 72MHz 主频下跑一次四元数更新加归一化只需几十微秒对控制周期毫无压力。四元数到欧拉角的转换公式是// 从 w,x,y,z 计算 rollpitchyaw单位弧度 roll atan2f(2.0f * (w*x y*z), 1.0f - 2.0f * (x*x y*y)); pitch asinf(2.0f * (w*y - x*z)); yaw atan2f(2.0f * (w*z x*y), 1.0f - 2.0f * (y*y z*z));这里 roll 和 yaw 用atan2f保证值域覆盖 -π 到 π而 pitch 用asin结果始终在 -90° 到 90° 之间。3.1.1 四元数的几何含义把四元数理解为“旋转轴加旋转角”更容易调试。比如绕 Z 轴旋转 90°四元数是[0.7071, 0, 0, 0.7071]四个分量符合“角度一半的余弦 轴分量乘以角度一半的正弦”的规律。如果解算结果在静止时 yaw 不归零而是一直漂移先检查是不是陀螺仪零偏没补偿。四元数本身不会自动纠正偏航漂移因为它缺少像加速度计那样的绝对参考。3.2 互补滤波一阶算法里的经典选择互补滤波的原理是陀螺仪动态响应快但有积分漂移加速度计无长期漂移但动态响应慢且有高频噪声。把两者互补起来就是让陀螺仪通过低通滤波后的加速度计来校正。一阶互补滤波公式是angle 0.98 * (angle gyro_rate * dt) 0.02 * accel_angle系数 0.98 和 0.02 表示信任陀螺仪 98%信任加速度计 2%。这个比例可以调但固定系数在小角度和小加速度场景下够用做大机动时加速度计算出的角度会混入运动加速度导致姿态被拉偏。互补滤波的优点是代码极简单适合没有浮点单元的低端 MCU 快速实现缺点是难以处理陀螺仪轴间耦合动态精度不如 Mahony。3.3 Mahony 姿态解算算法飞控项目的主流方案Mahony 算法属于互补滤波的改进版它不在角度域融合而是在四元数域用加速度计和磁力计的测量值构建误差向量通过 PI 控制器修正陀螺仪漂移。该算法需要两个关键输入归一化后的加速度计向量和当前四元数旋转出的重力参考向量。void mahony_update(float gx, float gy, float gz, float ax, float ay, float az, float dt) { float q0 q[0], q1 q[1], q2 q[2], q3 q[3]; float norm; float vx, vy, vz; float ex, ey, ez; float half_t 0.5f * dt; // 加速度计归一化 norm sqrtf(ax*ax ay*ay az*az); if (norm 1e-6f) return; ax / norm; ay / norm; az / norm; // 由当前四元数计算重力方向参考向量 vx 2.0f * (q1*q3 - q0*q2); vy 2.0f * (q0*q1 q2*q3); vz q0*q0 - q1*q1 - q2*q2 q3*q3; // 测量向量与参考向量的叉积作为误差 ex (ay*vz - az*vy); ey (az*vx - ax*vz); ez (ax*vy - ay*vx); // 误差积分项消除陀螺仪常值漂移 integral_fb_x ex * Ki * half_t; integral_fb_y ey * Ki * half_t; integral_fb_z ez * Ki * half_t; // 陀螺仪角速度补偿 gx ex * Kp integral_fb_x; gy ey * Kp integral_fb_y; gz ez * Kp integral_fb_z; // 四元数一阶龙格库塔更新然后归一化 q0 half_t * (-q1*gx - q2*gy - q3*gz); q1 half_t * ( q0*gx q2*gz - q3*gy); q2 half_t * ( q0*gy - q1*gz q3*gx); q3 half_t * ( q0*gz q1*gy - q2*gx); norm sqrtf(q0*q0 q1*q1 q2*q2 q3*q3); q[0] q0/norm; q[1] q1/norm; q[2] q2/norm; q[3] q3/norm; }这里的Kp决定加速度计修正陀螺仪的速度Ki决定对陀螺仪零漂的长期补偿。从静止启动时Kp 过大容易让姿态在高频抖动Kp 过小则动态跟踪慢。我给的默认值是 Kp1.2、Ki0.05适合普通穿越机机架如果发现悬停时姿态呈波浪状来回摆把 Kp 降到 0.8如果推油门瞬间姿态明显滞后把 Kp 提到 2.0。dt是两次更新之间的间隔秒数必须由定时器精确测量不能直接用固定常量否则受中断影响时算法会出问题。4. 用 ANOTC 匿名地面站 V5 把姿态解算结果可视化4.1 ANOTC 地面站的数据协议格式ANOTC 匿名地面站 V5 的串口协议核心是“帧头 长度 功能字 数据 校验和”。官方文档里常用的帧头是0xAA 0xAA但我见过网上流传的一些版本实际用的是0xAA 0x55所以移植前最好确认接收端源码里的宏定义。最常见的姿态数据帧是 DATA_ID_ATTITUDE功能字0x01数据区 6 个 float依次是 roll、pitch、yaw 和它们的速率。typedef struct { uint8_t head[2]; // 0xAA 0xAA uint8_t len; // 数据长度不包含头、长度、功能字和校验 uint8_t funct; // 功能字1 表示姿态 float roll; float pitch; float yaw; float roll_speed; float pitch_speed; float yaw_speed; uint8_t sum; // 校验和从 head[0] 累加到 yaw_speed 末字节 } __packed anotc_atti_t;打包发送时注意STM32 默认的小端模式正好与地面站解析一致float 可以按数组直接 memcpy。校验和计算要包含帧头两字节、长度、功能字和数据漏掉帧头是最常见的错误表现为地面站偶尔收到一帧但马上断线。发送周期建议 50Hz 到 100Hz即每 10ms 到 20ms 发一次太高的频率会占满串口带宽太低则地面站曲线看起来卡顿。4.2 STM32 串口发送姿态数据到地面站发送函数的关键是把浮点数拆成字节不能直接用printf(%f)转字符串那样既慢又会被串口中断打断浮点格式化。私募函数的实现如下void anotc_send_attitude(float roll, float pitch, float yaw) { uint8_t buf[20]; uint8_t sum 0; int i 0; buf[i] 0xAA; buf[i] 0xAA; buf[i] 16; // 3 个 float12 字节 roll_speed 等暂不发送先发 12 buf[i] 0x01; memcpy(buf[i], roll, 4); i 4; memcpy(buf[i], pitch, 4); i 4; memcpy(buf[i], yaw, 4); i 4; for (int j 0; j i; j) { sum buf[j]; } buf[i] sum; // 串口 DMA 发送或直接阻塞发送 HAL_UART_Transmit(huart1, buf, i, 10); }这里暂未发送角速度长度就填 12。如果地面站端需要角速度做微分前馈可以改成 24 字节并把len改成 24校验和累加范围不变。注意 H3 节提到的“先发什么后发什么”必须与地面站的解析顺序一致否则数据显示出来的 roll 和 pitch 会互换。可以在地面站上做“机头朝上倾斜”的快速测试把飞控绕 X 轴旋转观察图形里的飞机是否同步绕 X 轴转动。4.2.1 用 USB-TTL 模块连接并验证ANOTC 地面站通常通过 USB 转串口模块连接飞控。接线顺序是USB-TTL 的 RX 接飞控 TXTX 接飞控 RXGND 共地。很多人喜欢只用一根 TX 线认为只要发数据就够了但 USB-TTL 模块的 RX 悬空时可能造成干扰导致地面站不断报串口错误所以建议三线连接。波特率统一设 115200地面站里选择对应的串口号后点击连接观察“无效帧”计数是否持续增长。若是先检查校验和代码再把串口助手打开看原始字节流确认每一帧是否以AA AA开头。5. 进阶调参与姿态数据验证方法5.1 陀螺仪零偏标定与数据质量判断姿态解算的效果上限由传感器质量决定。陀螺仪静止时输出不为零长时间积分会累积出几度每秒的漂移。简单标定方法是上电后保持静止 2 秒采样 100 次取平均值存下来作为零偏在运行时刻从原始读数中减去。float gyro_offset[3]; for (int i 0; i 100; i) { mpu6050_read_raw(accel, gyro); gyro_offset[0] gyro[0]; gyro_offset[1] gyro[1]; gyro_offset[2] gyro[2]; delay_ms(2); } gyro_offset[0] / 100; gyro_offset[1] / 100; gyro_offset[2] / 100;但零偏随温度变化起飞前陀螺仪内部温度还未稳定刚上电的零偏和飞行几分钟后的零偏可能差别较大。更稳妥的做法是把 Mahony 算法的 Ki 调到 0.08 到 0.1让积分项持续在线估计零漂。离线标定时注意飞机不要放在强振动源旁边否则加速度计采集到的是振动导致地面站上姿态静止时也在 ±0.5° 内波动。5.2 解算频率与控制频率的匹配我见过很多新手把姿态解算放在主循环里和电机控制混在一起。当电机 PWM 频率高、串口打印频繁时主循环周期不稳定Mahony 的dt跳动姿态解算很容易发散。更合理的做法是用定时器触发 1kHz 中断中断里只做陀螺仪和加速度计读取、四元数更新然后将欧拉角放到共享变量里主循环只负责读取并发送地面站。// 定时器中断服务函数1kHz void TIM1_UP_IRQHandler(void) { int16_t accel[3], gyro[3]; float gx_raw[3], ax_raw[3]; static uint32_t last_time; mpu6050_read_raw(accel, gyro); // 转换为物理单位陀螺仪 /16.4加速度 /4096 ax_raw[0] accel[0] / 4096.0f; ax_raw[1] accel[1] / 4096.0f; ax_raw[2] accel[2] / 4096.0f; gx_raw[0] gyro[0] / 16.4f * DEG2RAD; gx_raw[1] gyro[1] / 16.4f * DEG2RAD; gx_raw[2] gyro[2] / 16.4f * DEG2RAD; float dt (HAL_GetTick() - last_time) / 1000.0f; last_time HAL_GetTick(); mahony_update(gx_raw[0], gx_raw[1], gx_raw[2], ax_raw[0], ax_raw[1], ax_raw[2], dt); // 四元数转欧拉角存给主循环 atti_roll atan2f(...); atti_pitch asinf(...); atti_yaw atan2f(...); }如果定时器中断和主循环都在使用q数组需要加临界区保护否则当主循环正在读四元数而中断写入时可能读到新旧混搭的数据。最简单的方法是只在中断里写主循环读取时临时关闭中断读完再打开。飞行控制任务如果跑在 4kHz 或 8kHz姿态解算频率不必一样高通常 1kHz 解算、控制率 1kHz 或 2kHz 就足够更高的频率只会增加微控制器的负载。5.3 验证姿态解算是否正确的三种方法第一种静态水平。把飞控平放地面站显示的 roll 与 pitch 应在 0° 附近yaw 任意。若 roll 恒偏 2°说明加速度计安装有角度误差可以在代码里做安装角补偿。第二种静态旋转。用手缓慢将飞控绕 X 轴转 90°读数应跟上且稳定在 90°停止后 1 秒内回落到 89° 到 91° 之间偏差大的话检查陀螺仪量程或 Kp 大小。第三种动态回零。快速晃动飞控再静止roll 和 pitch 最终应回到 0°yaw 不能回零属于正常现象因为它没有绝对参考。若回不到 0° 而是停在某个固定角度多半是振动导致加速度计被拉偏或 Mahony 的 Ki 太小无法抑制积分饱和。5.4 从地面站数据反推 PID 的可用技巧ANOTC 地面站不仅能看姿态还能通过自带的数据分析窗口做波形对比。把油门从 0 推到 50%再看 pitch 波形可以判断飞机是否有低头或抬头趋势。具体技巧是在遥控器上一个通道叠加一个 5° 的 pitch 阶跃信号同时记录姿态响应如果响应有超调且来回振荡说明 PID 的 P 太大或 D 太小如果响应迟缓且到不了目标角度说明 P 太小。地面站自带的时域波形比肉眼观察飞机更可靠因为高频抖动看不清波形能直观看到 10Hz 以下的低频振荡。发送数据时还可以加入一个自定义状态量通道比如把电池电压或控制量输出发到地面站的“用户数据”功能字里用它的曲线对照姿态角响应这样可以判断是姿态环振荡还是电机响应慢造成的耦合。不同版本的地面站功能字定义略有不同建议先用官方示例的“示波器”功能字测试一帧确认通路后再加上其他传感器数据。本文还有配套的精品资源点击获取
