C语言嵌入式无人机飞控源码拆解：1kHz调度与串级PID实践

简介：面向嵌入式无人机控制学习者与开发者的一份C语言项目源码包，适合课程设计、毕业设计及飞控入门参考。项目围绕微控制器展开，涵盖电机、GPS与遥控器模块的硬件控制，加速度计、陀螺仪、气压计的数据采集，以及基于PID的姿态、速度与高度控制，并附带图像处理和无线通信相关模块，便于理解飞控系统的整体组织方式。压缩包共1740个文件，约19.23MB，以278个h头文件、258个c源文件为主体，辅以html说明、png截图、launch配置及ld链接脚本、uvproj/uvopt等工程文件，兼顾阅读与多环境编译参考。已有870人学习下载。代码注释较丰富、目录结构清晰，读者可据此梳理传感器采集到控制输出的链路，参考PID参数组织与模块划分，用于二次开发、方案对照和排错思路积累。

1. 一份 C 语言嵌入式无人机源码，能直接抄的其实只有三分之一

把一份飞控源码拖进编辑器，很多人的第一反应是从main往下读，读到三千行发现还在初始化，于是关掉不再看。这类 C 语言嵌入式无人机控制项目的价值并不在于它已经能飞起来，而在于它把最麻烦也最琐碎的部分完整展示出来了：1 kHz 定时器中断怎么排任务、陀螺仪零偏怎么标、串级 PID 的输出怎么限幅、电机混控怎么写才不让四个角互相干扰。

它适合已经会写外设驱动、但没做过完整闭环系统的人，也适合手里有块板子、想验证自己控制算法的老手。源码配了设计与说明文档，等于把“为什么这么写”和“怎么写的”分开摆，前者比后者更值得看，因为代码能抄，取舍抄不走。

2. 代码骨架拆解：从 main 到 1 kHz 控制回路的任务分层

拿到工程先别急着make。飞控代码真正难的地方不在某一行算法，而在于哪些事必须在中断里做、哪些事绝对不能。这一章先把目录切开，再看时基怎么打，最后才是编译烧录。

2.1 目录切分：六层边界与推荐的阅读顺序

这类工程通常按职责切成六层，边界清楚比文件数量更重要：

阅读顺序建议按“配置 → 驱动 → 控制 → 应用”走，而不是从main开始。先看config/里的引脚和传感器型号，你就知道这份代码默认面向什么样的板子；再看drivers/里 IMU 的读函数返回值有没有人检查，这一眼基本能判断代码成熟度；最后进control/，因为那里的每个常量都对应一次炸机经验。

反过来，如果你想改行为（比如加一个定高模式），走的是相反路径：先在app/的状态机里加状态位，再到control/加外环，最后才动drivers/去拿气压计数据。跨层直接调用是这类工程最常见的架构问题，app/里直接写寄存器操作，后面换板子就是灾难。

注意：如果control/里出现#include "board_xxx.h"这种板级头文件，说明分层已经漏了，移植时会很麻烦。先把它换成抽象接口再动手改逻辑。

2.2 时基与调度：中断只打标志，主循环干活

飞控的节奏靠一个 1 kHz 的定时器中断打底。新手最容易犯的错是把 PID 计算写进中断服务函数里，短时间看不出问题，一旦加了日志或遥测，中断执行时间超标，整个控制周期就开始抖。

合理的做法是中断只做三件事：清标志、累加时间、置任务标志位。

/* 任务标志：全部 volatile，因为主循环和中断都会碰 */ typedef struct { volatile uint8_t imu; /* 1 kHz 读 IMU + 角速度内环 + 混控输出 */ volatile uint8_t attitude; /* 500 Hz 姿态解算 + 角度外环 */ volatile uint8_t rc; /* 100 Hz 遥控帧超时检查 */ volatile uint8_t telem; /* 20 Hz 遥测下行 */ volatile uint8_t log; /* 200 Hz 黑匣子入缓冲 */ } task_flag_t; static task_flag_t g_flag; /* 1 kHz 时基中断：只置标志，不读 SPI、不算浮点、不写 flash */ void TIM6_IRQHandler(void) { static uint16_t n = 0; if (TIM6->SR & TIM_SR_UIF) { TIM6->SR = ~TIM_SR_UIF; /* 直接写寄存器清标志，比库函数少几十个周期 */ n++; g_flag.imu = 1; /* 最高优先级，永远第一拍 */ if ((n & 1) == 0) g_flag.attitude = 1; /* 2 分频 -> 500 Hz */ if ((n % 10) == 0) g_flag.rc = 1; /* 100 Hz */ if ((n % 50) == 0) g_flag.telem = 1; /* 20 Hz */ if ((n % 5) == 0) g_flag.log = 1; /* 200 Hz */ } }

