本文讲清楚凌霄飞控二次开发中最容易卡住的几件事：工程从哪里打开、主循环怎么跑、串口到底接谁、T265 数据怎样送进 STM32、如何在用户层写自动任务，以及路径规划代码应该怎么改才不容易炸机。

---

1. 适用范围与安全声明

这篇教程适合以下读者：

• 已经拿到匿名科创/凌霄飞控相关例程，但不知道从哪里二次开发；
• 想用 STM32F407 做上层控制板，通过凌霄飞控完成解锁、起飞、定高、速度控制和降落；
• 想把 Intel RealSense T265、Jetson/NUC、光流、测距等外部定位数据接入飞控；
• 正在做无人机路径规划、定点飞行或室内自主飞行任务。

本文分析的主工程入口是：

ProjectSTM32F407/ANO_LX_STM32F407.uvprojx

工程目录中虽然还有 ProjectTM4C123、ProjectMSP432，但当前路径规划代码实际应优先看 ProjectSTM32F407。

安全提醒一定要放在最前面：

• 第一次上电调试必须拆桨；
• 串口必须共地，电平应确认是 3.3 V TTL；
• 自动任务必须保留遥控器开关作为总使能；
• 不要只相信软件限高，光流/T265 丢数据时必须能手动接管；
• 每次改 Automatic_Flay() 这类任务状态机，都先在地面看串口变量和电机输出，再飞。

---

2. 工程整体架构

先看目录，不要急着改代码：