主循环按固定优先级顺序消费标志，顺序本身就是调度策略：

for (;;) { if (g_flag.imu) { g_flag.imu = 0; imu_read_burst(&g_imu); /* SPI 突发读，约 10~15 us */ control_rate_loop(&g_imu); /* 角速度内环 -> 混控 -> 写输出比较寄存器 */ } if (g_flag.attitude) { g_flag.attitude = 0; attitude_update(&g_imu, g_dt); /* 互补滤波 -> 角度外环 */ } if (g_flag.rc) { g_flag.rc = 0; rc_check_failsafe(); } if (g_flag.telem) { g_flag.telem = 0; telemetry_send(); } if (g_flag.log) { g_flag.log = 0; blackbox_push(); } feed_watchdog(); }

参数上要盯住两点。第一，imu标志必须独占优先级，姿态解算可以丢拍，角速度内环丢拍就是姿态失控。第二，blackbox_push()只往环形缓冲里拷数据，绝不能在里面直接写 flash 或 SD 卡，一次页擦除动辄几十毫秒，足够让 1 kHz 的回路丢二十拍。

提示：如果主循环里出现while (!flag);这种忙等，说明调度模型没设计好。飞控的主循环应该永远是“有事就干、没事就空转”，不允许阻塞。

2.3 编译与烧录：Makefile 里必须先确认的三个宏

这类工程的 Makefile 一般把板级差异收敛到几个宏上，改错一个就是“编译过了但没反应”。

# 板级配置：决定引脚映射、传感器型号、输出方式 BOARD ?= f405_v2 # 0 = PWM 400 Hz，1 = DShot300，需要 DMA 支持 USE_DSHOT ?= 0 # 内环频率，不能高于 IMU 的输出数据率（ODR） LOOP_HZ ?= 1000 CFLAGS += -D BOARD_$(BOARD) -D USE_DSHOT=$(USE_DSHOT) -D LOOP_HZ=$(LOOP_HZ) CFLAGS += -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 -O2 -g3 CFLAGS += -Wall -Wextra -Wdouble-promotion # 最后一个专门抓意外的双精度运算

-mfloat-abi=hard要求启动文件和链接脚本也走硬件浮点，三者不匹配会出现“能编译、进 HardFault”的经典症状。-g3保留宏定义信息，用调试器看变量时不会满屏<optimized out>。-Wdouble-promotion在这个场景特别有用：单精度 MCU 上，一个不注意写下的3.1415926字面量会被提升成 double，一次运算几百个周期，在 1 kHz 回路里就是灾难。

烧录前后的完整动作：

# 交叉工具链前缀按你的环境填 CROSS ?= arm-none-eabi- make clean && make BOARD=f405_v2 LOOP_HZ=1000 USE_DSHOT=0 -j8 # 看体积：text 是 flash，data+bss 是 RAM，溢出了就别谈跑了 ${CROSS}size build/firmware.elf # 烧录与复位，具体命令由 BOARD 决定用哪种调试器 make flash

text超过片内 flash 的 80% 就要警惕：后面加日志、加参数表很容易顶到上限。bss更关键，飞控的栈通常放在 RAM 尾部，堆栈余量小于 1 KB 时，一次深一点的浮点运算嵌套就可能把栈踩穿，表现是随机的 HardFault，极难定位。比较省事的办法是在启动文件里把栈填成固定图案（比如0xDEADBEEF），运行一段时间后检查尾部被改写了多少，实际用量大致就在这个范围内。

3. 姿态解算与控制环：互补滤波系数、串级 PID 与混控的落地参数

这一章是整份源码里最值得逐行读的部分。算法本身不复杂，难的是每个系数的物理含义，以及一旦设错会以什么形式表现。

3.1 陀螺仪零偏标定与互补滤波：先标定，再谈滤波

上电第一件事是标定，不是滤波。原始陀螺数据里有一个几十到几百 dps 量级的零偏，不扣掉它，任何滤波器都只能把错误积分得更平滑。

/* 上电静置标定：采 2000 点，丢掉前 200 点（电源爬升和传感器启动）， * 算均值作为零偏，算标准差作为噪声门限。 * 噪声超过 3 dps 说明电机在转或者没放平，直接拒绝解锁。 */ int gyro_calibrate(float bias[3], float *noise_dps) { const int N = 2000, SKIP = 200; float sum[3] = {0}, sum2[3] = {0}; int cnt = 0; for (int i = 0; i < N; i++) { imu_raw_t r; if (imu_read_raw(&r) != 0) return -1; /* 驱动层返回值必须检查 */ if (i < SKIP) continue; sum[0] += r.gx; sum[1] += r.gy; sum[2] += r.gz; sum2[0] += r.gx * r.gx; sum2[1] += r.gy * r.gy; sum2[2] += r.gz * r.gz; cnt++; delay_us(500); /* 约 2 kHz 采样，避开 ODR 混叠 */ } float worst = 0.0f; for (int a = 0; a < 3; a++) { bias[a] = sum[a] / cnt; /* 均值 = 零偏 */ float var = sum2[a] / cnt - bias[a] * bias[a]; float sd = sqrtf(var > 0 ? var : 0); if (sd > worst) worst = sd; } *noise_dps = worst; return (worst < 3.0f) ? 0 : -1; /* 噪声太大直接判失败 */ }

标定完之后才轮到互补滤波。它的思路很朴素：陀螺积分短期准、长期漂；加速度计长期准、短期被机体振动污染。用一个系数把两者加权。

/* 一阶互补滤波，alpha 由时间常数决定：alpha = tau / (tau + dt) * tau = 0.5 s、dt = 2 ms 时 alpha ≈ 0.996，加速度计每拍只贡献 0.4% */ #define TAU_S 0.5f #define RAD2DEG 57.29578f static float roll_f, pitch_f; void attitude_update(const imu_t *imu, float dt) { float ax = imu->ax, ay = imu->ay, az = imu->az; /* 加速度模长偏离 1g 太多，说明在做机动或者振动大，这时候别再信它 */ float amag = sqrtf(ax*ax + ay*ay + az*az); float w_acc = 1.0f; if (amag < 0.85f || amag > 1.15f) w_acc = 0.0f; float acc_roll = atan2f(ay, az) * RAD2DEG; float acc_pitch = atan2f(-ax, sqrtf(ay*ay + az*az)) * RAD2DEG; float alpha = TAU_S / (TAU_S + dt); roll_f = alpha * (roll_f + imu->gx * dt) + (1.0f - alpha) * w_acc * acc_roll; pitch_f = alpha * (pitch_f + imu->gy * dt) + (1.0f - alpha) * w_acc * acc_pitch; }

三个参数值得单独说。dt必须用实测间隔，不能用标称的 1 ms，下面第 5 章会讲为什么这是最隐蔽的一个坑。tau取 0.5 s 是一个折中：再小会让机架振动直接进姿态角，再大会让姿态回正变迟钝，手感发黏。w_acc这个门限是判断滤波是否稳定的关键，没有它，急加速时加速度计会把“机体加速度”误判成“重力方向偏了”，姿态角会突然甩出去一大截。

注意：imu->gx这里已经是扣过零偏的值（单位 dps）。如果驱动层直接吐原始值，务必在进入控制层之前减掉bias，别在滤波函数里做减法，否则后面每一处用到角速度的地方都要重复一遍。

3.2 串级 PID：内环角速度、外环角度的整定顺序

飞控的姿态控制几乎都是串级结构：外环输入是目标角度，输出是目标角速度；内环输入是目标角速度，输出是力矩（最终转成四路电机增量）。外环的带宽要明显低于内环，通常是 1:5 到 1:10，否则两个环会互相激励。