25路径规划(32接收）
├─ FcSrc                         飞控协议、调度、凌霄命令封装
├─ DriversBsp                    板级初始化、遥控、ADC、光流等 BSP
├─ DriversMcu/STM32F407/Drivers  STM32F407 串口、定时器、PWM 等底层驱动
├─ Headware                      用户任务、坐标控制、路径规划、T265 接收
├─ Jetson                        T265 到 STM32 的串口桥接、ROS/RViz 工具
├─ ProjectSTM32F407              Keil 工程，本文主入口
├─ ProjectTM4C123                其他 MCU 工程模板
└─ ProjectMSP432                 其他 MCU 工程模板

如果用一张图概括数据流，大致是这样：

这份工程不是把姿态控制、混控、电机闭环全放到 STM32F407 上做。F407 更像“上层任务控制器”：它接收外部定位和遥控输入，然后通过凌霄协议告诉飞控“解锁、起飞、往某个方向飞、发速度目标、降落”。

---

3. 从哪里开始读代码

主函数在：

FcSrc/main.c

核心逻辑非常短：

int main(void)
{
    All_Init();

    Scheduler_Setup();
    while (1)
    {
        Scheduler_Run();
    }
}

所以读代码可以分成三条线：

线索	入口	作用
初始化线	DriversBsp/Drv_BSP.c -> All_Init()	初始化系统、LED、PWM、串口、遥控、ADC、匿名协议、定时器
调度线	FcSrc/Ano_Scheduler.c	定时调用 1000 Hz、500 Hz、50 Hz、2 Hz 等任务
用户任务线	Headware/My_control.c -> Automatic_Flay()	写比赛逻辑、路径点、起飞降落和速度控制

All_Init() 中最关键的是串口初始化：

DrvUart1Init(500000);
DrvUart2Init(115200);
DrvUart3Init(500000);
DrvUart5Init(500000);

调度器中，Automatic_Flay() 被放在 50 Hz 任务里：

static void Loop_50Hz(void)
{
    Automatic_Flay();
}

这意味着用户自动飞行逻辑约每 20 ms 执行一次。写状态机时，所有延时计数都要按这个周期理解。例如源码里的 Time_dly_cnt_ms += 20，就是因为这里是 50 Hz。

---

4. 串口分配：这份工程最容易接错的地方

很多教程或 README 会写 Jetson 接 STM32 的 PB11 / USART3_RX。但以当前 F407 源码为准，实际路由不是这样。

在：

DriversMcu/STM32F407/Drivers/Drv_Uart.c

可以看到：

#define U1GetOneByte    T265_GetOneByte
#define U2GetOneByte    xavier_GetOneByte
#define U3GetOneByte    AnoOF_GetOneByte
#define U4GetOneByte    NoUse
#define U5GetOneByte    ANO_DT_LX_Data_Receive_Prepare

也就是说，当前工程实际串口表如下：

串口	波特率	STM32F407 引脚	当前源码接收函数	建议连接对象
USART1	500000	PA9 TX / PA10 RX	T265_GetOneByte	Jetson/NUC 发来的 T265 位姿帧
USART2	115200	PD5 TX / PD6 RX	xavier_GetOneByte	Xavier/识别模块/自定义命令
USART3	500000	PB10 TX / PB11 RX	AnoOF_GetOneByte	匿名光流/测距
UART5	500000	PC12 TX / PD2 RX	ANO_DT_LX_Data_Receive_Prepare	凌霄飞控/匿名数传协议

所以本文建议按源码接线：T265 桥接程序的 TX 接 STM32F407 的 PA10(USART1_RX)，STM32 与 Jetson/USB-TTL 共地。如果你坚持接到 PB11(USART3_RX)，那就必须同步修改 Drv_Uart.c 的宏映射和初始化逻辑，否则 T265 数据会被当成光流数据解析。

---

5. 凌霄飞控命令封装

上层最常用的飞控命令在：

FcSrc/LX_FC_Fun.c

常用接口包括：

函数	作用
FC_Unlock()	解锁
FC_Lock()	上锁
LX_Change_Mode(new_mode)	切换飞行模式
OneKey_Takeoff(height_cm)	一键起飞到指定高度，单位 cm
OneKey_Land()	一键降落
OneKey_Return_Home()	一键返航
Horizontal_Move(distance_cm, velocity_cmps, dir_angle)	按距离、速度、方向角做水平移动
hover()	悬停

例如一键起飞本质上是组织一帧凌霄命令：

u8 OneKey_Takeoff(u16 height_cm)
{
    if (dt.wait_ck == 0)
    {
        dt.cmd_send.CID = 0X10;
        dt.cmd_send.CMD[0] = 0X00;
        dt.cmd_send.CMD[1] = 0X05;
        dt.cmd_send.CMD[2] = BYTE0(height_cm);
        dt.cmd_send.CMD[3] = BYTE1(height_cm);
        CMD_Send(0xff, &dt.cmd_send);
        return 1;
    }
    return 0;
}

这里有一个非常重要的机制：所有命令发送前都会检查 dt.wait_ck == 0。

dt.wait_ck 可以理解为“上一条命令是否还在等 ACK”。如果飞控还没确认上一条命令，新命令就不应该继续塞进去。否则你在状态机里看起来写了“解锁、起飞、前进、降落”，飞控实际可能只接到部分命令，或者命令顺序错乱。

命令真正打包发送在：

FcSrc/ANO_DT_LX.c

CMD_Send() 会触发 0xE0 命令帧发送，并把 dt.wait_ck 置 1。飞控 ACK 回来之后，协议解析层再清除等待状态。

---

6. 飞控状态回传怎么看

飞控状态回传同样在：

FcSrc/ANO_DT_LX.c

比较重要的帧包括：

帧 ID	作用
0x03	姿态角，例如 yaw
0x06	飞行模式、解锁状态等
0x07	速度数据
0x20	电机 PWM
0xE0	命令 ACK 相关

在二次开发时，不建议一上来就改底层协议。更稳的方式是：

1. 先确认 ANO_DT_LX_Data_Receive_Prepare() 能收到飞控回传；
2. 用匿名地面站或串口助手确认姿态、模式、解锁状态是否刷新；
3. 再在 Headware/My_control.c 写自己的任务逻辑；
4. 如果命令不执行，先查 dt.wait_ck 是否一直为 1。

---

7. T265 到 STM32 的数据链路

当前工程的 T265 接收代码在：

Headware/T265_receive.c

协议格式是匿名风格的双校验帧：

AA  地址  功能字  长度  负载...  SC  AC

其中 T265 位姿帧使用功能字：

0x51

源码要求负载第 0 字节必须为 2：

if (*(data + 2) == 0X51)
{
    if (*(data + 4) == 2 && len >= 21)
    {
        t265_ok = 0;
        ano_of.of2_sta = *(data + 5);

        t265_debug_dx = *((s16 *)(data + 6));
        t265_debug_dy = *((s16 *)(data + 8));
        t265_debug_x  = *((s16 *)(data + 14));
        t265_debug_y  = *((s16 *)(data + 16));
        t265_debug_quality = *(data + 18);

        ano_of.intergral_x = t265_debug_x;
        ano_of.intergral_y = t265_debug_y;
        ano_of.of_quality  = t265_debug_quality;
    }
}

这里的坐标单位是 cm。最终进入用户控制层的坐标宏在：

Headware/coordinate_control.h

#define nowx (ano_of.intergral_x)
#define nowy (ano_of.intergral_y)
#define nowz (ano_of.of_alt_cm)

注意：nowx/nowy 来自 T265 或类似外部定位，nowz 仍然来自光流/测距高度 ano_of.of_alt_cm，不是 T265 的 z 轴。

---

8. Jetson 端桥接程序怎么用

工程里已经有 Jetson 端程序：

Jetson/t265_to_stm32.cpp

它完成三件事：

1. 用 librealsense2 读取 T265 位姿；
2. 把坐标从 m 转成 cm；
3. 按 STM32 需要的 AA FF 51 len payload SC AC 格式发送到串口。

默认参数很适合当前工程：

std::string port = "/dev/ttyTHS1";
int baud = 500000;
double rate_hz = 50.0;
char x_axis = 'x';
char y_axis = 'z';
int x_sign = -1;
int y_sign = -1;

典型编译方式：

g++ t265_to_stm32.cpp -o t265_to_stm32 \
  -std=c++17 -lrealsense2

运行示例：

sudo ./t265_to_stm32 \
  --port /dev/ttyTHS1 \
  --baud 500000 \
  --rate 50 \
  --zero-on-start

如果发现飞机向前走时 nowx 变成负数，或者向右走时 nowy 变化方向反了，不要急着改 STM32 控制逻辑，优先在 Jetson 程序里调：

--x-axis x
--y-axis z
--x-sign 1
--y-sign -1

建议地面调试时先只看变量：

• t265_rx_byte_cnt：USART1 是否真的收到字节；
• t265_frame_ok_cnt：完整帧是否校验通过；
• t265_checksum_fail_cnt：是否存在波特率/接线/协议错误；
• t265_debug_x/y：坐标方向是否符合预期；
• t265_debug_quality：T265 跟踪质量。

---

9. 外部速度和高度如何注入凌霄飞控

外部传感器桥接在：

FcSrc/LX_FC_EXT_Sensor.c

工程会把光流横向速度封装成 0x33 外部速度帧：

ext_sens.gen_vel.st_data.hca_velocity_cmps[0] = ano_of.of1_dx;
ext_sens.gen_vel.st_data.hca_velocity_cmps[1] = ano_of.of1_dy;
dt.fun[0x33].WTS = 1;

也会把测距高度封装成 0x34 外部距离帧：

ext_sens.gen_dis.st_data.direction = 0;
ext_sens.gen_dis.st_data.angle_100 = 270;
ext_sens.gen_dis.st_data.distance_cm = ano_of.of_alt_cm;
dt.fun[0x34].WTS = 1;

这说明当前系统里：

• T265 主要提供平面累计坐标 nowx/nowy；
• 光流/测距主要提供横向速度和高度；
• 凌霄飞控使用外部速度/距离帧增强定点和定高能力；
• 用户层再根据 nowx/nowy/nowz 生成速度目标或任务命令。

---

10. 用户层二次开发入口：Automatic_Flay()

真正写比赛任务的地方是：

Headware/My_control.c

核心入口：

void Automatic_Flay(void)
{
    if(rc_in.rc_ch.st_data.ch_[ch_6_aux2] > 1700)
    {
        limit_height(60,180);

        if(Go_Aim_height == 1)
        {
            Heigh_fix(Aim_Height, SIMPLE_MISSION_HEIGHT_SPEED_CMPS);
        }

        switch(Control_Flow)
        {
            ...
        }
    }
    else
    {
        Control_Flow = -4;
        Time_dly_cnt_ms = 0;
        Go_Aim_height = 0;
        OK_to = 1;
        stop_station = 0;
    }
}

这段代码里最值得学习的是三个设计：

第一，自动任务必须由遥控器通道使能。当前用的是：

ch_6_aux2 > 1700

开关关闭时，状态机会复位到 -4。这对比赛现场非常重要：一旦飞机状态不对，先关自动开关，逻辑不要继续往下跑。

第二，状态机用 Control_Flow 推进。不同数字代表不同阶段：

状态	含义
-4	记录起点、目标高度、初始航向
-3	解锁并等待约 1 秒
-2	一键起飞到目标高度
0	飞到正方形第一条边终点
1	飞到正方形第二个点
2	飞到正方形第三个点
66	停止速度输出并降落

第三，水平飞行不是直接写死延时，而是根据坐标闭环判断到点。

---

11. 当前工程的正方形自主飞行示例

源码中的简化任务是一个 100 cm 左右的正方形路径。任务启动后，会记录起点：

simple_start_x = nowx;
simple_start_y = nowy;

然后依次飞向三个目标点：

Simple_Mission_Goto(simple_start_x,
                    simple_start_y + SIMPLE_MISSION_DISTANCE_CM);

Simple_Mission_Goto(simple_start_x + SIMPLE_MISSION_DISTANCE_CM,
                    simple_start_y + SIMPLE_MISSION_DISTANCE_CM);

Simple_Mission_Goto(simple_start_x + SIMPLE_MISSION_DISTANCE_CM,
                    simple_start_y);

Simple_Mission_Goto() 的思想很适合入门：

static int Simple_Mission_Goto(float target_x, float target_y)
{
    float err_x = target_x - nowx;
    float err_y = target_y - nowy;

    if(err_x > SIMPLE_MISSION_POS_TOL_CM)
        rt_tar.st_data.vel_x = speed_x;
    else if(err_x < -SIMPLE_MISSION_POS_TOL_CM)
        rt_tar.st_data.vel_x = -speed_x;
    else
        rt_tar.st_data.vel_x = 0;

    if(err_y > SIMPLE_MISSION_POS_TOL_CM)
        rt_tar.st_data.vel_y = speed_y;
    else if(err_y < -SIMPLE_MISSION_POS_TOL_CM)
        rt_tar.st_data.vel_y = -speed_y;
    else
        rt_tar.st_data.vel_y = 0;

    ...
}

它不是复杂的轨迹规划，而是“误差大就定速飞，误差小就停，连续稳定多次才算到点”。当前工程里还有几个关键参数：

参数	含义
SIMPLE_MISSION_DISTANCE_CM	正方形边长
SIMPLE_MISSION_SPEED_CMPS	巡航速度
SIMPLE_MISSION_SLOW_RADIUS_CM	接近目标点后降速半径
SIMPLE_MISSION_SLOW_SPEED_CMPS	慢速接近速度
SIMPLE_MISSION_POS_TOL_CM	到点误差容差
SIMPLE_MISSION_ARRIVE_STABLE_CNT	连续多少次在容差内才确认到点

假设 Automatic_Flay() 是 50 Hz，SIMPLE_MISSION_ARRIVE_STABLE_CNT = 40，那就表示飞机需要连续约 40 / 50 = 0.8 s 稳定在目标点附近，才进入下一段。

---

12. 坐标闭环控制：Station_fix_x/y

更传统的坐标闭环函数在：

Headware/coordinate_control.c

例如 Station_fix_x()：

void Station_fix_x(float x, float user_speedx)
{
    if(x > nowx)
    {
        if(x - nowx > 20)
            pwmoutx = user_speedx;
        else
            mypid_x(x);
    }

    if(x < nowx)
    {
        if(x - nowx < -20)
            pwmoutx = -user_speedx;
        else
            mypid_x(x);
    }

    rt_tar.st_data.vel_x = pwmoutx;
}

它的策略是：

• 距离目标较远时，用固定速度飞；
• 进入 20 cm 附近后，切到 PID 微调；
• 最终把速度目标写入 rt_tar.st_data.vel_x。

GO_Station() 中有更复杂的 63 点网格路径、航向旋转补偿等示例。如果你是第一次二次开发，建议先吃透 Simple_Mission_Goto()，再看 GO_Station()，否则很容易被比赛遗留变量绕晕。

---

13. 如何改成自己的路径

假设你要让飞机起飞后依次飞过 4 个点：

(0, 0) -> (100, 0) -> (100, 100) -> (0, 100)

最容易维护的写法是做一个航点数组：

typedef struct {
    float x;
    float y;
} Waypoint;

static Waypoint mission_wp[] = {
    {0, 0},
    {100, 0},
    {100, 100},
    {0, 100},
};

static uint8_t wp_index = 0;

然后在状态机中推进：

case 0:
{
    float tx = simple_start_x + mission_wp[wp_index].x;
    float ty = simple_start_y + mission_wp[wp_index].y;

    if(Simple_Mission_Goto(tx, ty))
    {
        wp_index++;
        if(wp_index >= sizeof(mission_wp) / sizeof(mission_wp[0]))
        {
            Control_Flow = 66;
            wp_index = 0;
        }
    }
    break;
}

这种写法比写很多个 case 0/case 1/case 2 更适合比赛临时改点位。改路径时只动数组，不动状态机骨架。

---

14. 推荐增加的失效保护

当前工程里已经有 chek_t265_ok() 这类 T265 超时计数，但自动状态机中还没有非常明确的“定位失效立即退出”保护。建议补上类似逻辑：

if(chek_t265_ok() > 100)
{
    rt_tar.st_data.vel_x = 0;
    rt_tar.st_data.vel_y = 0;
    rt_tar.st_data.vel_z = 0;
    OneKey_Land();
    Control_Flow = 99;
}

是否直接降落，要看比赛场地和飞控模式。如果场地狭窄、下方有障碍，可能更适合先悬停并等待人工接管。核心原则是：定位失效时，状态机不能继续按旧坐标往前飞。

还建议增加：

• t265_debug_quality 低于阈值时禁止进入自动任务；
• 起飞前检查 nowx/nowy 是否稳定；
• 每个航点设置最大执行时间，超时后退出；
• 降落前先清零 rt_tar.st_data.vel_x/y/z；
• 自动开关关闭时，不只复位状态机，还要清零速度目标。

---

15. 调试流程：从不飞到能飞

建议按这个顺序调，不要直接抱着飞机上天：

15.1 只烧录，不接外设

打开：

ProjectSTM32F407/ANO_LX_STM32F407.uvprojx

确认 Keil 能编译通过，下载后板子能正常上电，LED、遥控输入、匿名地面站通信基本正常。

15.2 接飞控，验证命令链路

先不接 T265，只测试：

• FC_Unlock() 是否能解锁；
• OneKey_Takeoff() 是否能被 ACK；
• OneKey_Land() 是否能降落；
• dt.wait_ck 是否会从 1 回到 0。

如果 dt.wait_ck 一直是 1，优先查 UART5 和凌霄飞控通信。

15.3 接光流/测距

确认：

• ano_of.of_alt_cm 随高度变化；
• ano_of.of1_dx/of1_dy 有合理速度；
• 凌霄飞控能收到 0x33/0x34 外部传感器帧。

15.4 接 T265/Jetson

确认：

• Jetson 串口输出是 500000；
• STM32 USART1 的 RX 接到了 Jetson TX；
• t265_rx_byte_cnt 增加；
• t265_frame_ok_cnt 增加；
• t265_checksum_fail_cnt 不持续暴涨；
• 手推飞机时 nowx/nowy 方向正确。

15.5 架高、拆桨、看电机输出

打开自动开关，观察状态机：

-4 -> -3 -> -2 -> 0 -> 1 -> 2 -> 66

此时重点看速度目标是否符合预期，不要急着装桨。

15.6 小速度、小距离首飞

第一次真实飞行建议：

• 高度 80 cm 左右；
• 路径边长 30 cm 到 50 cm；
• 水平速度不要太大；
• 旁边必须有人盯遥控器，随时关闭自动开关。

---

16. 常见问题排查

16.1 T265 没有数据

先看 t265_rx_byte_cnt。如果完全不动，通常是接线、波特率、串口号问题。当前源码 T265 进 USART1，不是 USART3。

16.2 有字节但没有完整帧

看 t265_checksum_fail_cnt。如果持续增加，检查：

• Jetson 发送协议是否为 AA FF 51 ... SC AC；
• 负载长度是否和 STM32 解析一致；
• 串口波特率是否都是 500000；
• 是否共地；
• USB-TTL 是否支持 500000 波特率。

16.3 飞机到点附近抖动

可能原因：

• 速度太大；
• 到点容差太小；
• T265 坐标噪声大；
• PID 参数过激；
• 高度或光流数据不稳定。

优先降低 SIMPLE_MISSION_SPEED_CMPS，增大 SIMPLE_MISSION_POS_TOL_CM，再调 PID。

16.4 飞机方向反了

不要先改 Simple_Mission_Goto()，先确认坐标定义。推荐在 Jetson 端改 --x-sign、--y-sign、--x-axis、--y-axis，让 nowx/nowy 与你的场地坐标一致。

16.5 一键命令偶尔不执行

检查 dt.wait_ck。如果上一条命令 ACK 没回来，新的 OneKey_Takeoff() 或 Horizontal_Move() 会返回 0。状态机中最好判断函数返回值，而不是默认命令一定发出。

---

17. 源码里建议优化的点

这份工程能跑比赛任务，但如果要做成稳定的二次开发模板，建议逐步优化：

1. T265 的 s16 解码不要用 *((s16 *)(data + offset))，改成显式小端解析，移植性更好；
2. 把航点路径从大量 case 改成数组驱动；
3. 把遥控自动开关、T265 质量、光流状态、飞控解锁状态统一做成 mission_can_run()；
4. 给每个任务状态加超时；
5. 所有退出分支都清零速度目标；
6. 把调试变量集中发到地面站，减少比赛现场盲调。

显式小端解析可以这样写：

static int16_t read_i16_le(const uint8_t *p)
{
    return (int16_t)((uint16_t)p[0] | ((uint16_t)p[1] << 8));
}

然后替换：

t265_debug_x = read_i16_le(data + 14);
t265_debug_y = read_i16_le(data + 16);

---

18. 二次开发最小模板

最后给一个适合新任务的状态机骨架：

void Automatic_Flay(void)
{
    if(rc_in.rc_ch.st_data.ch_[ch_6_aux2] <= 1700)
    {
        mission_reset();
        return;
    }

    if(!mission_sensor_ok())
    {
        mission_stop_or_land();
        return;
    }

    limit_height(60, 180);

    switch(Control_Flow)
    {
        case -4:
            mission_record_origin();
            Control_Flow = -3;
            break;

        case -3:
            if(FC_Unlock())
                mission_delay_1s_then_next();
            break;

        case -2:
            if(OneKey_Takeoff(Aim_Height) && hight_successed())
                Control_Flow = 0;
            break;

        case 0:
            mission_run_waypoints();
            break;

        case 66:
            mission_zero_velocity();
            OneKey_Land();
            break;

        default:
            mission_zero_velocity();
            break;
    }
}

这个模板的核心思想是：状态机只管流程，坐标控制只管到点，传感器检查只管能不能继续飞。三者分开之后，比赛当天改路径和查问题都会轻松很多。

---

19. 总结

这份 25路径规划(32接收） 工程的二次开发主线可以概括成一句话：

STM32F407 通过 USART1 接收 T265 坐标，通过 USART3 接收光流/高度，通过 UART5 与凌霄飞控交换命令和状态；用户只需要在 Headware/My_control.c 里基于 nowx/nowy/nowz 写任务状态机，再通过 rt_tar 或凌霄命令接口控制飞机。

真正开发时，建议按这个顺序推进：

1. 先确认 Keil 工程能编译下载；
2. 再确认 UART5 与凌霄飞控通信正常；
3. 再确认光流/高度和 T265 坐标分别正常；
4. 最后才改 Automatic_Flay() 写自己的路径任务。

只要串口路由不接错、坐标方向先调对、自动任务保留遥控总开关，这份工程就很适合作为电赛室内路径规划的二次开发起点。

---