typedef struct { float kp, ki, kd; float i_limit; /* 积分限幅：抗饱和的第一道闸 */ float out_limit; /* 输出限幅：告诉内环"你能要多大的角速度" */ float i_term; float prev_meas; /* 微分先行用 */ float d_filt; /* 微分一阶低通状态 */ } pid_t; #define D_LPF_ALPHA 0.15f float pid_update(pid_t *p, float target, float meas, float dt) { float err = target - meas; p->i_term += p->ki * err * dt; if (p->i_term > p->i_limit) p->i_term = p->i_limit; if (p->i_term < -p->i_limit) p->i_term = -p->i_limit; /* 微分先行：对测量值微分而不是对误差微分， * 目标值阶跃时不会产生一个尖峰把输出打到饱和 */ float d_raw = -(meas - p->prev_meas) / dt; p->prev_meas = meas; p->d_filt += D_LPF_ALPHA * (d_raw - p->d_filt); float out = p->kp * err + p->i_term + p->kd * p->d_filt; if (out > p->out_limit) out = p->out_limit; if (out < -p->out_limit) out = -p->out_limit; return out; }

整定顺序不能反。先把外环断开（外环输出直接给内环当目标，用调试器强制写一个角速度目标），只调内环的kp：一点点加，加到用手快速拨动机身能感到明显阻力、松手后有一到两次回弹但不停不下来，就到位了。再加kd，把回弹压掉，kd太大会听到电机高频啸叫，那是微分把噪声放大了。ki一般给得很小，甚至给 0，因为角速度环的静差对姿态影响有限，加了反而容易在降落触地时积分饱和。

内环调好再闭外环。外环只上kp，从 2.0 起，小型四轴常见落点在 6~8 之间；kd给 0 或极小；ki通常直接给 0，除非后面要做定点悬停才需要它消稳态误差。

out_limit这个参数很多人随手填个大数，结果是外环一旦算错就直接要一个物理上做不到的角速度，内环长期饱和，表现是“打杆没反应，然后突然过冲”。合理做法是先测出机体能达到的最大角速度（比如 400 dps），再让out_limit略小于这个值。

3.3 混控输出：X 型四轴映射、归一化与死区顺序

混控是整份代码里最容易写反的一段，写反了飞机不会报错，只会在起飞瞬间翻过去。

/* X 型四轴：M1 右前、M2 左后、M3 左前、M4 右后 * 约定：roll 右倾为正、pitch 抬头为正、yaw 右转为正、thr 0~1 * 对角电机同转向，相邻电机反转向 */ #define ESC_MIN 0.06f /* 电调最小油门，低于这个值电机停转 */ void mixer_x4(float thr, float roll, float pitch, float yaw, float out[4]) { out[0] = thr - roll + pitch - yaw; /* 右前 */ out[1] = thr + roll - pitch - yaw; /* 左后 */ out[2] = thr + roll + pitch + yaw; /* 左前 */ out[3] = thr - roll - pitch + yaw; /* 右后 */ /* 第一步：整体归一化。任一路超过 1 就等比缩放， * 保证姿态力矩的方向和比例不变，而不是单路削顶 */ float maxv = 0.0f; for (int i = 0; i < 4; i++) if (out[i] > maxv) maxv = out[i]; if (maxv > 1.0f) for (int i = 0; i < 4; i++) out[i] /= maxv; /* 第二步：先钳到 0，再映射到电调区间。顺序反了会把负值压进死区里， * 表现为低油门段姿态响应突然变迟钝 */ for (int i = 0; i < 4; i++) { if (out[i] < 0.0f) out[i] = 0.0f; out[i] = ESC_MIN + out[i] * (1.0f - ESC_MIN); } }

方向验证有一套不需要起飞的笨办法，但非常有效：拆掉桨，解锁后用手把机身往右倾，观察右前和左后两个电机的输出是不是在减小，另外两个在增大。如果反了，说明roll的符号或者电机编号对不上，改代码或者改接线，二者只能改一个。

另外，ESC_MIN的取值直接影响低油门段的表现。设得太高，悬停点会被推到油门行程的中段，手感发冲；设得太低，电机在低油门时可能转不匀，出现“怠速游车”。常见做法是接电调做一次行程校准，然后把这个实测下限写进宏，而不是照抄别人的数字。

4. 外设与链路：遥控帧解析、二进制遥测与解锁状态机

控制算法跑得再好，只要输入输出链路有一处不可靠，飞机一样不可控。这一章处理三件事：怎么把遥控信号可靠地读进来、怎么把参数改出去、怎么在上天之前把所有不该发生的事拦下来。

4.1 遥控帧解析：定长帧、位拼接与失控标志

串口遥控协议常见的形态是 25 字节定长帧：一个帧头、22 字节承载 16 个 11 bit 通道、两个标志字节。电气上是 100000 波特、8E2、信号反相。

/* 25 字节定长帧：buf[0] = 0x0F 帧头，buf[1..22] 承载 16 路 11 bit 通道， * buf[23] 保留，buf[24] 是标志位（bit2 = 失控保护，bit3 = 帧丢失） */ #define RC_FRAME_LEN 25 static uint8_t s_buf[RC_FRAME_LEN]; static uint8_t s_idx; static uint32_t s_last_frame_ms; static rc_t s_rc; void rc_uart_rx_isr(uint8_t byte) { /* 不同步就一直丢字节，直到见到帧头，避免半帧错位后一路错下去 */ if (s_idx == 0 && byte != 0x0F) return; s_buf[s_idx++] = byte; if (s_idx < RC_FRAME_LEN) return; s_idx = 0; if (s_buf[24] & 0x04) { /* 发射端已经在告诉你信号没了 */ s_rc.failsafe = 1; return; } for (int ch = 0; ch < 16; ch++) { int bit = ch * 11; /* 11 bit 会跨字节，所以要先拼两个字节再右移掩码 */ uint16_t raw = (uint16_t)s_buf[1 + bit / 8] | ((uint16_t)s_buf[2 + bit / 8] << 8); s_rc.ch[ch] = (raw >> (bit % 8)) & 0x7FF; } s_rc.failsafe = 0; s_last_frame_ms = millis(); }

位拼接是这段代码的全部难点。第ch个通道的起始位是ch * 11，落在哪个字节由bit / 8决定，字节内偏移是bit % 8。因为 11 不是 8 的倍数，几乎每个通道都跨字节，所以必须拼两个字节再移位，不能只读一个。

信号反相有两种处理方式：硬件上加一个反相器，或者用 MCU 串口自带的 RX 反相配置位。软件方案（每个字节取反）在 100 k 波特下中断频率约 10 kHz，对 1 kHz 的控制回路来说负担偏重，一般不推荐。

4.2 遥测与参数在线整定：把 PID 参数放进可读写参数表

参数写死在头文件里，每改一次kp就要编译烧录一次，试飞效率低到让人放弃调参。正确的做法是建一张参数表，用 ID 索引，遥测链路读写。

typedef enum { P_F32, P_U16, P_I32 } p_type_t; typedef struct { uint16_t id; p_type_t type; void *ptr; float min, max; /* 写入前必须校验范围，否则一个越界值就能炸机 */ } param_t; static pid_t g_pid_rate_roll; static float g_att_tau = 0.5f; static const param_t g_params[] = { { 0x0101, P_F32, &g_pid_rate_roll.kp, 0.0f, 10.0f }, { 0x0102, P_F32, &g_pid_rate_roll.ki, 0.0f, 5.0f }, { 0x0103, P_F32, &g_pid_rate_roll.kd, 0.0f, 1.0f }, { 0x0104, P_F32, &g_pid_rate_roll.i_limit, 0.0f, 0.5f }, { 0x0201, P_F32, &g_att_tau, 0.1f, 2.0f }, };

几个调参项的典型初值和边界可以列成一张表，方便第一次上手时有参照：

写入流程上有一个必须遵守的约束：收到写入请求后先按min/max校验，通过后先写 RAM 副本，等解锁状态下再落 flash。飞行中擦写 flash 会阻塞几十毫秒，足够让控制回路丢几十拍。

提示：参数表里每一项都建议带一个“是否已校验”的标记，上电时把所有值过一遍范围检查，任何一项越界就拒绝解锁并闪错误码，比飞到一半发现参数不对强得多。

4.3 解锁状态机：把不该起飞的情况挡在地面

解锁不是一个if，而是一个带时序的状态机。所有检查必须连续通过一段时间才真正解锁，避免开关抖动或者瞬间的传感器毛刺骗过逻辑。

typedef enum { ARM_IDLE, ARM_CHECKING, ARM_ARMED, ARM_FAULT } arm_state_t; static uint16_t s_ok_ticks; arm_state_t arming_step(const rc_t *rc, float noise_dps, float batt_v, float tilt_deg) { if (!rc->ch[ARM_SW]) { s_ok_ticks = 0; return ARM_IDLE; } /* 开关回位就复位 */ if (rc->failsafe) { s_ok_ticks = 0; return ARM_FAULT; } if (noise_dps > 3.0f) { s_ok_ticks = 0; return ARM_FAULT; } /* 陀螺噪声大 */ if (batt_v < 3.5f * CELLS) { s_ok_ticks = 0; return ARM_FAULT; } /* 单片电压门限 */ if (rc->ch[THR] > THR_MIN + 30) { s_ok_ticks = 0; return ARM_FAULT; } /* 油门没回底 */ if (tilt_deg > 25.0f) { s_ok_ticks = 0; return ARM_FAULT; } /* 机身没放平 */ /* 连续 200 ms 全部通过才真解锁，各检查项的采样率是 100 Hz */ if (++s_ok_ticks > 20) return ARM_ARMED; return ARM_CHECKING; }

失控保护要和状态机绑在一起，而不是各写各的。判定逻辑很简单：连续 100 ms 没收到合法帧，就判定为失控，立即把油门置零、姿态目标置零，并保持到重新收到帧为止。这个 100 ms 不能省，因为单帧丢失在正常飞行里是会发生的，判得太快会导致空中频繁进入保护状态。

5. 排查与避坑：上电抖、高频振、姿态飘这五类现场

下面五条基本覆盖了第一次上手会撞到的绝大多数问题。每条的“现象”都是肉眼可见的，“原因”往往藏在很反直觉的地方。

5.1 硬件与供电：看起来像软件问题的两个坑

坑一：上电后电机短促抖动一下就没反应，解锁灯快闪

现象：接电瞬间四个电机同时“哒”一声，之后飞控进入故障态，拨解锁开关无反应，重启也复现。

原因：3.3 V 和 5 V 的上电时序不对，IMU 比 MCU 晚就绪，初始化时读到的WHO_AM_I是全 0，但驱动层初始化函数根本没检查返回值，一路带着全 0 的配置跑下去，后面的自检自然全挂。

解决：每一个传感器初始化都判返回值，失败重试三次；SPI 时钟先降到 1 MHz 完成 ID 校验，再升到工作频率；在传感器上电后加 100 ms 延时。这三条一起加，基本不会再出现。

坑二：悬停时高度缓慢下沉，落地后电调烫手

现象：油门推到中位附近才能勉强悬停，响应发钝，落地摸电调明显偏热。

原因：混控里归一化和死区的顺序写反了。先加ESC_MIN再归一化，会把低油门段的有效分辨率压掉一大截，飞控为了让飞机起来只能一路加输出，实际工作在很陡的映射曲线上，效率低、发热大。

解决：把顺序改成“先归一化、后钳零、最后映射死区”，也就是第 3.3 节那段代码的顺序。验证办法是把四路输出打到串口，手动给一个小的 roll 目标，看四路输出是否在 0.05 以内有连续的、单调的变化，出现跳变就说明顺序还是错的。

5.2 控制与参数：三个只在空中才暴露的坑

坑三：推到悬停油门附近出现高频抖动，收油就好

现象：悬停点附近机身开始 20~30 Hz 的抖动，桨声变尖，落地检查螺丝都是紧的。

原因：角速度内环的kp偏大，或者kd项没有低通，把机架的共振峰放大了。四轴机架的固有频率常常就落在 20~40 Hz 这个区间，控制带宽一高就正反馈上去。

解决：先看黑匣子日志里陀螺数据的频谱，找到共振峰在哪；把kd的一阶低通系数调到 0.1~0.15，或者把内环从 1 kHz 降到 500 Hz；还不行就把rate.kp降 30% 再试。这三步按顺序做，别一次全改，否则下次不知道是哪一步起的作用。

坑四：松杆后缓慢自转，姿态明明是正的

现象：俯仰和横滚回正正常，但机头方向慢慢偏，一分钟能偏二三十度。

原因：偏航方向只有角速度环没有角度外环，靠陀螺积分维持，零偏残差一直累积；或者用了磁罗盘但罗盘离电池线太近，被电流产生的磁场干扰。

解决：纯姿态模式下，航向本来就不该做闭环，交给飞手修就行，硬加一个不可靠的罗盘闭环反而更危险。如果确实需要航向保持，先把罗盘挪到离电池线 3 cm 以上，上电做一次硬铁标定，再看残差能不能压到 5 度以内。

坑五：姿态角偶尔突然偏一大截，落地重启就恢复

现象：飞行中姿态角毫无征兆地偏了二三十度，飞机跟着歪一下，落地断电重启完全正常，怎么也复现不了。

原因：姿态解算里的dt用的是标称值 0.001，而某一拍主循环被阻塞了 20 ms（比如在循环里写了 flash 或者阻塞式打印串口），积分项一下子放大了二十倍，姿态角直接飞掉。

解决：用定时器计数器或者 DWT 周期计数器算真实dt，并且做上下限钳位，超出 0.5 ms ~ 5 ms 就丢掉这一拍不积分。同时把主循环里所有可能阻塞的操作全部挪走，flash 写入只在解锁状态下做，打印走环形缓冲加 DMA。这个坑最阴的地方在于它不可复现，只有加了dt钳位才算真正关掉。

dt钳位的写法很短，但值得单独摆出来：

/* dt 必须来自真实计时，并且钳位。超出范围说明这一拍已经被污染，直接丢弃 */ uint32_t now = micros(); float dt = (now - s_last_us) * 1e-6f; s_last_us = now; if (dt < 0.0005f || dt > 0.005f) { s_dt_drop++; /* 计数器上报到遥测，能看出系统有多"脏" */ return; }

s_dt_drop这个计数器建议一直上报。它是一块很灵敏的指示表：正常悬停时它应该一直是 0 或者个位数，一旦开始快速上涨，说明主循环里有东西在阻塞，别等到姿态出问题才回头查。

6. 用日志回放验证控制环：把试飞次数从二十次压到三次

调参最耗的其实不是算参数，而是每次改完都要出去飞一趟。缩短这个循环的办法只有一个：让每次飞行带回尽可能多的信息，然后在地面上把数据翻来覆去看。

日志的记录点放在混控之后、写输出之前，200 Hz 就够，字段按下面的结构组织：

地面回放用一段脚本就能跑，重点是三个指标：阶跃响应的超调量、稳态误差、陀螺频谱的峰值位置。

import struct import numpy as np import matplotlib.pyplot as plt # 帧格式：时间戳 + 三轴陀螺 + 三轴角度 + 三轴 PID 输出 + 四路电机 + 状态位 FRAME = struct.Struct('<I3h3h3h4HB') def load(path): rows = [] with open(path, 'rb') as f: data = f.read() for off in range(0, len(data) - FRAME.size + 1, FRAME.size): rows.append(FRAME.unpack_from(data, off)) arr = np.array(rows, dtype=np.float64) return arr def step_metrics(t_us, target, meas, lo=0.1, hi=0.9): """用 10%~90% 上升段估超调量和调节时间，比肉眼判断稳得多""" t = (t_us - t_us[0]) * 1e-6 span = target[-1] - target[0] if abs(span) < 1e-6: return None y = (meas - meas[0]) / span peak = y.max() overshoot = (peak - 1.0) * 100.0 idx = np.where(np.abs(y - 1.0) > 0.02)[0] settle = t[idx[-1]] if len(idx) else 0.0 return overshoot, settle if __name__ == '__main__': a = load('flight_01.bin') t_us, gyro, ang, pid, motor = a[:, 0], a[:, 1:4], a[:, 4:7], a[:, 7:10], a[:, 10:14] # 只用 roll 轴做示范，另外两轴同理 print('roll overshoot / settle:', step_metrics(t_us, ang[:, 0], ang[:, 0])) plt.plot((t_us - t_us[0]) * 1e-6, ang[:, 0], label='roll') plt.plot((t_us - t_us[0]) * 1e-6, pid[:, 0] * 0.001, label='pid out') plt.legend(); plt.xlabel('t / s'); plt.show()

step_metrics里那两个阈值（0.02 的稳态带、

本文还有配套的精品资源，点击获取