凌霄运动源码学习与二次开发详解：任务调度、坐标控制、MID360/Jetson 接入与重要源码解析

摘要

本文围绕凌霄运动源码的 STM32F407 工程，系统梳理了项目的整体结构、任务调度方式、控制流程与二次开发入口。文章重点分析了 main.c、Ano_Scheduler.c、My_control.c、coordinate_control.c、mid360_receive.c、Drv_AnoOf.c、Drv_Uart.c 等关键模块，说明飞行任务如何从初始化、调度执行到坐标控制与外设接入逐步展开。同时结合 Jetson、激光雷达、摄像头等扩展场景，给出了航点修改、任务状态机调整、串口接入和控制参数优化的方法，并附上重要源码，便于学习、复现与后续移植。

项目背景

很多人第一次拿到凌霄工程，会先被目录吓到：既有 FcSrc、DriversBsp、DriversMcu，又有 ProjectSTM32F407、ProjectMSP432、ProjectTM4C123，再加上一层自定义的 Headware，如果没有梳理，基本很难判断到底该从哪一层下手。

这套工程的关键点在于：

• 底层飞控协议、调度器、驱动框架主要在 FcSrc、DriversBsp、DriversMcu。
• 真正和任务流程、航点、投放、降落、视觉联动相关的自定义逻辑主要在 Headware。
• 对于 STM32F407 版本，最值得关心的并不是旧示例里的简单一键任务，而是 Headware/My_control.c 里的自主任务状态机。

如果你的目标是“读懂项目并做二次开发”，那就不要一上来埋进全部底层库。正确方法是先抓主链路，再回头看被调用的控制函数和设备接入函数。

整理后的项目结构

结合当前工程实际内容，我建议把这个仓库按“开发时真正需要关注的层次”来理解：

project/
|-- Headware/                 自定义任务层，重点阅读
|   |-- My_control.c/h        任务状态机、航点切换、投放/降落
|   |-- coordinate_control.c/h 位置/高度/偏航控制
|   |-- mid360_receive.c/h    MID360 与 Jetson 数据解析
|   |-- User_Task.c/h         遥控触发、安全开关、简单示例
|   |-- User_DT.c/h           自定义上行调试数据
|   `-- Expand_IO.c/h         扩展 IO 与状态灯
|-- FcSrc/                    飞控协议和调度层
|   |-- main.c                程序入口
|   |-- Ano_Scheduler.c       周期调度器
|   `-- LX_FC_Fun.c/h         解锁、模式切换、起飞降落等封装
|-- DriversBsp/               板级与传感器基础支持
|   `-- Drv_AnoOf.c/h         光流/高度信息桥接
|-- DriversMcu/STM32F407/     STM32F407 驱动、CMSIS、StdPeriph
|   `-- Drivers/Drv_Uart.c    串口映射与中断接收
|-- ProjectSTM32F407/         STM32F407 的 Keil 工程入口
|-- ProjectMSP432/            其他平台移植，可忽略
|-- ProjectTM4C123/           其他平台移植，可忽略
`-- Doc/                      文档目录

如果你现在只做 STM32F407 自主飞行任务，那么建议先忽略 ProjectMSP432 和 ProjectTM4C123，把注意力集中在下面 4 个入口：

1. 编译入口：ProjectSTM32F407/ANO_LX_STM32F407.uvprojx
2. 运行入口：FcSrc/main.c
3. 调度入口：FcSrc/Ano_Scheduler.c
4. 任务入口：Headware/My_control.c 中的 Automatic_Flay()

这 4 个入口，就是我建议你后续二次开发时的“统一功能入口”。

功能介绍

从源码实际行为看，这个工程已经不是一个纯演示程序，而是一套比较完整的任务飞行框架，至少包含下面几类能力：

• 遥控触发与安全锁定。
• 切换程控模式、解锁、一键起飞。
• 基于坐标的定点飞行。
• 高度闭环、偏航保持、限高。
• 基于 Jetson 返回结果的任务分支。
• 基于 MID360/光流位置数据的定位。
• 指定投放点、识别点、降落点的任务切换。
• 扩展 IO、上位机调试数据回传。

换句话说，这套源码更像是“STM32 任务执行器 + 外部感知输入 + 凌霄飞控指令封装”的组合。

技术选型与依赖说明

1. 硬件依赖

• MCU：STM32F407VGTx
• 飞控协议：凌霄飞控通信协议
• 位置源：匿名光流 / 融合定位数据 / MID360 数据帧
• 外部计算单元：Jetson 或其他上位机
• 遥控接收机：用于模式切换、安全锁定、人工接管

2. 软件依赖

• IDE：Keil MDK 5
• MCU 库：STM32F4 CMSIS + STM32F4 StdPeriph
• 工程入口：ProjectSTM32F407/ANO_LX_STM32F407.uvprojx
• 自定义头文件目录：Headware/

3. Keil 工程真实包含路径

从工程文件可以看出，STM32F407 目标主要包含以下目录：

• FcSrc
• DriversBsp
• DriversMcu/STM32F407/Drivers
• DriversMcu/STM32F407/Libraries/STM32F4xx_StdPeriph_Driver/inc
• DriversMcu/STM32F407/Libraries/CMSIS/Include
• DriversMcu/STM32F407/Libraries/CMSIS/ST/STM32F4xx/Include
• Headware

这说明 Headware 不是附属演示目录，而是当前 STM32F407 工程真正参与编译的业务层。

4. 串口分工

从 DriversMcu/STM32F407/Drivers/Drv_Uart.c 的映射关系可以看出，这个工程已经把外设接入分工做得比较明确：

• USART2 -> Jetson_GetOneByte：接 Jetson / 上位机任务输入
• USART3 -> mid360_GetOneByte：接 MID360 数据
• UART4 -> AnoOF_GetOneByte：接匿名光流/高度数据
• UART5 -> ANO_DT_LX_Data_Receive_Prepare：接凌霄飞控协议数据
• USART1：当前工程里常被拿来挂发射机构或简单字符通道

开发思路：这套工程到底是怎么跑起来的

1. 主程序入口

程序从 FcSrc/main.c 开始，先执行 All_Init() 完成硬件初始化，再执行 Scheduler_Setup() 初始化调度器，然后进入死循环不停调用 Scheduler_Run()。

这一层不负责任务决策，只负责把系统带起来。

2. 调度器入口

真正把功能模块串起来的是 FcSrc/Ano_Scheduler.c。这个文件把任务分成了多个频率执行：

• 200Hz：执行 safety_switch()，做安全开关检测。
• 50Hz：执行 Automatic_Flay()，这是整个任务状态机的核心入口。
• 2Hz：执行 ANODT_SendF111(...)，把调试数据送出去。

所以对二次开发来说，最重要的一句话就是：

这个工程的自主任务不是在 main() 里跑，也不是在中断里硬写，而是在 50Hz 周期任务里由 Automatic_Flay() 稳定推进。

3. 自主任务状态机

Headware/My_control.c 里定义了 Control_Flow 这个大状态机。它的大致流程是：

1. 记录初始偏航角。
2. 解锁。
3. 一键起飞到目标高度。
4. 飞到二维码识别点或初始等待点。
5. 依次飞向多个任务点。
6. 根据 Jetson 返回的识别结果决定是否投放、是否继续。
7. 进入二维码点、降落点或返航点。
8. 触发降落流程。

这里最关键的不是每个 case 的细节，而是它的分层方式：

• My_control.h 负责定义任务坐标。
• My_control.c 负责决定现在应该飞去哪个点、执行哪个动作。
• coordinate_control.c 负责把“目标点”转换成速度控制量。
• LX_FC_Fun.c 负责把高层命令转换成凌霄飞控协议。

这种分法非常适合二次开发，因为你改任务流程时通常只需要改 My_control.h/.c，不需要一上来动底层驱动。

4. 位置控制链路

Headware/coordinate_control.c 的作用，是把“我要去哪里”变成“当前应该给飞控发多大的速度指令”。

它的核心做法是：

• 当前位置来源于 ano_of.intergral_x / intergral_y / of_alt_cm。
• 当前偏航角来源于 fc_att.st_data.yaw_x100。
• Station_fix_x() / Station_fix_y() 负责 X、Y 方向的位置闭环。
• Heigh_fix() 负责高度闭环。
• Yaw_fix() 负责偏航保持。
• 最终都写入 rt_tar.st_data.vel_x / vel_y / vel_z / yaw_dps。

也就是说，STM32 并不是直接算电机，而是把速度/角速度目标交给凌霄飞控。

5. 感知接入链路

Headware/mid360_receive.c 和 DriversBsp/Drv_AnoOf.c 共同构成了这个工程的感知输入层。

• mid360_receive.c 负责解析 MID360 数据和 Jetson 数据。
• Drv_AnoOf.c 负责解析匿名光流/高度数据，并在 MID360 失联时承担基础位置来源。
• NUC_receive_Command 是上位机识别结果落地后的共享结构体。
• ano_of 是任务控制和位置控制都在使用的定位状态结构体。

这也解释了为什么接入新设备时，不应该直接改任务状态机里的每个判断，而是应该先把新设备的数据规整到统一结构体里。

6. 命令下发链路

FcSrc/LX_FC_Fun.c 对外提供了解锁、锁定、切模式、起飞、降落、平移等动作封装。

这样做的好处是：

• 任务层不必关心协议帧细节。
• 任务层只需要调用 FC_Unlock()、OneKey_Takeoff()、OneKey_Land()、Horizontal_Move() 等接口。
• 后续如果协议改了，理论上只要维护这一层。

详细说明：你真正应该改哪些地方

1. 改航点和坐标，先看 Headware/My_control.h

这个头文件里定义了：

• 起飞点
• 多个中间任务点 Location_X1 ~ Location_X21
• 二维码识别点 QR_Code_X / QR_Code_Y
• 左右降落点 Land_Left_* / Land_Right_*

如果你只是想改路线，不改总体流程，优先改这里。

2. 改任务执行顺序，改 Headware/My_control.c

如果你要把流程从“识别-投放-降落”改成“巡点-拍照-返航”，那重点不是改坐标宏，而是改 Automatic_Flay() 里的 Control_Flow 状态切换逻辑。

一个比较稳妥的改法是：

1. 先保留 -4 / -3 / -2 这几个准备阶段。
2. 从 0 开始重新定义你的业务状态。
3. 每个状态只做一件事，例如“飞到点 A”“等待识别结果”“执行投放”“进入降落”。
4. 不要在一个状态里同时写多个条件分支和多层延时，否则后面很难维护。

3. 改控制手感和限幅，改 Headware/coordinate_control.c

如果飞机飞得太猛、太慢、刹不住或者上下抖，通常不要先怀疑状态机，而是先看这一层：

• Station_fix_x() / Station_fix_y() 的比例控制和限幅
• Heigh_fix() 的高度控制幅度
• limit_height() 的高度上下限
• 调用这些函数时传入的 user_speedx、user_speedy、ex_speed

这层决定的是“飞行质感”，不是“飞哪儿”。

4. 改遥控接管和安全逻辑，改 Headware/User_Task.c

safety_switch() 会检测通道并在条件满足时执行 FC_Lock()。这部分很关键，因为它决定了飞行过程中你能不能人工中断任务。

做二次开发时，建议你保留这条链路，并补充两点原则：

• 遥控中断优先级必须高于自动任务。
• 退出程控时不要直接丢控制量，最好先悬停或安全降落，再切换模式。

5. 改串口接线和设备接入，改 DriversMcu/STM32F407/Drivers/Drv_Uart.c

如果你换了 Jetson 接口、增加了新的雷达串口、或者想把某一路串口改成调试口，这里就是入口。

注意，这个文件做的是“串口字节流分发”，不是业务解析。更推荐的做法是：

• 在 Drv_Uart.c 里把字节流接到某个解析函数。
• 在单独的 xxx_receive.c 里完成协议解析。
• 解析后再把结果写入 NUC_receive_Command 或新的共享结构体。

如何接入其他设备

1. 接 Jetson

Jetson 最适合做“重计算、高层决策”，STM32 最适合做“实时执行、协议桥接”。

推荐接法是：

• Jetson 做视觉识别、SLAM、目标判别、二维码识别。
• Jetson 不直接控制电机，只把结果压缩成少量结构化字段。
• STM32 在 Jetson_DataAnl() 中解析这些字段，写入 NUC_receive_Command。
• My_control.c 只读取这些结果并决定下一状态。

这套工程现在就是这个思路，因此继续沿用即可。不要尝试让 STM32 直接处理图像。

2. 接激光雷达

如果是简单测高雷达，可以直接把高度值规整后送进 ano_of.of_alt_cm 或你自己的高度变量，再让 Heigh_fix() 使用。

如果是二维/三维激光雷达，建议不要把点云直接送给 STM32，而是：

• 在 Jetson 上做障碍提取、定位融合、目标点计算。
• 只把最终的障碍方向、偏移量、是否可降落等结果发给 STM32。
• STM32 继续作为任务状态机执行端。

3. 接摄像头

摄像头接入原则和 Jetson 一样：图像处理放到上位机，STM32 只吃结果。

比较推荐的数据字段包括：

• 目标是否检测到
• 目标在图像中的左右偏差
• 目标在图像中的前后偏差
• 是否允许投放
• 是否允许降落

这正好可以映射到当前 NUC_receive_Command 这类结构。

4. 接更多模块时的建议

后续模块多了以后，最怕的是每加一个设备就直接改 My_control.c。这样代码会非常快失控。更合理的结构是：

• 新设备一律先做独立 xxx_receive.c/.h。
• 所有解析后的结果统一汇总到共享状态结构体。
• 任务状态机只读“结果”，不读底层串口字节。
• 控制层只读“目标”，不关心识别算法来源。

运行方式

1. 打开工程

使用 Keil MDK 5 打开 ProjectSTM32F407/ANO_LX_STM32F407.uvprojx。

2. 检查硬件连接

• 飞控串口是否接到 UART5
• Jetson 是否接到 USART2
• MID360 是否接到 USART3
• 光流/高度模块是否接到 UART4
• 遥控器通道映射是否和当前代码一致

3. 关注启动链路

程序启动后，建议按下面顺序排查：

1. main() 是否正常进入主循环。
2. Scheduler_Run() 是否正常调到 50Hz。
3. Automatic_Flay() 是否被执行。
4. ano_of 是否在更新。
5. NUC_receive_Command 是否在更新。
6. rt_tar.st_data 是否被正确写入。

4. 调试建议

• 优先看 ANODT_SendF111(...) 送出的调试数据。
• 先单独验证定位，再验证高度，再验证状态机。
• 先在悬停和短距离定点飞行阶段把控制调顺，再做复杂任务。

扩展方向

基于现有工程，比较适合继续扩展的方向有：

• 把 Control_Flow 拆成更清晰的枚举状态，而不是大量裸数字。
• 给 NUC_receive_Command 增加版本号、校验和、超时判定。
• 给每个任务阶段加统一超时保护，防止卡死。
• 把“航点配置”从宏改成结构体数组，方便做多条航线。
• 增加“中断任务后安全悬停/返航/降落”的统一出口。
• 把调试输出标准化，便于后期接地面站或日志系统。

重要源码

说明：下面只附“自研逻辑和关键接入逻辑”，不附 CMSIS、StdPeriph、芯片厂家库代码。这样既方便阅读，也更适合 CSDN 单篇文章发布。

1. 遥控触发与安全控制头文件

文件路径：Headware/User_Task.h

#ifndef __USER_TASK_H
#define __USER_TASK_H

#include "SysConfig.h"


void UserTask_OneKeyCmd(void);
void safety_switch(void);
void t265_led(void);
void sem_rec_test(void);

#endif

2. 遥控触发、安全开关与旧示例任务

文件路径：Headware/User_Task.c

#include "User_Task.h"
#include "Drv_RcIn.h"
#include "LX_FC_Fun.h"
#include "LX_FC_State.h"
#include "Drv_Uart.h"
#include "t265_receive.h"

//安全开关
void safety_switch(void)
{
	if (rc_in.no_signal == 0)
	{
		if(rc_in.rc_ch.st_data.ch_[ch_8_aux4] > 1700)
		{
			FC_Lock();
		}
	}
}

//t265指示灯测试
void t265_led(void)
{
	if (rc_in.rc_ch.st_data.ch_[ch_6_aux2] > 800 && rc_in.rc_ch.st_data.ch_[ch_6_aux2] < 1200)
  {
		GPIO_SetBits(GPIOB,GPIO_Pin_0);								 //亮
		GPIO_SetBits(GPIOB,GPIO_Pin_1);
	}
	
	if (rc_in.rc_ch.st_data.ch_[ch_6_aux2] > 1300 && rc_in.rc_ch.st_data.ch_[ch_6_aux2] < 1700)
	{
		//中位，空操作
	}
	
	if(rc_in.rc_ch.st_data.ch_[ch_6_aux2]>1700 && rc_in.rc_ch.st_data.ch_[ch_6_aux2]<2200)
	{
		GPIO_ResetBits(GPIOB,GPIO_Pin_0);								//灭
		GPIO_ResetBits(GPIOB,GPIO_Pin_1);
	}
}

void sem_rec_test(void)
{	
	static int vnalga=0;
	//判断第6通道拨杆位置 800<CH_6<1200
	if (rc_in.rc_ch.st_data.ch_[ch_6_aux2] > 800 && rc_in.rc_ch.st_data.ch_[ch_6_aux2] < 1200)
	{
		if( vnalga== 0)
		{
			vnalga = 1;
		}
			
	}
	//判断第6通道拨杆位置 1300<CH_6<1700
	if (rc_in.rc_ch.st_data.ch_[ch_6_aux2] > 1300 && rc_in.rc_ch.st_data.ch_[ch_6_aux2] < 1700)
	{
		vnalga = 0;
	}
	//判断第6通道拨杆位置 1700<CH_6<2200 
	if(rc_in.rc_ch.st_data.ch_[ch_6_aux2]>1700 && rc_in.rc_ch.st_data.ch_[ch_6_aux2]<2200)
	{
		if( vnalga== 0)
		{
			vnalga=1;
		}
		
	}
	
}


void UserTask_OneKeyCmd(void)
{
    //////////////////////////////////////////////////////////////////////
    //一键起飞/降落例程
    //////////////////////////////////////////////////////////////////////
    //用静态变量记录一键起飞/降落指令已经执行。
    static u8 one_key_takeoff_f = 1, one_key_land_f = 1, one_key_mission_f = 0;
    static u8 mission_step;
    //判断有遥控信号才执行
    if (rc_in.no_signal == 0)
    {
        //判断第6通道拨杆位置 1300<CH_6<1700
        if (rc_in.rc_ch.st_data.ch_[ch_6_aux2] > 1300 && rc_in.rc_ch.st_data.ch_[ch_6_aux2] < 1700)
        {
            //还没有执行
            if (one_key_takeoff_f == 0)
            {
                //标记已经执行
                one_key_takeoff_f =
                    //执行一键起飞
                    OneKey_Takeoff(100); //参数单位：厘米； 0：默认上位机设置的高度。
            }
        }
        else
        {
            //复位标记，以便再次执行
            one_key_takeoff_f = 0;
        }
        //
        //判断第6通道拨杆位置 800<CH_6<1200
        if (rc_in.rc_ch.st_data.ch_[ch_6_aux2] > 800 && rc_in.rc_ch.st_data.ch_[ch_6_aux2] < 1200)
        {
            //还没有执行
            if (one_key_land_f == 0)
            {
                //标记已经执行
                one_key_land_f =
                    //执行一键降落
                    OneKey_Land();
            }
        }
        else
        {
            //复位标记，以便再次执行
            one_key_land_f = 0;
        }
	//判断第6通道拨杆位置 1700<CH_6<2200 
		if(rc_in.rc_ch.st_data.ch_[ch_6_aux2]>1700 && rc_in.rc_ch.st_data.ch_[ch_6_aux2]<2200)
		{
			//还没有执行
			if(one_key_mission_f ==0)
			{
				//标记已经执行
				one_key_mission_f = 1;
				//开始流程
				mission_step = 1;
			}
		}
		else
		{
			//复位标记，以便再次执行
			one_key_mission_f = 0;		
		}
		//
		if(one_key_mission_f==1)
		{
			static u16 time_dly_cnt_ms;
			//
			switch(mission_step)
			{
				case 0:
				{
					//reset
					time_dly_cnt_ms = 0;
				}
				break;
				case 1:
				{
					//切换程控模式
					mission_step += LX_Change_Mode(3);
				}
				break;
				case 2:
				{
					//解锁
					mission_step += FC_Unlock();
				}
				break;
				case 3:
				{
					//等2秒
					if(time_dly_cnt_ms<2000)
					{
						time_dly_cnt_ms+=20;//ms
					}
					else
					{
						time_dly_cnt_ms = 0;
						mission_step += 1;
					}
				}
				break;
				case 4:
				{
					//起飞
					mission_step += OneKey_Takeoff(100);//参数单位：厘米； 0：默认上位机设置的高度。
				}
				break;
				case 5:
				{
					
					//等10秒
					if(time_dly_cnt_ms<10000)
					{
						time_dly_cnt_ms+=20;//ms
					}
					else
					{
						time_dly_cnt_ms = 0;
						mission_step += 1;
					}					
				}
				break;
				case 6:
				{
					//前进1米
					mission_step += Horizontal_Move(100,150,0);
				}
				break;	
				case 7:
				{
					//等10秒
					if(time_dly_cnt_ms<10000)
					{
						time_dly_cnt_ms+=20;//ms
					}
					else
					{
						time_dly_cnt_ms = 0;
						mission_step += 1;
					}	
				}
				break;
				case 8:
				{
					//右移1米
					mission_step += Horizontal_Move(100,150,90);
				}
				break;
				case 9:
				{
					//等10秒
					if(time_dly_cnt_ms<10000)
					{
						time_dly_cnt_ms+=20;//ms
					}
					else
					{
						time_dly_cnt_ms = 0;
						mission_step += 1;
					}						
				}
				break;
				case 10:
				{
					//执行一键降落
					OneKey_Land();					
				}
				break;	
				case 11:
				{
					
				}
				break;
				case 12:
				{
				
				}
				break;
				case 13:
				{
					
				}
				break;
				case 14:
				{
					
				}
				break;				
				default:break;
			}
		}
		else
		{
			mission_step = 0;
		}
	}
    ////////////////////////////////////////////////////////////////////////
}

3. 任务坐标和任务接口定义

文件路径：Headware/My_control.h

#ifndef __MY_CONTROL_H__
#define __MY_CONTROL_H__

#include "sysconfig.h"
#include "LX_FC_Fun.h"
#include "Drv_Uart.h"

//起降点坐标
#define Take_Off_X1 0.0
#define Take_Off_Y1 0.0

//坐标系坐标
#define Location_X1 180	//遍历投放
#define Location_Y1 -160

#define Location_X2 360
#define Location_Y2 -160

#define Location_X3 180		
#define Location_Y3 160

#define Location_X4 360
#define Location_Y4 160

#define Location_X5 600		
#define Location_Y5 160

#define Location_X6 600		//特殊靶投放
#define Location_Y6 100

#define Location_X7 600		
#define Location_Y7 0

//#define Location_X8 600		//穿越圆环
//#define Location_Y8 -160
//#define Location_X88 600		
//#define Location_Y88 -160

#define Location_X9 440			
#define Location_Y9 -215

#define Location_X10 200
#define Location_Y10 -160

#define Location_X20 600
#define Location_Y20 160

#define Location_X21 200
#define Location_Y21 160

#define Location_X11 0
#define Location_Y11 0

#define Location_X12 0
#define Location_Y12 0


//二维码坐标
#define QR_Code_X 180
#define QR_Code_Y 0

//降落点坐标
#define Land_Left_X 0
#define Land_Left_Y 160

#define Land_Right_X 0
#define Land_Right_Y -160


void Automatic_Flay(void);
void mark_seng(void);
void Hard_thowd(void);


#endif

4. 自主任务状态机核心

文件路径：Headware/My_control.c

/**********************************************************************************
 * 文件名    : My_control.c
 * 功能描述  : 飞控控制算法 - 自主飞行和任务控制核心模块
 * 创建时间  : 2018年
 * 作者      : ANO技术团队
 * 版权声明  : Copyright (C) 2018 ANO Tech
 * 
 * 本文件包含飞控系统的核心控制算法，包括：
 * - 自主导航控制
 * - 目标点飞行
 * - 激光打击任务
 * - 定点降落控制
 * - 数据稳定性分析
 * - 任务流程管理
 **********************************************************************************/

#include "My_control.h"
#include "t265_receive.h"
#include "coordinate_control.h"
#include "Ano_Math.h"
#include "Drv_Sys.h"
#include <stdlib.h>
#include <math.h>

// 函数声明
void GO_Station(int Num);
void Stop_fix(uint8_t chx,uint8_t chy);
int GO_Station_Succeed(void);
int Launch_strikes(void);
void Fixed_Land(void);
void Laser_Strike(void);
int GO_Toufang_Station_Succeed(void);
void mark_seng(void);
void Uart_seng(void);
int FIX_hight_successed(void);

// 数据稳定性分析参数
#define MAX_DATA 50                    // 最大数据存储数量
#define STABLE_THRESHOLD 6             // 稳定性阈值
#define MIN_STABLE_COUNT 4             // 稳定数据最小数量

// 数据稳定性分析函数声明
double calculateStableAverage(double data);

//////////////////////////////////////////////////////////////////////
// 控制变量定义区域
//////////////////////////////////////////////////////////////////////

// 控制流程状态变量
static int Control_Flow = -4;          // 控制流程状态机
int reset_control = 0;                 // 控制重置标志位
int stop_station = 0;                  // 是否停靠标志，1为停靠

// 目标位置相关变量
float  Aim_sta_x=0,Aim_sta_y=0;        // 目标站点坐标
int fly_single = 1;                    // 是否启用单点飞行
uint8_t Go_Aim_height=0;               // 目标高度控制标志位

// 姿态控制相关变量
int Yaw_control_mark = 0;              // 偏航控制标记
int OK_to = 1;                         // 允许执行标志
float Init_Angle = 0;                  // 初始角速度
int8_t Angle_init = 0;                 // 初始角度赋值标志位

// 任务执行相关变量
uint8_t GoLaser_Strike = 0;            // 激光打击任务标志
int bark_tin = 0;                      // 任务执行时间计数
static uint16_t Time_dly_cnt_ms = 0;   // 延时计数器
static int Control_litter = 1;         // 小范围控制标志

// 位置和发射相关变量
int Location_X8=0;                     // 位置X坐标
int Location_Y8=0;                     // 位置Y坐标
int Launch=0;                          // 发射状态
int Launchcount = 0;                   // 发射计数

// 二维码识别相关
int QR=0;                              // 二维码识别结果

int Precision=0;
int ccc=0;

int touf = 0;

double average = 0;

int ch_tf = 0;

#define FIXDOPIT 	70	//Recognition height
#define AIMHEIGHT 150	//Flight altitude
int Weather_looped = 1;	//Whether it is looped or not


u16 Aim_Height = AIMHEIGHT;									//One-click take-off height


void Automatic_Flay(void)
{
	if(rc_in.rc_ch.st_data.ch_[ch_7_aux3]>1700)
	{//Process initiation marker
		if(Yaw_control_mark == 1)
		{
				
			Yaw_fix(Init_Angle);

		}
		
		limit_height(60,180);    		//�޸ߺ���
		//if(reset_control == 1 && land_biaozhiwei == 0)
		if(Go_Aim_height == 1)
		{
			Heigh_fix(Aim_Height,40);
		}
		
		if(OK_to == 1)
		{
			switch(Control_Flow)
			{//switch��ʼ����
				case -4:
					Angle_init = 1;
					Init_Angle = NowAngle;	
					Control_Flow = -3;
					break;
				
				case -3:
					FC_Unlock();
					if(Time_dly_cnt_ms<1000)
					{
						Time_dly_cnt_ms+=20;//ms
					}
					else
					{
						Time_dly_cnt_ms = 0;
						Go_Aim_height = 1;
						Control_Flow = -2;
						
					}
					
					break;
				case -2:
					OneKey_Takeoff(Aim_Height);
					if(ano_of.of_alt_cm<Aim_Height+5 && ano_of.of_alt_cm>Aim_Height-5) //�Ƿ񵽴���ɸ߶�
					{
						Yaw_control_mark = 1;
						Control_Flow = 0;
						reset_control = 1;
					}
					else
					{
					
					}
					break;
				case -1:
					GO_Station(0);
					
					if(GO_Station_Succeed())
					{
						NUC_receive_Command.QR_strikes = 0;
						Control_Flow = 0;
					}
					else
					{
					
					}						
					break;
				case 0:
					GO_Station(100);
					if(GO_Station_Succeed())
					{
						Stop_fix(NUC_receive_Command.Chx,NUC_receive_Command.Chy);

						if(Time_dly_cnt_ms<3000)
						{
							
							Time_dly_cnt_ms+=20;//ms
						}
						else
						{
							Time_dly_cnt_ms = 0;
							Control_Flow = 1;
							
						}
					}


					break;
				case 1:
					GO_Station(1);
					if(GO_Station_Succeed())
					{
						Stop_fix(NUC_receive_Command.Chx,NUC_receive_Command.Chy);
						Launch = NUC_receive_Command.QR_strikes;
						if(Launch == 1)
						{
							touf = 1;
						}
						
						switch(Control_litter)
							
						{
							case 1:
								GO_Station(1);
								if(GO_Station_Succeed())
								{
									Control_litter = 2;
								}
								break;
							case 2:
								Aim_Height = FIXDOPIT;
								if(FIX_hight_successed())
								{
									Control_litter = 3;
								}
								else
								{

								}
								break;
							case 3:
								if(touf==1 && Launchcount<2)
								{
									Time_dly_cnt_ms = 0;
									if(Launch_strikes())
									{
										Launchcount++;
										Launch=0;
										Control_litter = 4;
										touf = 0;
										
										NUC_receive_Command.QR_strikes=0;
									}
								}
								else
								{
									if(Launchcount>=2)
									{
										Control_litter = 4;
										Time_dly_cnt_ms = 0;
										NUC_receive_Command.QR_strikes=0;
									}
									else if(Time_dly_cnt_ms <=4000)
									{
										Time_dly_cnt_ms += 20;
									}
									else
									{
										Control_litter = 4;
										Time_dly_cnt_ms = 0;
										Launch=0;
										NUC_receive_Command.QR_strikes=0;
									}
								}								
								break;
							case 4:
								Aim_Height = AIMHEIGHT;
								if(FIX_hight_successed())
								{
									Launch=0;
									Control_litter = 1;
									Control_Flow = 2;
								}
								else
								{

								}						
								break;
						}
					}

	
					break;
				case 2:
					GO_Station(2);
					if(GO_Station_Succeed())
					{
						Stop_fix(NUC_receive_Command.Chx,NUC_receive_Command.Chy);
						Launch = NUC_receive_Command.QR_strikes;
						if(Launch == 1)
						{
							touf = 1;
						}
						
						switch(Control_litter)
							
						{
							case 1:
								GO_Station(2);
								if(GO_Station_Succeed())
								{
									Control_litter = 2;
								}
								break;
							case 2:
								Aim_Height = FIXDOPIT;
								if(FIX_hight_successed())
								{
									Control_litter = 3;
								}
								else
								{

								}
								break;
							case 3:
								if(touf==1 && Launchcount<2)
								{
									Time_dly_cnt_ms = 0;
									if(Launch_strikes())
									{
										Launchcount++;
										Launch=0;
										Control_litter = 4;
										touf = 0;
										
										NUC_receive_Command.QR_strikes=0;
									}
								}
								else
								{
									if(Launchcount>=2)
									{
										Control_litter = 4;
										Time_dly_cnt_ms = 0;
										NUC_receive_Command.QR_strikes=0;
									}
									else if(Time_dly_cnt_ms <=4000)
									{
										Time_dly_cnt_ms += 20;
									}
									else
									{
										Control_litter = 4;
										Time_dly_cnt_ms = 0;
										Launch=0;
										NUC_receive_Command.QR_strikes=0;
									}
								}								
								break;
							case 4:
								Aim_Height = AIMHEIGHT;
								if(FIX_hight_successed())
								{
									Launch=0;
									Control_litter = 1;
									Control_Flow = 3;
								}
								else
								{

								}						
								break;
						}
					}
			
					break;
					
				case 3:
					
					GO_Station(3);
					if(GO_Station_Succeed())
					{
						Stop_fix(NUC_receive_Command.Chx,NUC_receive_Command.Chy);
						Launch = NUC_receive_Command.QR_strikes;
						if(Launch == 1)
						{
							touf = 1;
						}
						
						switch(Control_litter)
							
						{
							case 1:
								GO_Station(3);
								if(GO_Station_Succeed())
								{
									Control_litter = 2;
								}
								break;
							case 2:
								Aim_Height = FIXDOPIT;
								if(FIX_hight_successed())
								{
									Control_litter = 3;
								}
								else
								{

								}
								break;
							case 3:
								if((touf==1 && Launchcount<2)||Launchcount == 0)
								{
									Time_dly_cnt_ms = 0;
									if(Launch_strikes())
									{
										Launchcount++;
										Launch=0;
										Control_litter = 4;
										touf = 0;
										
										NUC_receive_Command.QR_strikes=0;
									}
								}
								else
								{
									if(Launchcount>=2)
									{
										Control_litter = 4;
										Time_dly_cnt_ms = 0;
										NUC_receive_Command.QR_strikes=0;
									}
									else if(Time_dly_cnt_ms <=1500)
									{
										Time_dly_cnt_ms += 20;
									}
									else
									{
										Control_litter = 4;
										Time_dly_cnt_ms = 0;
										Launch=0;
										NUC_receive_Command.QR_strikes=0;
									}
								}								
								break;
							case 4:
								Aim_Height = AIMHEIGHT;
								if(FIX_hight_successed())
								{
									Launch=0;
									Control_litter = 1;
									Control_Flow = 4;
								}
								else
								{

								}						
								break;
						}
					}

					break;
					
				case 4:
					GO_Station(4);
					if(GO_Station_Succeed())
					{
						Stop_fix(NUC_receive_Command.Chx,NUC_receive_Command.Chy);
						Launch = NUC_receive_Command.QR_strikes;
						if(Launch == 1)
						{
							touf = 1;
						}
						
						switch(Control_litter)
							
						{
							case 1:
								GO_Station(4);
								if(GO_Station_Succeed())
								{
									Control_litter = 2;
								}
								break;
							case 2:
								Aim_Height = FIXDOPIT;
								if(FIX_hight_successed())
								{
									Control_litter = 3;
								}
								else
								{

								}
								break;
							case 3:
								if((touf==1 && Launchcount<2)||Launchcount == 1)
								{
									Time_dly_cnt_ms = 0;
									if(Launch_strikes())
									{
										Launchcount++;
										Launch=0;
										Control_litter = 4;
										touf = 0;
										
										NUC_receive_Command.QR_strikes=0;
									}
								}
								else
								{
									if(Launchcount>=2)
									{
										Control_litter = 4;
										Time_dly_cnt_ms = 0;
										NUC_receive_Command.QR_strikes=0;
									}
									else if(Time_dly_cnt_ms <=1500)
									{
										Time_dly_cnt_ms += 20;
									}
									else
									{
										Control_litter = 4;
										Time_dly_cnt_ms = 0;
										Launch=0;
										NUC_receive_Command.QR_strikes=0;
									}
								}								
								break;
							case 4:
								Aim_Height = AIMHEIGHT;
								if(FIX_hight_successed())
								{
									Launch=0;
									Control_litter = 1;
									Control_Flow = 5;
								}
								else
								{

								}						
								break;
						}
					}
			
					break;
					
				case 5:
					
					
					GO_Station(5);
					if(GO_Station_Succeed())
					{
						ccc = 1;
						Control_Flow = 6;
					}	
					else
					{
					
					}											
				
					break;
				case 6:
					
					
					GO_Station(6);
					if(GO_Station_Succeed())
					{
							Stop_fix(NUC_receive_Command.Chx,NUC_receive_Command.Chy);
							Time_dly_cnt_ms +=20;
							if(Time_dly_cnt_ms>2500)
							{
										USART_SendData(USART2,'c');

									Time_dly_cnt_ms=3001;
									if(Launch_strikes())
									{
										Launchcount++;
										Launch=0;
										Control_Flow = 7;
										
										Time_dly_cnt_ms=0;
										NUC_receive_Command.QR_strikes=0;
										NUC_receive_Command.Chx=0;
										NUC_receive_Command.Chy=0;
										NUC_receive_Command.Circular_distancex = 0;
										NUC_receive_Command.Circular_distancey = 0;
									}
							}

						
					}
					else
					{
					
					}										
			
					break;
					
				case 7:
					
					
						GO_Station(7);
					
					
					if(GO_Station_Succeed())
					{
						
						Time_dly_cnt_ms +=20;
						 for (int i = 0; i <MAX_DATA ; i++) {
								 average = calculateStableAverage(NUC_receive_Command. Circular_distancex);
								if (average != -1.0) {
										break;
								}
						}
						if(Time_dly_cnt_ms>=1000 && Location_Y8==0)
						{
							if(Weather_looped == 1)
							{
								Location_X8 = average+8;
							}
							else
							{
								Location_X8 = 0;
							}
							
							if(Location_X8 != 0 && Location_X8>480 && Location_X8 <720)
							{
								Control_Flow = 8;
								Time_dly_cnt_ms=0;
							}
							else
							{
								Control_Flow = 20;
								Time_dly_cnt_ms=0;
							}
							
						}
					
					}
					else
					{
					
					}		
				
					break;
					
				case 8:
					
					GO_Station(8);
					if(GO_Station_Succeed())
					{
						Control_Flow = 88;
					}
					else
					{
					
					}											
					break;
				case 9:
					
					GO_Station(9);
					if(GO_Station_Succeed())
					{
						Control_Flow = 10;
					}
					else
					{
					
					}											
					break;
				case 88:
					
					GO_Station(88);
					if(GO_Station_Succeed())
					{
						Control_Flow = 9;
					}
					else
					{
					
					}											
					break;
				case 10:
					
					GO_Station(10);
					if(GO_Station_Succeed())
					{
						
						if(NUC_receive_Command.land_useful == 0)
						{
								Control_Flow = 101;
						}
						else if(NUC_receive_Command.land_useful == 1)
						{
							Control_Flow = 102;
						}
						else
						{
							Control_Flow = 102;
						}
					}
					else
					{
					
					}											
					break;
				case 20:
					
					GO_Station(20);
					if(GO_Station_Succeed())
					{
						Control_Flow = 21;
					}
					else
					{
					
					}											
					break;
				case 21:
					
					GO_Station(21);
					if(GO_Station_Succeed())
					{
						
						if(NUC_receive_Command.land_useful == 0)
						{
								Control_Flow = 101;
						}
						else if(NUC_receive_Command.land_useful == 1)
						{
							Control_Flow = 102;
						}
						else
						{
							Control_Flow = 101;
						}
					}
					else
					{
					
					}											
					break;
				
				case 101:
					GO_Station(101);
					if(GO_Station_Succeed())
					{
						USART_SendData(USART2,'j');
						Control_Flow = 66;
					}
					else
					{
					
					}							
					break;	
				case 102:
					GO_Station(102);
					if(GO_Station_Succeed())
					{
						USART_SendData(USART2,'j');
						Control_Flow = 66;
					}
					else
					{
					
					}							
					break;						
				case 66:
						USART_SendData(USART2,'j');
						stop_station = 1;
						Go_Aim_height = 0;
						fly_single = 0;
						reset_control = 0;
						Yaw_control_mark = 0;
						Fixed_Land();
					
					break;
				case 99:
					break;
				}
			}//switch��������
		else
		{
			if(Go_Aim_height == 0)
			{
				OK_to = 0;
				Angle_init = 0;
				
			}
				
		}		
	}//���̿��ƽ�������
	
}

void GO_Station(int Num)
{
	float rotated_x,rotated_y,rotated_theta;
	switch(Num)
	{
		case 0:
			
			Aim_sta_x=Take_Off_X1;
			Aim_sta_y=Take_Off_Y1;
			break;
		case 1:	
			Aim_sta_x=Location_X1-5;
			Aim_sta_y=Location_Y1;
			break;
		case 2:
			Aim_sta_x=Location_X2-5;
			Aim_sta_y=Location_Y2;
			break;
		
		case 3:
			Aim_sta_x=Location_X3;
			Aim_sta_y=Location_Y3;
			break;
		case 4:
			Aim_sta_x=Location_X4;
			Aim_sta_y=Location_Y4;
			break;
		case 5:
			Aim_sta_x=Location_X5;
			Aim_sta_y=Location_Y5;
			break;
		case 6:
			Aim_sta_x=Location_X6;
			Aim_sta_y=Location_Y6-10;
			break;
		
		case 7:
			Aim_sta_x=Location_X7;
			Aim_sta_y=Location_Y7;
			break;
		case 8:
			Aim_sta_x=Location_X8;
			Aim_sta_y=Location_Y8;
			break;
		case 88:
			Aim_sta_x=Location_X8;
			Aim_sta_y=Location_Y8-215;
			break;
		case 9:
			Aim_sta_x=Location_X9;
			Aim_sta_y=Location_Y9;
			break;
		case 10:
			Aim_sta_x=Location_X10;
			Aim_sta_y=Location_Y10;
			break;
		case 20:
			Aim_sta_x=Location_X20;
			Aim_sta_y=Location_Y20;
			break;
		case 21:
			Aim_sta_x=Location_X21;
			Aim_sta_y=Location_Y21;
			break;
		case 11:
			Aim_sta_x=Location_X11;
			Aim_sta_y=Location_Y11;
			break;
		case 100:
			Aim_sta_x=QR_Code_X;
			Aim_sta_y=QR_Code_Y;
			break;
		case 101:
			Aim_sta_x=Land_Left_X;
			Aim_sta_y=Land_Left_Y;
			break;
		case 102:
			Aim_sta_x=Land_Right_X;
			Aim_sta_y=Land_Right_Y;			
			break;
	}
	
	if(stop_station == 0)
	{
		rotated_theta = NowAngle - Init_Angle;
		rotated_theta = my_deg_angle(rotated_theta);
		rotated_x = (Aim_sta_x-nowx)*my_cos(rotated_theta) - (Aim_sta_y-nowy)*my_sin(rotated_theta);
		rotated_y = (Aim_sta_x-nowx)*my_sin(rotated_theta) + (Aim_sta_y-nowy)*my_cos(rotated_theta);
		rotated_x = rotated_x+nowx;
		rotated_y = rotated_y+nowy; 
		
//		Station_slow_fix_x(Aim_sta_x);
//		Station_slow_fix_y(Aim_sta_y);
		Station_fix_x(rotated_x,23);
		Station_fix_y(rotated_y,23);
	}
	
}


/**********************************************************************************************************
*�� �� ��: Stop_fix(uint8_t ch)
*����˵��: ֹͣ���΢��
*��    ��: ΢������
*�� �� ֵ: ��
**********************************************************************************************************/
//void Stop_fix(uint8_t chx,uint8_t chy)
//{
//	
//	static float poutx,pouty;
//	
//	if(chx == 'C')
//	{
//		poutx=0;
//		pouty=0;
//	}

//	if(chy == 'E')
//	{
//		poutx=0;
//	}
//	else if(chy == 'W')
//	{
//		poutx = 8;
//		rt_tar.st_data.vel_x = poutx;	
//	}
//	else if(chy == 'S')
//	{
//		poutx = -8;
//		rt_tar.st_data.vel_x = poutx;	
//	}
//	
//	if(chx == 'Q')
//	{
//		pouty=0;
//	}
//	else if(chx == 'A')
//	{
//		pouty = 8;
//		rt_tar.st_data.vel_y = pouty;	
//	}
//	else if(chx == 'D')
//	{
//		pouty = -8;
//		rt_tar.st_data.vel_y = pouty;	
//	}
//	

//}


int GO_Station_Succeed(void)
{
	if((Aim_sta_x-nowx>-15)&&(Aim_sta_x-nowx<15)&&(Aim_sta_y-nowy<15)&&(Aim_sta_y-nowy>-15))
	{
		return 1;
	}
	else
	{
		return 0;
	}			
}

int Launch_strikes(void)
{
	static int Height_Control_Flow=0;		//�߶ȿ�������
	static int ex_now_height=0;
	static int dly_cnt_ms = 0;
	float toufang_gaodu = 50;
	if(ex_now_height==1)
	{
		Heigh_fix(toufang_gaodu,30);		//���ٽ���
	}
			
	switch(Height_Control_Flow)
	{
		case 0:
			Go_Aim_height = 0;					//ֹͣAim_height����
			ex_now_height=1;					//�����¸߶�
			Height_Control_Flow = 1;
			break;
		case 1:
			if(ano_of.of_alt_cm<toufang_gaodu+5 && ano_of.of_alt_cm>toufang_gaodu-5)
			{
				if(dly_cnt_ms<200)
				{
				Uart_seng();
				dly_cnt_ms+=20;
				}
				else
				{
				Height_Control_Flow = 2;
				}
			}
			break;
		case 2:
			if(dly_cnt_ms<1000)
			{
				dly_cnt_ms+=20;//ms
			}
			else
			{
				dly_cnt_ms = 0;
				ex_now_height=0; 				//�ر��¸߶�
				Height_Control_Flow = 3;	
				
			}
			
			break;
		case 3:
			ex_now_height=0;					//ֹͣ�¸߶�
			Heigh_fix(Aim_Height,30);			//���ٻָ������߶�
			if(ano_of.of_alt_cm<Aim_Height+5 && ano_of.of_alt_cm>Aim_Height-5)
			{	
				Height_Control_Flow = 4;
				Go_Aim_height = 1;					//����������
			}
			break;
		case 4:
			Height_Control_Flow = 0;
			return 1;
	}
	return 0;
}





void Fixed_Land(void)
{
	static float fixed_hi = 80;
	static int fixed_jiangluo = 0;
	static float fix_la_x,fix_la_y;
	Go_Aim_height=0;							//ֹͣ����
	Heigh_fix(fixed_hi,30);
	if(fixed_hi==80&&ano_of.of_alt_cm<fixed_hi+15 && ano_of.of_alt_cm>fixed_hi-15 &&fixed_jiangluo==0)
	{		
			Aim_sta_x = nowx - 15;
			Aim_sta_y = nowy + 0;
			fix_la_x = Aim_sta_x;
			fix_la_y=  Aim_sta_y;
			fixed_jiangluo = 1;
	}
	
	if(fixed_jiangluo == 1)
	{
		Aim_sta_x = fix_la_x;
		Aim_sta_y = fix_la_y-15;
		Station_slow_fix_x(Aim_sta_x);
		Station_slow_fix_y(Aim_sta_y);
		if(ano_of.of_alt_cm<fixed_hi+10 && ano_of.of_alt_cm>fixed_hi-10)
		{
			fixed_jiangluo = 0;
			fixed_hi =5;
			
		}
	}
	if(fixed_hi==5 && ano_of.of_alt_cm<fixed_hi+10 && ano_of.of_alt_cm>fixed_hi-10)
	{
			OneKey_Land();
			FC_Lock();
		
	}
	

}


/**********************************************************************************************************
*�� �� ��: Laser_Strike
*����˵��: ������
*��    ��: ��
*�� �� ֵ: ��
**********************************************************************************************************/
void Laser_Strike(void)
{
	static float fixed_hi = 135;
	static int fixed_jiangluo = 0;
	static float fix_la_x,fix_la_y;
	static int jiangluo = 0;
	static uint16_t Timecnt_ms = 0;

//	static int ex_now_height=0;
	Go_Aim_height = 0;
	Heigh_fix(fixed_hi,10);
	
	if(fixed_hi==135&&ano_of.of_alt_cm<fixed_hi+5 && ano_of.of_alt_cm>fixed_hi-5 &&fixed_jiangluo==0)
	{		
			Aim_sta_x = nowx - 0;
			Aim_sta_y = nowy + 0;
			fix_la_x = Aim_sta_x;
			fix_la_y=  Aim_sta_y;
			
			GPIO_SetBits(GPIOB,GPIO_Pin_1);
			if(Timecnt_ms<3000)
			{
				Timecnt_ms+=20;
			}
			else
			{
				fixed_jiangluo = 1;
				Timecnt_ms = 0;
			}
	}
	
	if(fixed_jiangluo == 1)
	{
		Aim_sta_x = fix_la_x;
		Aim_sta_y = fix_la_y;
		Station_slow_fix_x(Aim_sta_x);
		Station_slow_fix_y(Aim_sta_y);
		if(jiangluo == 0)
		{
			bark_tin = 1;
			jiangluo = 1;
		}
		if(GO_Toufang_Station_Succeed())//(Aim_sta_x-nowx>-10)&&(Aim_sta_x-nowx<10)&&(Aim_sta_y-nowy<10)&&(Aim_sta_y-nowy>-10)
		{
			jiangluo = 0;
			fixed_jiangluo = 0;
			Go_Aim_height=1;
			GoLaser_Strike = 2;	
			bark_tin = 0;
//			ex_now_height=1;
			GPIO_ResetBits(GPIOB,GPIO_Pin_1);
			
		
		}
	}
	
	
}



int GO_Toufang_Station_Succeed(void)
{
	if((Aim_sta_x-nowx>-10)&&(Aim_sta_x-nowx<10)&&(Aim_sta_y-nowy<10)&&(Aim_sta_y-nowy>-10))
	{
		return 1;
	}
	else
	{
		return 0;
	}		
		
}


int GO_cycle_Station_Succeed(void)
{
	if((Aim_sta_x-nowx>-5)&&(Aim_sta_x-nowx<5)&&(Aim_sta_y-nowy<5)&&(Aim_sta_y-nowy>-5))
	{
		return 1;
	}
	else
	{
		return 0;
	}		
		
}



void Stop_fix(uint8_t chx,uint8_t chy)
{
	
	static float poutx,pouty;
	
	

	if(chx == 1)
	{
		poutx=0;
		rt_tar.st_data.vel_x = poutx;	
	}
	else if(chx == 3)
	{
		poutx = 8;
		rt_tar.st_data.vel_x = poutx;	
	}
	else if(chx == 2)
	{
		poutx = -8;
		rt_tar.st_data.vel_x = poutx;	
	}
	
	if(chy == 3)
	{
		pouty=0;
		rt_tar.st_data.vel_y = pouty;	
	}
	else if(chy == 2)
	{
		pouty = 8;
		rt_tar.st_data.vel_y = pouty;	
	}
	else if(chy == 1)
	{
		pouty = -8;
		rt_tar.st_data.vel_y = pouty;	
	}
	

}
void Uart_seng(void)
{
	if(Launchcount == 0)
	{
		USART_SendData(USART1,'A');
	}
	if(Launchcount == 1)
	{
		USART_SendData(USART1,'B');
	}
	if(Launchcount == 2  || ccc==1)
	{
		USART_SendData(USART1,'C');
	}	
}

double calculateStableAverage(double data) {
    static double dataBuffer[MAX_DATA];
    static int dataCount = 0; 
    static int stableCount = 0; 

    
    dataBuffer[dataCount % MAX_DATA] = data;
    dataCount++;

    
    if (dataCount > 1) {
        double diff = fabs(data - dataBuffer[(dataCount - 2) % MAX_DATA]);
        if (diff <= STABLE_THRESHOLD) {
            stableCount++;
        } else {
            stableCount = 0; 
        }
    }

   
    if (stableCount >= MIN_STABLE_COUNT) {
        double sum = 0.0;
        int count = 0;
        for (int i = 0; i < dataCount; i++) {
            sum += dataBuffer[i % MAX_DATA];
            count++;
        }
        return sum / count; 
    }

    return -1.0; 
	}

int FIX_hight_successed()
{
	if(ano_of.of_alt_cm<Aim_Height+5 && ano_of.of_alt_cm>Aim_Height-5) 
	{
		return 1;
	}
	else
	{
		return 0;
	}
}


//					if(GO_Station_Succeed())
//					{
//						Stop_fix(NUC_receive_Command.Chx,NUC_receive_Command.Chy);

//						Launch = NUC_receive_Command.QR_strikes;
//						if(Launch == 1)
//						{
//							touf = 1;
//						}
//						if(touf==1 && Launchcount<2)
//						{
//							Time_dly_cnt_ms = 0;
//							if(Launch_strikes())
//							{
//								Launchcount++;
//								Launch=0;
//								Control_Flow = 2;
//								touf = 0;
//								
//								NUC_receive_Command.QR_strikes=0;
//							}
//						}
//						else
//						{
//							if(Launchcount>=2)
//							{
//								Control_Flow = 2;
//								Time_dly_cnt_ms = 0;
//								NUC_receive_Command.QR_strikes=0;
//							}
//							else if(Time_dly_cnt_ms <=3000)
//							{
//								Time_dly_cnt_ms += 20;
//							}
//							else
//							{
//								Control_Flow = 2;
//								Time_dly_cnt_ms = 0;
//								Launch=0;
//								NUC_receive_Command.QR_strikes=0;
//							}
//						}
//					}


//�ֶ�Ͷ�ź���
void Hard_thowd()
{
	if(rc_in.rc_ch.st_data.ch_[ch_8_aux4]>1400&&rc_in.rc_ch.st_data.ch_[ch_8_aux4]<1600)
	{
		USART_SendData(USART1,'A');
	}
	else if(rc_in.rc_ch.st_data.ch_[ch_8_aux4]>1700)
	{
		ch_tf = 1;
		USART_SendData(USART1,'B');
	}
	else if(rc_in.rc_ch.st_data.ch_[ch_8_aux4]<1300 && ch_tf == 1)
	{
		USART_SendData(USART1,'C');
	}
}

5. 坐标控制头文件

文件路径：Headware/coordinate_control.h

#ifndef __COORDINATE_CONTROL_H__
#define __COORDINATE_CONTROL_H__
#include "sysconfig.h"

#include "ANO_LX.h"											//实时控制帧来源，IMU数据来源
#include "Drv_AnoOf.h"									//光流数据/t265数据来源
#define MY_PPII 3.14159f

#define nowx (ano_of.intergral_x)   					//自身X坐标来源
#define nowy (ano_of.intergral_y)							//自身y坐标来源
#define nowz (ano_of.of_alt_cm)								//高度数据来源
#define NowAngle fc_att.st_data.yaw_x100/100	//航向角数据来源（需要手动配置输出）

void Station_fix_x(float x,float user_speedx);			//到达指定位置X，常用30
void Station_fix_y(float y,float user_speedy);			//到达指定位置y，常用30
void Heigh_fix(float expectz,float ex_speed);			//高度控制,常用20
void Yaw_fix(float ExAngle);					 			//正为顺时针旋转
void Yaw_station(float ExpectAngle);					//水平角度控制

void limit_height(float min,float max);
void Station_slow_fix_y(float y);
void Station_slow_fix_x(float x);


float my_deg_angle(float angle);
//		rt_tar.st_data.yaw_dps = 0;  //航向转动角速度，度每秒，逆时针为正
//		rt_tar.st_data.vel_x = 0;    //头向速度，厘米每秒
//		rt_tar.st_data.vel_y = 0;    //左向速度，厘米每秒
//		rt_tar.st_data.vel_z = 0;	 //天向速度，厘米每秒

#endif

6. 位置/高度/偏航控制核心

文件路径：Headware/coordinate_control.c

#include "coordinate_control.h"


void mypid_x(float expectx);										//水平pid（不需要调用）（可以重新调参）
void mypid_y(float expecty);										//水平pid（不需要调用）
void Height_fix(float expectz); 								//高度PID控制（不需要调用）

void Station_fix_x(float x,float user_speedx);	//到达指定位置X
void Station_fix_y(float y,float user_speedy);	//到达指定位置y
void Heigh_fix(float expectz,float ex_speed);		//高度控制
void Yaw_fix(float ExpectAngle);					 				//水平角度控制




float Fix_Yaw,Fix_Yaw_Station; 									//航向修正作用
float pwmoutx,pwmouty,pwmoutz;									//x,y方向速度赋值用
/**********************************************************************************************************
*函 数 名: Station_fix
*功能说明: 到达指定坐标
*参    数: 预期位置x，y
*返 回 值: 无
**********************************************************************************************************/
void Station_fix_x(float x,float user_speedx)
{

	if(x>nowx)
	{
		if(x-nowx>20)	
		{
			pwmoutx=user_speedx;
		}
		else
		{
			mypid_x(x);					//水平pid
		}
	}
	
	if(x<nowx)
	{
		if(x-nowx<-20)
		{
			pwmoutx=-user_speedx;

		}
		else
		{

			mypid_x(x);	
		}
	}
	
	
	
	rt_tar.st_data.vel_x = pwmoutx;
}

void Station_fix_y(float y,float user_speedy)
{
	if(y>nowy)
	{
		if(y-nowy>20)	
		{
			pwmouty=+user_speedy;
		}
		else
		{
			mypid_y(y);					//水平pid
		}
	}
	
	if(y<nowy)
	{
		if(y-nowy<-20)
		{
			pwmouty=-user_speedy;
		}
		else
		{
			mypid_y(y);	
		}
	}
	
	
	rt_tar.st_data.vel_y = pwmouty;
	
}





/**********************************************************************************************************
*函 数 名: Heigh_fix
*功能说明: 高度控制
*参    数: 期望高度
*返 回 值: 无
**********************************************************************************************************/
void Heigh_fix(float expectz,float ex_speed)					//高度控制函数
{
	//float nowz=ano_of.of_alt_cm;
	if(expectz-nowz>25)
	{
		pwmoutz = ex_speed;
	}
	else if(expectz-nowz<-25)
	{
		pwmoutz = -ex_speed;
	}
	else
	{
		Height_fix(expectz);
	}
	
	rt_tar.st_data.vel_z = pwmoutz;
}

void limit_height(float min,float max)
{
	if(nowz<min)
	{
		pwmoutz = 1;
	}
	
	if(nowz>max)
	{
		pwmoutz = -1;
	}
	rt_tar.st_data.vel_z = pwmoutz;
}






/**********************************************************************************************************
*函 数 名: Yaw_fix
*功能说明: 修正航向  //注意数据类型
*参    数: 预期值
*返 回 值: 无
**********************************************************************************************************/
void Yaw_fix(float ExpectAngle)
{
	
	s16 pwmoutYaw = 0;																	//航向角
	

	
	float kp=1.2,ki=0,kd=0.2;
	static float Error=0,lastError=0,lastlastError=0; 
	float limit=10;  
	  
	lastlastError= lastError;
	lastError= Error; 
	Error=ExpectAngle-NowAngle;
	
	pwmoutYaw=kp*Error+ki*(Error+lastError+lastlastError)+kd*(Error-lastError);
	if(pwmoutYaw> limit) 
	{
		pwmoutYaw= limit;                       	 //限幅
	}
	if(pwmoutYaw<-limit)
	{
		pwmoutYaw=limit*(-1);						//限幅
	}
	
	if(ExpectAngle - NowAngle > -4 && ExpectAngle - NowAngle < 4)
	{
		pwmoutYaw = 0;
	}
	//Program_Ctrl_User_Set_YAWdps(-pwmout);
//	rt_tar.st_data.yaw_dps = pwmoutYaw;
	my_yaw_dps = -pwmoutYaw;

}

//需要到达的角度
void Yaw_station(float ExAngle)
{
	
	s16 pwmoutYaw = 0;																	//航向角

	float kp=1.2,ki=0,kd=0.2;
	static float Error=0,lastError=0,lastlastError=0; 
	float limit=10;  
	  
	lastlastError= lastError;
	lastError= Error; 
	Error=ExAngle-NowAngle;
	
	pwmoutYaw=kp*Error+ki*(Error+lastError+lastlastError)+kd*(Error-lastError);
	if(pwmoutYaw> limit) 
	{
		pwmoutYaw= limit;                       	 //限幅
	}
	if(pwmoutYaw<-limit)
	{
		pwmoutYaw=limit*(-1);						//限幅
	}
	
	//Program_Ctrl_User_Set_YAWdps(-pwmout);
	rt_tar.st_data.yaw_dps = -pwmoutYaw;
//	my_yaw_dps = -pwmoutYaw;

}


void mypid_x(float expectx)  //水平pid
{
	
	float kpx=0.1,kix=0.03,kdx=0.1;
	
	static float Errorx=0; 
	static float lastErrorx=0;
	static float lastlastErrorx=0;
	static float limitx=25;


	lastlastErrorx= lastErrorx;
	lastErrorx= Errorx; 
	Errorx=expectx-nowx;
	

	pwmoutx=kpx*Errorx+kix*(Errorx+lastErrorx+lastlastErrorx)+kdx*(Errorx-lastErrorx);
	
	if(pwmoutx> limitx) 
	{
		pwmoutx= limitx;                        //限幅
	}  
	if(pwmoutx< -limitx) 
	{
		pwmoutx= limitx*(-1);                        //限幅
	}  
	

	
	
	rt_tar.st_data.vel_x = pwmoutx;
}



void mypid_y(float expecty)  //水平pid
{
	float kpy=0.1,kiy=0.03,kdy=0.1;
	
	static float Errory=0; 
	static float lastErrory=0;
	static float lastlastErrory=0;
	static float limity=25;


	lastlastErrory= lastErrory;
	lastErrory= Errory; 
	Errory=expecty-nowy;
	
	
	pwmouty=kpy*Errory+kiy*(Errory+lastErrory+lastlastErrory)+kdy*(Errory-lastErrory);
	
	if(pwmouty> limity) 
	{
		pwmouty= limity;                        //限幅
	}  
	if(pwmouty< -limity) 
	{
		pwmouty= limity*(-1);                        //限幅
	}  
	
	
	rt_tar.st_data.vel_y = pwmouty;

}


void Height_fix(float expectz)			//pid控制高度
{	
	
	float kp=1.2;   
	float ki=0.1;	
	float kd=0.1;	
	
	static float Error; 
	static float lastError;
	static float lastlastError;
	float limit=15;
	 
	lastlastError= lastError;
	lastError= Error; 
	Error=expectz-nowz;
	
	pwmoutz=kp*Error+ki*(Error+lastError+lastlastError)+kd*(Error-lastError);
	if(pwmoutz> limit) 
	{
		pwmoutz= limit;                        //限幅
	}
	if(pwmoutz< -limit) 
	{
		pwmoutz= -limit;                        //限幅
	}

	rt_tar.st_data.vel_z= pwmoutz;
}

float my_deg_angle(float angle)
{
	return angle/180*MY_PPII;
}

void Station_slow_fix_x(float x)//x为到达位置
{
	

	if(x>nowx)
	{
		if(x-nowx>20)	
		{
			pwmoutx=22;
		}
		else
		{
			mypid_x(x);					//水平pid
		}
	}
	
	if(x<nowx)
	{
		if(x-nowx<-20)
		{
			pwmoutx=-22;
		}
		else
		{
			mypid_x(x);	
		}
	}
	
	
	
	
	
	rt_tar.st_data.vel_x = pwmoutx;
}

void Station_slow_fix_y(float y)
{
	if(y>nowy)
	{
		if(y-nowy>20)	
		{
			pwmouty=+22;
		}
		else
		{
			mypid_y(y);					//水平pid
		}
	}
	
	if(y<nowy)
	{
		if(y-nowy<-20)
		{
			pwmouty=-22;
		}
		else
		{
			mypid_y(y);	
		}
	}
	
	
	rt_tar.st_data.vel_y = pwmouty;
	
}

7. MID360 与 Jetson 数据结构定义

文件路径：Headware/mid360_receive.h

#ifndef __MID360_RECEIVE_H__
#define __MID360_RECEIVE_H__

#include "SysConfig.h"

typedef struct
{
	u8 	land_useful;
	u8 	land_directionx;
	u8 	land_directiony;
	s16 land_distancex;
	s16 land_distancey;

	u8 	Chx;//΢��x����
	u8 	Chy;
	u8  stop;
	u8  useful;
	s16 Circular_distancex;	//Բ��x����
	s16 Circular_distancey;
	
	u8 QR_strikes;
	
	
} _Command_receive_st;

extern _Command_receive_st NUC_receive_Command;

u16 chek_mid360_ok(void);
void mid360_GetOneByte(uint8_t data);
void Jetson_GetOneByte(uint8_t bata);



#endif

8. MID360 与 Jetson 串口解析

文件路径：Headware/mid360_receive.c

#include "mid360_receive.h"
#include "Drv_AnoOf.h"
#include "ANO_DT_LX.h"

static void mid360_DataAnl(uint8_t *data, uint8_t len);
static void Jetson_DataAnl(uint8_t *data, uint8_t len);

static uint8_t _datatemp[50];
static uint8_t _batatemp[50];

static u16 mid360_ok=0;

_Command_receive_st NUC_receive_Command = {0};

u16 chek_mid360_ok(void)
{
	mid360_ok++;
	if(mid360_ok > 850)
		mid360_ok = 900;
	return mid360_ok;
}

//mid360_GetOneByte是初级数据解析函数，串口每接收到一字节光流数据，调用本函数一次，函数参数就是串口收到的数据
//当本函数多次被调用，最终接收到完整的一帧数据后，会自动调用数据解析函数AnoOF_DataAnl
void mid360_GetOneByte(uint8_t data)
{
	static u8 _data_len = 0, _data_cnt = 0;
	static u8 rxstate = 0;

	if (rxstate == 0 && data == 0xAA)
	{
		rxstate = 1;
		_datatemp[0] = data;
	}
	else if (rxstate == 1 && (data == HW_TYPE || data == HW_ALL))
	{
		rxstate = 2;
		_datatemp[1] = data;
	}
	else if (rxstate == 2)
	{
		rxstate = 3;
		_datatemp[2] = data;
	}
	else if (rxstate == 3 && data < 250)
	{
		rxstate = 4;
		_datatemp[3] = data;
		_data_len = data;
		_data_cnt = 0;
	}
	else if (rxstate == 4 && _data_len > 0)
	{
		_data_len--;
		_datatemp[4 + _data_cnt++] = data;
		if (_data_len == 0)
			rxstate = 5;
	}
	else if (rxstate == 5)
	{
		rxstate = 6;
		_datatemp[4 + _data_cnt++] = data;
	}
	else if (rxstate == 6)
	{
		rxstate = 0;
		_datatemp[4 + _data_cnt] = data;

		
		
		mid360_DataAnl(_datatemp, _data_cnt + 5); //
	}
	else
	{
		rxstate = 0;
	}
}
//AnoOF_DataAnl为光流数据解析函数，可以通过本函数得到光流模块输出的各项数据
//具体数据的意义，请参照匿名光流模块使用手册，有详细的介绍

static void mid360_DataAnl(uint8_t *data, uint8_t len)
{
	u8 check_sum1 = 0, check_sum2 = 0;
	if (*(data + 3) != (len - 6)) //判断数据长度是否正确
		return;
	for (u8 i = 0; i < len - 2; i++)
	{
		check_sum1 += *(data + i);
		check_sum2 += check_sum1;
	}
	if ((check_sum1 != *(data + len - 2)) || (check_sum2 != *(data + len - 1))) //判断sum校验
		return;
	//================================================================================

	if (*(data + 2) == 0X51) //光流信息
	{
		if (*(data + 4) == 2) //惯导融合后光流信息,特别注意
		{
			mid360_ok = 0;
			GPIO_ResetBits(GPIOC,GPIO_Pin_6);			//灭
			
			ano_of.of2_sta = *(data + 5);					//sta恒为1代表有效
			ano_of.of1_sta = *(data + 5);					//sta恒为1代表有效
			
			//		ano_of.of1_dx = *((s16 *)(data + 6));
			//		ano_of.of1_dy = *((s16 *)(data + 8));
			
			
			//		ano_of.of2_dx_fix = *((s16 *)(data + 6));				//X方向速度信息
			//		ano_of.of2_dy_fix = *((s16 *)(data + 8));				//Y方向速度信息
			// ano_of.of2_dx= *((s16 *)(data + 10));			//X方向加速度信息
			NUC_receive_Command. Circular_distancex = *((s16 *)(data + 10));

			// ano_of.of2_dy = *((s16 *)(data + 12));			//Y方向加速度信息
			ano_of.intergral_x = *((s16 *)(data + 14));			//X方向位置信息
			ano_of.intergral_y = *((s16 *)(data + 16));			//Y方向位置信息
			ano_of.of_quality = *(data + 14);					//恒为254
			
			ano_of.of_update_cnt++;
			

			
		}
	}
}


void Jetson_GetOneByte(uint8_t bata)
{
	static u8 _bata_len = 0, _bata_cnt = 0;
	static u8 txstate = 0;

	if (txstate == 0 && bata == 0xAA)
	{

		txstate = 1;
		_batatemp[0] = bata;
	}
	else if (txstate == 1 && (bata == HW_TYPE || bata == HW_ALL))
	{
		txstate = 2;
		_batatemp[1] = bata;
	}
	else if (txstate == 2)
	{
		txstate = 3;
		_batatemp[2] = bata;
	}
	else if (txstate == 3 && bata < 250)
	{
		txstate = 4;
		_batatemp[3] = bata;
		_bata_len = bata;
		_bata_cnt = 0;
	}
	else if (txstate == 4 && _bata_len > 0)
	{
		_bata_len--;
		_batatemp[4 + _bata_cnt++] = bata;
		if (_bata_len == 0)
			txstate = 5;
	}
	else if (txstate == 5)
	{
		txstate = 6;
		_batatemp[4 + _bata_cnt++] = bata;
	}
	else if (txstate == 6)
	{
		txstate = 0;
		_batatemp[4 + _bata_cnt] = bata;

		

		Jetson_DataAnl(_batatemp, _bata_cnt + 5); //
	}
	else
	{
		txstate = 0;
	}
}

static void Jetson_DataAnl(uint8_t *data, uint8_t len)
{

	u8 check_sum1 = 0, check_sum2 = 0;
	if (*(data + 3) != (len - 6)) //判断数据长度是否正确
		return;
	for (u8 i = 0; i < len - 2; i++)
	{
		check_sum1 += *(data + i);
		check_sum2 += check_sum1;
	}
			
	if ((check_sum1 != *(data + len - 2)) || (check_sum2 != *(data + len - 1))) //判断sum校验
		return;
	//

	if (*(data + 2) == 0X52) 
	{
		GPIO_ResetBits(GPIOB,GPIO_Pin_0);			//灭
		//		NUC_receive_Command.QR_strikes 	= *(data + 4);		//是否投放
		NUC_receive_Command.Chx = *(data + 6); //1:中间 2：后退 3：前进
		NUC_receive_Command.Chy = *(data + 7);	//1：向右，2：向左，3：中间
		NUC_receive_Command.stop = *(data + 8);//	c：不动
		NUC_receive_Command.QR_strikes = *(data + 9);//投放	t/1：投放
		//		NUC_receive_Command.Circular_distancex = *(data + 7);
		//		NUC_receive_Command.Circular_distancey = *(data + 9);
		NUC_receive_Command.land_useful = *(data + 10); //0:左
		//		NUC_receive_Command.land_distancex = *(data + 13);
		//		NUC_receive_Command.land_distancey = *(data + 8);
	}
	
	
}

9. 自定义调试数据头文件

文件路径：Headware/User_DT.h

#ifndef __USER_DT_H__
#define __USER_DT_H__

#include "sysconfig.h"

extern uint8_t C8T6_commod;

void ANODT_SendF1(s32 _a,s32 _b,s32 _c,s32 _d); 
void ANODT_SendF2(int16_t _a,int16_t _b,int16_t _c,int16_t _d);
void ANODT_Send54(s16 _a,s16 _b);
void ANODT_Send53(s16 _a,s16 _b);
void ANODT_Send56(int8_t _a);
void C8T6_receive(uint8_t data);
void ANODT_SendF3(s32 _a,s32 _b,s32 _c,s32 _d);
void ANODT_SendF4(int16_t _a,int16_t _b,int16_t _c,int16_t _d);
void ANODT_Send57(s32 _a,s32 _b,s32 _c,s32 _d);


#endif

10. 自定义调试数据上报

文件路径：Headware/User_DT.c

#include "User_DT.h"
#include "Drv_Uart.h"


//前面四帧给lcd屏幕发送数据要求
u8 DataToSend[100];

uint8_t C8T6_Receive;
uint8_t C8T6_commod = 0;


void ANODT_SendF1(s32 _a,s32 _b,s32 _c,s32 _d)
{
	u8 _cnt = 0;
	u8 sc =0,ac=0;
	DataToSend[_cnt++] = 0xAA;
	DataToSend[_cnt++] = 0xFF;
	DataToSend[_cnt++] = 0x55;
	DataToSend[_cnt++] = 16;
	
	DataToSend[_cnt++] = BYTE0(_a);
	DataToSend[_cnt++] = BYTE1(_a);
	DataToSend[_cnt++] = BYTE2(_a);
	DataToSend[_cnt++] = BYTE3(_a);
	
	DataToSend[_cnt++] = BYTE0(_b);
	DataToSend[_cnt++] = BYTE1(_b);
	DataToSend[_cnt++] = BYTE2(_b);
	DataToSend[_cnt++] = BYTE3(_b);
	
	DataToSend[_cnt++] = BYTE0(_c);
	DataToSend[_cnt++] = BYTE1(_c);
	DataToSend[_cnt++] = BYTE2(_c);
	DataToSend[_cnt++] = BYTE3(_c);
	
	DataToSend[_cnt++] = BYTE0(_d);
	DataToSend[_cnt++] = BYTE1(_d);
	DataToSend[_cnt++] = BYTE2(_d);
	DataToSend[_cnt++] = BYTE3(_d);
	
	for(u8 i=0; i<DataToSend[3]+4;i++)
	{
		sc += DataToSend[i];
		ac += sc;
	}
	
	DataToSend[_cnt++] = sc;
	DataToSend[_cnt++] = ac;
	
	DrvUart2SendBuf(DataToSend,_cnt);
	
}

void ANODT_SendF2(int16_t _a,int16_t _b,int16_t _c,int16_t _d)
{
	u8 _cnt = 0;
	u8 sc =0,ac=0;
	DataToSend[_cnt++] = 0xAA;
	DataToSend[_cnt++] = 0xFF;
	DataToSend[_cnt++] = 0x56;
	DataToSend[_cnt++] = 8;
	
	DataToSend[_cnt++] = BYTE0(_a);
	DataToSend[_cnt++] = BYTE1(_a);

	
	DataToSend[_cnt++] = BYTE0(_b);
	DataToSend[_cnt++] = BYTE1(_b);

	
	DataToSend[_cnt++] = BYTE0(_c);
	DataToSend[_cnt++] = BYTE1(_c);

	
	DataToSend[_cnt++] = BYTE0(_d);
	DataToSend[_cnt++] = BYTE1(_d);

	
	for(u8 i=0; i<DataToSend[3]+4;i++)
	{
		sc += DataToSend[i];
		ac += sc;
	}
	
	DataToSend[_cnt++] = sc;
	DataToSend[_cnt++] = ac;
	
	DrvUart2SendBuf(DataToSend,_cnt);
	
}

void ANODT_Send54(s16 _a,s16 _b)
{
	u8 _cnt = 0;
	u8 sc =0,ac=0;
	DataToSend[_cnt++] = 0xAA;
	DataToSend[_cnt++] = 0xFF;
	DataToSend[_cnt++] = 0x54;
	DataToSend[_cnt++] = 4;
	
	DataToSend[_cnt++] = BYTE0(_a);
	DataToSend[_cnt++] = BYTE1(_a);

	
	DataToSend[_cnt++] = BYTE0(_b);
	DataToSend[_cnt++] = BYTE1(_b);
	
	for(u8 i=0; i<DataToSend[3]+4;i++)
	{
		sc += DataToSend[i];
		ac += sc;
	}
	
	DataToSend[_cnt++] = sc;
	DataToSend[_cnt++] = ac;
	
	DrvUart2SendBuf(DataToSend,_cnt);
	
}

void ANODT_Send53(s16 _a,s16 _b)
{
	u8 _cnt = 0;
	u8 sc =0,ac=0;
	DataToSend[_cnt++] = 0xAA;
	DataToSend[_cnt++] = 0xFF;
	DataToSend[_cnt++] = 0x53;
	DataToSend[_cnt++] = 4;
	
	DataToSend[_cnt++] = BYTE0(_a);
	DataToSend[_cnt++] = BYTE1(_a);

	
	DataToSend[_cnt++] = BYTE0(_b);
	DataToSend[_cnt++] = BYTE1(_b);
	
	for(u8 i=0; i<DataToSend[3]+4;i++)
	{
		sc += DataToSend[i];
		ac += sc;
	}
	
	DataToSend[_cnt++] = sc;
	DataToSend[_cnt++] = ac;
	
	DrvUart2SendBuf(DataToSend,_cnt);
	
}



void ANODT_Send56(int8_t _a)
{
	u8 _cnt = 0;
	u8 sc =0,ac=0;
	DataToSend[_cnt++] = 0xAA;
	DataToSend[_cnt++] = 0xFF;
	DataToSend[_cnt++] = 0x53;
	DataToSend[_cnt++] = 1;
	
	DataToSend[_cnt++] = BYTE0(_a);

	
	for(u8 i=0; i<DataToSend[3]+4;i++)
	{
		sc += DataToSend[i];
		ac += sc;
	}
	
	DataToSend[_cnt++] = sc;
	DataToSend[_cnt++] = ac;
	
	DrvUart2SendBuf(DataToSend,_cnt);
	
}
//F3和F4帧主要是给上位机查看数据debug的
void ANODT_SendF3(s32 _a,s32 _b,s32 _c,s32 _d)
{
	u8 _cnt = 0;
	u8 sc =0,ac=0;
	DataToSend[_cnt++] = 0xAA;
	DataToSend[_cnt++] = 0xFF;
	DataToSend[_cnt++] = 0x56;
	DataToSend[_cnt++] = 16;
	
	DataToSend[_cnt++] = BYTE0(_a);
	DataToSend[_cnt++] = BYTE1(_a);
	DataToSend[_cnt++] = BYTE2(_a);
	DataToSend[_cnt++] = BYTE3(_a);
	
	DataToSend[_cnt++] = BYTE0(_b);
	DataToSend[_cnt++] = BYTE1(_b);
	DataToSend[_cnt++] = BYTE2(_b);
	DataToSend[_cnt++] = BYTE3(_b);
	
	DataToSend[_cnt++] = BYTE0(_c);
	DataToSend[_cnt++] = BYTE1(_c);
	DataToSend[_cnt++] = BYTE2(_c);
	DataToSend[_cnt++] = BYTE3(_c);
	
	DataToSend[_cnt++] = BYTE0(_d);
	DataToSend[_cnt++] = BYTE1(_d);
	DataToSend[_cnt++] = BYTE2(_d);
	DataToSend[_cnt++] = BYTE3(_d);
	
	for(u8 i=0; i<DataToSend[3]+4;i++)
	{
		sc += DataToSend[i];
		ac += sc;
	}
	
	DataToSend[_cnt++] = sc;
	DataToSend[_cnt++] = ac;
	
	DrvUart2SendBuf(DataToSend,_cnt);
	
}

//F3和F4帧主要是给上位机查看数据debug的
void ANODT_Send57(s32 _a,s32 _b,s32 _c,s32 _d)
{
	u8 _cnt = 0;
	u8 sc =0,ac=0;
	DataToSend[_cnt++] = 0xAA;
	DataToSend[_cnt++] = 0xFF;
	DataToSend[_cnt++] = 0x57;
	DataToSend[_cnt++] = 16;
	
	DataToSend[_cnt++] = BYTE0(_a);
	DataToSend[_cnt++] = BYTE1(_a);
	DataToSend[_cnt++] = BYTE2(_a);
	DataToSend[_cnt++] = BYTE3(_a);
	
	DataToSend[_cnt++] = BYTE0(_b);
	DataToSend[_cnt++] = BYTE1(_b);
	DataToSend[_cnt++] = BYTE2(_b);
	DataToSend[_cnt++] = BYTE3(_b);
	
	DataToSend[_cnt++] = BYTE0(_c);
	DataToSend[_cnt++] = BYTE1(_c);
	DataToSend[_cnt++] = BYTE2(_c);
	DataToSend[_cnt++] = BYTE3(_c);
	
	DataToSend[_cnt++] = BYTE0(_d);
	DataToSend[_cnt++] = BYTE1(_d);
	DataToSend[_cnt++] = BYTE2(_d);
	DataToSend[_cnt++] = BYTE3(_d);
	
	for(u8 i=0; i<DataToSend[3]+4;i++)
	{
		sc += DataToSend[i];
		ac += sc;
	}
	
	DataToSend[_cnt++] = sc;
	DataToSend[_cnt++] = ac;
	
	DrvUart2SendBuf(DataToSend,_cnt);
	
}


void ANODT_SendF4(int16_t _a,int16_t _b,int16_t _c,int16_t _d)
{
	u8 _cnt = 0;
	u8 sc =0,ac=0;
	DataToSend[_cnt++] = 0xAA;
	DataToSend[_cnt++] = 0xFF;
	DataToSend[_cnt++] = 0xF4;
	DataToSend[_cnt++] = 8;
	
	DataToSend[_cnt++] = BYTE0(_a);
	DataToSend[_cnt++] = BYTE1(_a);

	
	DataToSend[_cnt++] = BYTE0(_b);
	DataToSend[_cnt++] = BYTE1(_b);

	
	DataToSend[_cnt++] = BYTE0(_c);
	DataToSend[_cnt++] = BYTE1(_c);

	
	DataToSend[_cnt++] = BYTE0(_d);
	DataToSend[_cnt++] = BYTE1(_d);

	
	for(u8 i=0; i<DataToSend[3]+4;i++)
	{
		sc += DataToSend[i];
		ac += sc;
	}
	
	DataToSend[_cnt++] = sc;
	DataToSend[_cnt++] = ac;
	
	DrvUart2SendBuf(DataToSend,_cnt);
	
}

void C8T6_receive(uint8_t data)
{
	C8T6_Receive = data;
	if(C8T6_Receive == 'K')
		C8T6_commod = 1;
}

11. 扩展 IO 头文件

文件路径：Headware/Expand_IO.h

#ifndef __EXPAND_IO_H__
#define __EXPAND_IO_H__
#include "sysconfig.h"


void All_IO_Init(void);
void IO3_Init(void);
void IO4_Init(void);

#endif

12. 扩展 IO 控制

文件路径：Headware/Expand_IO.c

#include "stm32f4xx.h"                  // Device header

/*
		PC6-TIM3_CH1-IO1			//接收定位数据显示
		PC7-TIM3_CH2-SBUS_IN-IO2....不能用，危险，接到信号线了
		PB0-TIM3_CH3-IO3
		PB1-TIM3_CH4-IO4			//激光笔
		PD12-TIM4_CH1-IO5
		PD13_TIM4_CH2-IO6
		PD14_TIM4_CH3-IO7
		PD15_TIM4_CH4-IO8
		
*/


void All_IO_Init(void) 		//不再初始化IO2
{
	GPIO_InitTypeDef GPIO_InitStructure;
	GPIO_StructInit(&GPIO_InitStructure);

	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC|RCC_AHB1Periph_GPIOD|RCC_AHB1Periph_GPIOB,ENABLE);

	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;//|GPIO_Pin_7;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
	GPIO_Init(GPIOC,&GPIO_InitStructure);
	GPIO_SetBits(GPIOC,GPIO_Pin_6);
	//GPIO_ResetBits(GPIOC,GPIO_Pin_6);

	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
	GPIO_Init(GPIOB,&GPIO_InitStructure);
	GPIO_SetBits(GPIOB,GPIO_Pin_0);
	GPIO_ResetBits(GPIOB,GPIO_Pin_1);

	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12|GPIO_Pin_13|GPIO_Pin_14|GPIO_Pin_15;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
	GPIO_Init(GPIOD,&GPIO_InitStructure);
	GPIO_ResetBits(GPIOD,GPIO_Pin_12|GPIO_Pin_13|GPIO_Pin_14|GPIO_Pin_15);

	
}

void IO3_Init(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	GPIO_StructInit(&GPIO_InitStructure);
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB,ENABLE);
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
	GPIO_Init(GPIOB,&GPIO_InitStructure);
	GPIO_SetBits(GPIOB,GPIO_Pin_0);

}

void IO4_Init(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	GPIO_StructInit(&GPIO_InitStructure);
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB,ENABLE);
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
	GPIO_Init(GPIOB,&GPIO_InitStructure);
	GPIO_ResetBits(GPIOB,GPIO_Pin_1);
}

13. 调度器源码

文件路径：FcSrc/Ano_Scheduler.c

/**********************************************************************************
 * 文件名    : Ano_Scheduler.c
 * 功能描述  : 任务调度器 - 飞控系统多任务调度管理
 * 创建时间  : 2017年
 * 作者      : ANO技术团队
 * 版权声明  : Copyright (C) 2017 ANO Tech
 * 网站      : www.anotc.com
 * 淘宝店铺  : anotc.taobao.com
 * QQ群      : 190169595
 * 
 * 本文件包含飞控系统的任务调度功能，包括：
 * - 多频率任务循环调度
 * - 任务优先级管理
 * - 系统时间管理
 * - 任务执行状态监控
 **********************************************************************************/

#include "Ano_Scheduler.h"
#include "User_Task.h"
#include "My_control.h"
#include "Drv_AnoOf.h"
#include "ANO_DT_LX.h"
#include "mid360_receive.h"

//////////////////////////////////////////////////////////////////////
// 用户任务函数定义区域
//////////////////////////////////////////////////////////////////////

/*******************************************************************************
 * 函数名    : Loop_1000Hz
 * 功能描述  : 1000Hz频率任务循环（1ms执行一次）
 * 输入参数  : 无
 * 返回值    : 无
 * 说明      : 最高优先级任务，用于需要快速响应的控制算法
 *******************************************************************************/
static void Loop_1000Hz(void) //1ms执行一次
{
	//////////////////////////////////////////////////////////////////////
	// 1000Hz任务区域 - 放置需要最高执行频率的任务
	//////////////////////////////////////////////////////////////////////
}

/*******************************************************************************
 * 函数名    : Loop_500Hz
 * 功能描述  : 500Hz频率任务循环（2ms执行一次）
 * 输入参数  : 无
 * 返回值    : 无
 * 说明      : 高优先级任务，用于传感器数据处理和快速控制
 *******************************************************************************/
static void Loop_500Hz(void) //2ms执行一次
{
	//////////////////////////////////////////////////////////////////////
	// 500Hz任务区域 - 放置高频率执行的任务
	//////////////////////////////////////////////////////////////////////
}

/*******************************************************************************
 * 函数名    : Loop_200Hz
 * 功能描述  : 200Hz频率任务循环（5ms执行一次）
 * 输入参数  : 无
 * 返回值    : 无
 * 说明      : 中高优先级任务，用于安全开关检测等关键功能
 *******************************************************************************/
static void Loop_200Hz(void) //5ms执行一次
{
	//////////////////////////////////////////////////////////////////////
	// 安全开关检测 - 确保系统安全运行
	safety_switch();
	//////////////////////////////////////////////////////////////////////
}

/*******************************************************************************
 * 函数名    : Loop_100Hz
 * 功能描述  : 100Hz频率任务循环（10ms执行一次）
 * 输入参数  : 无
 * 返回值    : 无
 * 说明      : 中优先级任务，用于状态指示和常规控制
 *******************************************************************************/
static void Loop_100Hz(void) //10ms执行一次
{
	//////////////////////////////////////////////////////////////////////
	// T265 LED状态指示（当前注释掉）
//	t265_led();
	
	//////////////////////////////////////////////////////////////////////
}

/*******************************************************************************
 * 函数名    : Loop_50Hz
 * 功能描述  : 50Hz频率任务循环（20ms执行一次）
 * 输入参数  : 无
 * 返回值    : 无
 * 说明      : 中低优先级任务，用于通信和状态更新
 *******************************************************************************/
static void Loop_50Hz(void) //20ms执行一次
{
	//////////////////////////////////////////////////////////////////////
	//UserTask_OneKeyCmd();
	Automatic_Flay();
	//////////////////////////////////////////////////////////////////////
}
int a=0;
static void Loop_20Hz(void) //50msִ��һ��
{
	//////////////////////////////////////////////////////////////////////

	
	//////////////////////////////////////////////////////////////////////
}

static void Loop_2Hz(void) //500msִ��һ��
{

	GPIO_SetBits(GPIOB,GPIO_Pin_0);
	
//	Hard_thowd();
	ANODT_SendF111(ano_of.intergral_x,ano_of.intergral_y,ano_of.of1_dx,ano_of.of1_dy,NUC_receive_Command.QR_strikes);
}
//////////////////////////////////////////////////////////////////////
//��������ʼ��
//////////////////////////////////////////////////////////////////////
//ϵͳ�������ã�������ִͬ��Ƶ�ʵġ��̡߳�
static sched_task_t sched_tasks[] =
	{
		{Loop_1000Hz, 1000, 0, 0},
		{Loop_500Hz, 500, 0, 0},
		{Loop_200Hz, 200, 0, 0},
		{Loop_100Hz, 100, 0, 0},
		{Loop_50Hz, 50, 0, 0},
		{Loop_20Hz, 20, 0, 0},
		{Loop_2Hz, 2, 0, 0},
};
//�������鳤�ȣ��ж��߳�����
#define TASK_NUM (sizeof(sched_tasks) / sizeof(sched_task_t))

void Scheduler_Setup(void)
{
	uint8_t index = 0;
	//��ʼ�������
	for (index = 0; index < TASK_NUM; index++)
	{
		//����ÿ���������ʱ������
		sched_tasks[index].interval_ticks = TICK_PER_SECOND / sched_tasks[index].rate_hz;
		//�������Ϊ1��Ҳ����1ms
		if (sched_tasks[index].interval_ticks < 1)
		{
			sched_tasks[index].interval_ticks = 1;
		}
	}
}
//��������ŵ�main������while(1)�У���ͣ�ж��Ƿ����߳�Ӧ��ִ��
void Scheduler_Run(void)
{
	uint8_t index = 0;
	//ѭ���ж������̣߳��Ƿ�Ӧ��ִ��

	for (index = 0; index < TASK_NUM; index++)
	{
		//��ȡϵͳ��ǰʱ�䣬��λMS
		uint32_t tnow = GetSysRunTimeMs();
		//�����жϣ������ǰʱ���ȥ��һ��ִ�е�ʱ�䣬���ڵ��ڸ��̵߳�ִ�����ڣ���ִ���߳�
		if (tnow - sched_tasks[index].last_run >= sched_tasks[index].interval_ticks)
		{

			//�����̵߳�ִ��ʱ�䣬������һ���ж�
			sched_tasks[index].last_run = tnow;
			//ִ���̺߳�����ʹ�õ��Ǻ���ָ��
			sched_tasks[index].task_func();
		}
	}
}

void Strike_time()
{
	if(bark_tin != 0)
	{
		bark_tin++;
		if(bark_tin > 3)
		{
			bark_tin = 4;
		}
	}
	
}

/******************* (C) COPYRIGHT 2014 ANO TECH *****END OF FILE************/

14. 飞控功能封装头文件

文件路径：FcSrc/LX_FC_Fun.h

#ifndef __LX_FC_FUN_H
#define __LX_FC_FUN_H

//==引用
#include "SysConfig.h"

//==定义/声明

//==数据声明

//==函数声明
//static

//public
u8 FC_Unlock(void);
u8 FC_Lock(void);
u8 LX_Change_Mode(u8 new_mode);
u8 OneKey_Takeoff(u16 height_cm);
u8 OneKey_Land(void);
u8 OneKey_Flip(void);
u8 OneKey_Return_Home(void);
u8 Horizontal_Calibrate(void);
u8 Horizontal_Move(u16 distance_cm, u16 velocity_cmps, u16 dir_angle_0_360);
u8 Mag_Calibrate(void);
u8 ACC_Calibrate(void);
u8 GYR_Calibrate(void);
u8 hover(void);
u8 Auto_xuant(void);

#endif

15. 飞控功能封装实现

文件路径：FcSrc/LX_FC_Fun.c

/**********************************************************************************
 * 文件名    : LX_FC_Fun.c
 * 功能描述  : 凌霄飞控功能函数库 - 飞控系统核心功能实现
 * 创建时间  : 2020-03-31
 * 作者      : ANO技术团队 - Jyoun
 * 版权声明  : Copyright (C) 2020 ANO Tech
 * 网站      : www.anotc.com
 * 淘宝店铺  : anotc.taobao.com
 * QQ群      : 190169595
 * 项目联系人: 18084888982 / 18061373080
 * 
 * 本文件包含凌霄飞控系统的核心功能函数，包括：
 * - 飞控解锁/锁定控制
 * - 飞行模式切换
 * - 安全保护功能
 * - 系统状态管理
 * 
 * ANO技术团队感谢大家的支持，欢迎加入QQ群交流讨论学习
 * 如果在使用过程中发现任何问题，欢迎提出宝贵意见
 * 如果您觉得我们的产品不错，请推荐给朋友支持我们
 * 开源代码欢迎使用、修改、扩展，但使用时请保留版权信息
 * 我们坚持开源共享，不抹黑同行，希望共同维护良好的开源环境
 * 只有大家的支持，我们才能做得更好
 **********************************************************************************/

#include "LX_FC_Fun.h"
#include "LX_FC_State.h"
#include "ANO_DT_LX.h"

//////////////////////////////////////////////////////////////////////
// 飞控核心功能函数
//////////////////////////////////////////////////////////////////////

/*******************************************************************************
 * 函数名    : FC_Unlock
 * 功能描述  : 飞控解锁函数
 * 输入参数  : 无
 * 返回值    : 1 - 解锁成功，0 - 解锁失败（等待校验中）
 * 说明      : 发送解锁指令，将飞控系统从锁定状态切换到解锁状态
 *******************************************************************************/
u8 FC_Unlock()
{
	// 设置解锁命令标志
	fc_sta.unlock_cmd = 1; // 解锁
	
	// 通过通信协议发送指令
	if (dt.wait_ck == 0) // 没有数据等待校验时，才发送CMD指令
	{
		// 设置解锁指令参数
		dt.cmd_send.CID = 0x10;     // 命令ID：解锁指令
		dt.cmd_send.CMD[0] = 0x00;  // 命令参数1
		dt.cmd_send.CMD[1] = 0x01;  // 命令参数2：解锁
		
		// 发送解锁指令
		CMD_Send(0XFF, &dt.cmd_send);
		return 1; // 解锁成功
	}
	else
	{
		return 0; // 解锁失败，系统正在等待校验
	}
}

/*******************************************************************************
 * 函数名    : FC_Lock
 * 功能描述  : 飞控锁定函数
 * 输入参数  : 无
 * 返回值    : 1 - 锁定成功，0 - 锁定失败（等待校验中）
 * 说明      : 发送锁定指令，将飞控系统从解锁状态切换到锁定状态
 *******************************************************************************/
u8 FC_Lock()
{
	// 设置锁定命令标志
	fc_sta.unlock_cmd = 0; // 锁定
	
	// 通过通信协议发送指令
	if (dt.wait_ck == 0) //û�������ȴ�У���CMDʱ�ŷ��ͱ�CMD
	{
		dt.cmd_send.CID = 0x10;
		dt.cmd_send.CMD[0] = 0x00;
		dt.cmd_send.CMD[1] = 0x02;
		CMD_Send(0XFF, &dt.cmd_send);
		return 1;
	}
	else
	{
		return 0;
	}
}

//�ı�ɿ�ģʽ(ģʽ0-3)
u8 LX_Change_Mode(u8 new_mode)
{
	static u8 old_mode;
	if (old_mode != new_mode)
	{
		//
		if (dt.wait_ck == 0) //û�������ȴ�У���CMDʱ�ŷ��ͱ�CMD
		{
			old_mode = fc_sta.fc_mode_cmd = new_mode;
			//��Э�鷢��ָ��
			dt.cmd_send.CID = 0X01;
			dt.cmd_send.CMD[0] = 0X01;
			dt.cmd_send.CMD[1] = 0X01;
			dt.cmd_send.CMD[2] = fc_sta.fc_mode_cmd;
			CMD_Send(0xff, &dt.cmd_send);
			return 1;
		}
		else
		{
			return 0;
		}
	}
	else //�Ѿ��ڵ�ǰģʽ
	{
		return 1;
	}
}

//һ������
//��Ҫע�⣬�̿�ģʽ�²���ִ�з���
u8 OneKey_Return_Home()
{
	//
	if (dt.wait_ck == 0) //û�������ȴ�У���CMDʱ�ŷ��ͱ�CMD
	{
		//��Э�鷢��ָ��
		dt.cmd_send.CID = 0X10;
		dt.cmd_send.CMD[0] = 0X00;
		dt.cmd_send.CMD[1] = 0X07;
		CMD_Send(0xff, &dt.cmd_send);
		return 1;
	}
	else
	{
		return 0;
	}
}

//һ�����(�߶�cm)
u8 OneKey_Takeoff(u16 height_cm)
{
	//
	if (dt.wait_ck == 0) //û�������ȴ�У���CMDʱ�ŷ��ͱ�CMD
	{
		//��Э�鷢��ָ��
		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;
	}
	else
	{
		return 0;
	}
}
//һ������
u8 OneKey_Land()
{
	//
	if (dt.wait_ck == 0) //û�������ȴ�У���CMDʱ�ŷ��ͱ�CMD
	{
		//��Э�鷢��ָ��
		dt.cmd_send.CID = 0X10;
		dt.cmd_send.CMD[0] = 0X00;
		dt.cmd_send.CMD[1] = 0X06;
		CMD_Send(0xff, &dt.cmd_send);
		return 1;
	}
	else
	{
		return 0;
	}
}

//������ͣ
u8 hover()
{
	//
	if (dt.wait_ck == 0) //û�������ȴ�У���CMDʱ�ŷ��ͱ�CMD
	{
		//��Э�鷢��ָ��
		dt.cmd_send.CID = 0X10;
		dt.cmd_send.CMD[0] = 0X00;
		dt.cmd_send.CMD[1] = 0X04;
		CMD_Send(0xff, &dt.cmd_send);
		return 1;
	}
	else
	{
		return 0;
	}
}

//ƽ��(����cm���ٶ�cmps������Ƕ�0-360��)
u8 Horizontal_Move(u16 distance_cm, u16 velocity_cmps, u16 dir_angle_0_360)
{
	//
	if (dt.wait_ck == 0) //û�������ȴ�У���CMDʱ�ŷ��ͱ�CMD
	{
		//��Э�鷢��ָ��
		dt.cmd_send.CID = 0X10;
		dt.cmd_send.CMD[0] = 0X02;
		dt.cmd_send.CMD[1] = 0X03;
		//
		dt.cmd_send.CMD[2] = BYTE0(distance_cm);
		dt.cmd_send.CMD[3] = BYTE1(distance_cm);
		dt.cmd_send.CMD[4] = BYTE0(velocity_cmps);
		dt.cmd_send.CMD[5] = BYTE1(velocity_cmps);
		dt.cmd_send.CMD[6] = BYTE0(dir_angle_0_360);
		dt.cmd_send.CMD[7] = BYTE1(dir_angle_0_360);
		//
		CMD_Send(0xff, &dt.cmd_send);
		return 1;
	}
	else
	{
		return 0;
	}
}

//ˮƽУ׼
u8 Horizontal_Calibrate()
{
	//
	if (dt.wait_ck == 0) //û�������ȴ�У���CMDʱ�ŷ��ͱ�CMD
	{
		//��Э�鷢��ָ��
		dt.cmd_send.CID = 0X01;
		dt.cmd_send.CMD[0] = 0X00;
		dt.cmd_send.CMD[1] = 0X03;
		CMD_Send(0xff, &dt.cmd_send);
		return 1;
	}
	else
	{
		return 0;
	}
}

//������У׼
u8 Mag_Calibrate()
{
	//
	if (dt.wait_ck == 0) //û�������ȴ�У���CMDʱ�ŷ��ͱ�CMD
	{
		//��Э�鷢��ָ��
		dt.cmd_send.CID = 0X01;
		dt.cmd_send.CMD[0] = 0X00;
		dt.cmd_send.CMD[1] = 0X04;
		CMD_Send(0xff, &dt.cmd_send);
		return 1;
	}
	else
	{
		return 0;
	}
}

//6����ٶ�У׼
u8 ACC_Calibrate()
{
	//
	if (dt.wait_ck == 0) //û�������ȴ�У���CMDʱ�ŷ��ͱ�CMD
	{
		//��Э�鷢��ָ��
		dt.cmd_send.CID = 0X01;
		dt.cmd_send.CMD[0] = 0X00;
		dt.cmd_send.CMD[1] = 0X05;
		CMD_Send(0xff, &dt.cmd_send);
		return 1;
	}
	else
	{
		return 0;
	}
}

//������У׼
u8 GYR_Calibrate()
{
	//
	if (dt.wait_ck == 0) //û�������ȴ�У���CMDʱ�ŷ��ͱ�CMD
	{
		//��Э�鷢��ָ��
		dt.cmd_send.CID = 0X01;
		dt.cmd_send.CMD[0] = 0X00;
		dt.cmd_send.CMD[1] = 0X02;
		CMD_Send(0xff, &dt.cmd_send);
		return 1;
	}
	else
	{
		return 0;
	}
}
u8 Auto_xuant()
{
	//
	if (dt.wait_ck == 0) //û�������ȴ�У���CMDʱ�ŷ��ͱ�CMD
	{
		//��Э�鷢��ָ��
		dt.cmd_send.CID = 0X10;
		dt.cmd_send.CMD[0] = 0X00;
		dt.cmd_send.CMD[1] = 0X04;
		CMD_Send(0xff, &dt.cmd_send);
		return 1;
	}
	else
	{
		return 0;
	}
}

16. 光流/定位桥接头文件

文件路径：DriversBsp/Drv_AnoOf.h

#ifndef __DRV_ANO_OF_H
#define __DRV_ANO_OF_H

//==引用
#include "SysConfig.h"

//==定义/声明

typedef struct
{
	//
	u8 of_update_cnt;  //光流数据更新计数。
	u8 alt_update_cnt; //高度数据更新计数。
	//
	u8 link_sta; //连接状态：0，未连接。1，已连接。
	u8 work_sta; //工作状态：0，异常。1，正常
	//
	u8 of_quality;
	//
	u8 of0_sta;
	s8 of0_dx;
	s8 of0_dy;
	//
	u8 of1_sta;
	s16 of1_dx;
	s16 of1_dy;
	//
	u8 of2_sta;
	s16 of2_dx;
	s16 of2_dy;
	s16 of2_dx_fix;
	s16 of2_dy_fix;
	s16 intergral_x;
	s16 intergral_y;
	//
	u32 of_alt_cm;
	//
	float quaternion[4];
	//
	s16 acc_data_x;
	s16 acc_data_y;
	s16 acc_data_z;
	s16 gyr_data_x;
	s16 gyr_data_y;
	s16 gyr_data_z;

} _ano_of_st;

//飞控状态

//==数据声明
extern _ano_of_st ano_of;
//==函数声明
//static
static void AnoOF_DataAnl(uint8_t *data_buf, uint8_t num);

//public
void AnoOF_GetOneByte(uint8_t data);
void AnoOF_Check_State(float dT_s);
#endif

17. 光流/定位桥接实现

文件路径：DriversBsp/Drv_AnoOf.c

#include "Drv_AnoOf.h"
#include "mid360_receive.h"
_ano_of_st ano_of;
static uint8_t _datatemp[50];
static float check_time_ms[3];
void AnoOF_Check_State(float dT_s)
{
	u8 tmp[2];
	//���Ӽ��
	if (check_time_ms[0] < 500)
	{
		check_time_ms[0]++;
		ano_of.link_sta = 1;
	}
	else
	{
		ano_of.link_sta = 0;
	}
	//���ݼ��1
	if (check_time_ms[1] < 500)
	{
		check_time_ms[1]++;
		tmp[0] = 1;
	}
	else
	{
		tmp[0] = 0;
	}
	//���ݼ��2
	if (check_time_ms[2] < 500)
	{
		check_time_ms[2]++;
		tmp[1] = 1;
	}
	else
	{
		tmp[1] = 0;
	}
	//���ù���״̬
	if (tmp[0] && tmp[1])
	{
		ano_of.work_sta = 1;
	}
	else
	{
		ano_of.work_sta = 0;
	}
}

//AnoOF_GetOneByte�dz������ݽ�������������ÿ���յ�һ�ֽڹ������ݣ����ñ�����һ�Σ������������Ǵ����յ�������
//����������α����ã����ս��յ�������һ֡���ݺ󣬻��Զ��������ݽ�������AnoOF_DataAnl
void AnoOF_GetOneByte(uint8_t data)
{
	static u8 _data_len = 0, _data_cnt = 0;
	static u8 rxstate = 0;

	if (rxstate == 0 && data == 0xAA)
	{
		rxstate = 1;
		_datatemp[0] = data;
	}
	else if (rxstate == 1 && (data == HW_TYPE || data == HW_ALL))
	{
		rxstate = 2;
		_datatemp[1] = data;
	}
	else if (rxstate == 2)
	{
		rxstate = 3;
		_datatemp[2] = data;
	}
	else if (rxstate == 3 && data < 250)
	{
		rxstate = 4;
		_datatemp[3] = data;
		_data_len = data;
		_data_cnt = 0;
	}
	else if (rxstate == 4 && _data_len > 0)
	{
		_data_len--;
		_datatemp[4 + _data_cnt++] = data;
		if (_data_len == 0)
			rxstate = 5;
	}
	else if (rxstate == 5)
	{
		rxstate = 6;
		_datatemp[4 + _data_cnt++] = data;
	}
	else if (rxstate == 6)
	{
		rxstate = 0;
		_datatemp[4 + _data_cnt] = data;
		//		DT_data_cnt = _data_cnt+5;
		//
		AnoOF_DataAnl(_datatemp, _data_cnt + 5); //
	}
	else
	{
		rxstate = 0;
	}
}
//AnoOF_DataAnlΪ�������ݽ�������������ͨ���������õ�����ģ������ĸ�������
//�������ݵ����壬�������������ģ��ʹ���ֲᣬ����ϸ�Ľ���

static void AnoOF_DataAnl(uint8_t *data, uint8_t len)
{
	u8 check_sum1 = 0, check_sum2 = 0;
	uint16_t chet_temp;
	if (*(data + 3) != (len - 6)) //�ж����ݳ����Ƿ���ȷ
		return;
	for (u8 i = 0; i < len - 2; i++)
	{
		check_sum1 += *(data + i);
		check_sum2 += check_sum1;
	}
	if ((check_sum1 != *(data + len - 2)) || (check_sum2 != *(data + len - 1))) //�ж�sumУ��
		return;
	//================================================================================

	if (*(data + 2) == 0X51) //������Ϣ
	{
		chet_temp = chek_mid360_ok();
		if (*(data + 4) == 0) //ԭʼ������Ϣ
		{
			ano_of.of0_sta = *(data + 5);
			ano_of.of0_dx = *(data + 6);
			ano_of.of0_dy = *(data + 7);
			ano_of.of_quality = *(data + 8);
			check_time_ms[1] = 0;
		}
		else if (*(data + 4) == 1  	) //�߶��ںϺ������Ϣ
		{
//&&  chet_temp > 600								
			ano_of.of1_sta = *(data + 5);
			ano_of.of1_dx = *((s16 *)(data + 6));
			ano_of.of1_dy = *((s16 *)(data + 8));
			ano_of.of_quality = *(data + 10);
			//
			check_time_ms[1] = 0;
			ano_of.of_update_cnt++;
		}
		else if (*(data + 4) == 2 &&  chet_temp > 850) //�ߵ��ںϺ������Ϣ
		{
			GPIO_SetBits(GPIOC,GPIO_Pin_6);//��
			ano_of.of2_sta = *(data + 5);
			ano_of.of2_dx = *((s16 *)(data + 6));
			ano_of.of2_dy = *((s16 *)(data + 8));
			ano_of.of2_dx_fix = *((s16 *)(data + 10));
			ano_of.of2_dy_fix = *((s16 *)(data + 12));
			ano_of.intergral_x = *((s16 *)(data + 14));
			ano_of.intergral_y = *((s16 *)(data + 16));
			ano_of.of_quality = *(data + 18);
			//
		}
	}
	else if (*(data + 2) == 0X34) //�߶���Ϣ
	{
		ano_of.of_alt_cm = *((u32 *)(data + 7));
		//
		check_time_ms[2] = 0;
		ano_of.alt_update_cnt++;
	}
	else if (*(data + 2) == 0X01) //��������
	{
		ano_of.acc_data_x = *((s16 *)(data + 4));
		ano_of.acc_data_y = *((s16 *)(data + 6));
		ano_of.acc_data_z = *((s16 *)(data + 8));
		ano_of.gyr_data_x = *((s16 *)(data + 10));
		ano_of.gyr_data_y = *((s16 *)(data + 12));
		ano_of.gyr_data_z = *((s16 *)(data + 14));
		//shock_sta+16
	}
	else if (*(data + 2) == 0X04) //��̬��Ϣ
	{
		//��Ԫ����ʽ
		ano_of.quaternion[0] = (*((s16 *)(data + 4))) * 0.0001f;
		ano_of.quaternion[1] = (*((s16 *)(data + 6))) * 0.0001f;
		ano_of.quaternion[2] = (*((s16 *)(data + 8))) * 0.0001f;
		ano_of.quaternion[3] = (*((s16 *)(data + 10))) * 0.0001f;
	}
}

18. 串口映射与中断接收

文件路径：DriversMcu/STM32F407/Drivers/Drv_Uart.c

/******************** (C) COPYRIGHT 2017 ANO Tech ********************************
 * ����    �������ƴ�
 * ����    ��www.anotc.com
 * �Ա�    ��anotc.taobao.com
 * ����QȺ ��190169595
 * ����    ����������
**********************************************************************************/
#include "Drv_Uart.h"
#include "Ano_DT_LX.h"
#include "Drv_UbloxGPS.h"
#include "Drv_AnoOf.h"

#include "mid360_receive.h"
#include "User_DT.h"
void NoUse(u8 data){}
//���ڽ��շ��Ϳ��ٶ��壬ֱ���޸Ĵ˴��ĺ������ƺ꣬�޸ij��Լ��Ĵ��ڽ����ͷ��ͺ������Ƽ��ɣ�ע�⺯��������ʽ��ͳһ
#define U1GetOneByte	//���ݷ���
#define U2GetOneByte	Jetson_GetOneByte
#define U3GetOneByte	mid360_GetOneByte
#define U4GetOneByte	AnoOF_GetOneByte
#define U5GetOneByte	ANO_DT_LX_Data_Receive_Prepare	
	
//====uart1
void DrvUart1Init(u32 br_num)
{
    USART_InitTypeDef USART_InitStructure;
    USART_ClockInitTypeDef USART_ClockInitStruct;
    NVIC_InitTypeDef NVIC_InitStructure;
    GPIO_InitTypeDef GPIO_InitStructure;

    USART_StructInit(&USART_InitStructure);

    RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); //����USART1ʱ��
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);

    //�����ж����ȼ�
    NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_UART1_P;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_UART1_S;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);

    GPIO_PinAFConfig(GPIOA, GPIO_PinSource9, GPIO_AF_USART1);
    GPIO_PinAFConfig(GPIOA, GPIO_PinSource10, GPIO_AF_USART1);

    //����PA9��ΪUSART1��Tx
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
    GPIO_Init(GPIOA, &GPIO_InitStructure);
    //����PA10��ΪUSART1��Rx
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
    GPIO_Init(GPIOA, &GPIO_InitStructure);
    //
    USART_DeInit(USART1);
    //����USART1
    //�жϱ�������
    USART_InitStructure.USART_BaudRate = br_num;                                    //�����ʿ���ͨ������վ����
    USART_InitStructure.USART_WordLength = USART_WordLength_8b;                     //8���
    USART_InitStructure.USART_StopBits = USART_StopBits_1;                          //��֡��β����1��ֹͣλ
    USART_InitStructure.USART_Parity = USART_Parity_No;                             //������żУ��
    USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //Ӳ��������ʧ��
    USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx;                 //���͡�����ʹ��
    //����USART1ʱ��
    USART_ClockInitStruct.USART_Clock = USART_Clock_Disable;     //ʱ�ӵ͵�ƽ�
    USART_ClockInitStruct.USART_CPOL = USART_CPOL_Low;           //SLCK������ʱ������ļ���->�͵�ƽ
    USART_ClockInitStruct.USART_CPHA = USART_CPHA_2Edge;         //ʱ�ӵڶ������ؽ������ݲ���
    USART_ClockInitStruct.USART_LastBit = USART_LastBit_Disable; //���һλ���ݵ�ʱ�����岻��SCLK���

    USART_Init(USART1, &USART_InitStructure);
    USART_ClockInit(USART1, &USART_ClockInitStruct);

    //ʹ��USART1�����ж�
    USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
    //ʹ��USART1
    USART_Cmd(USART1, ENABLE);
}

u8 Tx1Buffer[256];
u8 Tx1Counter = 0;
u8 count1 = 0;
void DrvUart1SendBuf(unsigned char *DataToSend, u8 data_num)
{
    u8 i;
    for (i = 0; i < data_num; i++)
    {
        Tx1Buffer[count1++] = *(DataToSend + i);
    }

    if (!(USART1->CR1 & USART_CR1_TXEIE))
    {
        USART_ITConfig(USART1, USART_IT_TXE, ENABLE); //�򿪷����ж�
    }
}
u8 U1RxDataTmp[100];
u8 U1RxInCnt = 0;
u8 U1RxoutCnt = 0;
void drvU1GetByte(u8 data)
{
	U1RxDataTmp[U1RxInCnt++] = data;
	if(U1RxInCnt >= 100)
		U1RxInCnt = 0;
}
void drvU1DataCheck(void)
{
	while(U1RxInCnt!=U1RxoutCnt)
	{
		U1GetOneByte(U1RxDataTmp[U1RxoutCnt++]);
		if(U1RxoutCnt >= 100)
			U1RxoutCnt = 0;
	}
}
void Usart1_IRQ(void)
{
    u8 com_data;

    if (USART1->SR & USART_SR_ORE) //ORE�ж�
    {
        com_data = USART1->DR;
    }
    //�����ж�
    if (USART_GetITStatus(USART1, USART_IT_RXNE))
    {
        USART_ClearITPendingBit(USART1, USART_IT_RXNE); //����жϱ�־
        com_data = USART1->DR;
        drvU1GetByte(com_data);
    }
    //���ͣ�������λ���ж�
    if (USART_GetITStatus(USART1, USART_IT_TXE))
    {
        USART1->DR = Tx1Buffer[Tx1Counter++]; //дDR����жϱ�־
        if (Tx1Counter == count1)
        {
            USART1->CR1 &= ~USART_CR1_TXEIE; //�ر�TXE�������жϣ��ж�
        }
    }
}



//====uart2
void DrvUart2Init(u32 br_num)
{
    USART_InitTypeDef USART_InitStructure;
    USART_ClockInitTypeDef USART_ClockInitStruct;
    NVIC_InitTypeDef NVIC_InitStructure;
    GPIO_InitTypeDef GPIO_InitStructure;

    USART_StructInit(&USART_InitStructure);

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); //����USART2ʱ��
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);

    //�����ж����ȼ�
    NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_UART2_P;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_UART2_S;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);

    GPIO_PinAFConfig(GPIOD, GPIO_PinSource5, GPIO_AF_USART2);
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource6, GPIO_AF_USART2);

    //����PD5��ΪUSART2��Tx
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
    GPIO_Init(GPIOD, &GPIO_InitStructure);
    //����PD6��ΪUSART2��Rx
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
    GPIO_Init(GPIOD, &GPIO_InitStructure);

    //����USART2
    //�жϱ�������
    USART_InitStructure.USART_BaudRate = br_num;                                    //�����ʿ���ͨ������վ����
    USART_InitStructure.USART_WordLength = USART_WordLength_8b;                     //8���
    USART_InitStructure.USART_StopBits = USART_StopBits_1;                          //��֡��β����1��ֹͣλ
    USART_InitStructure.USART_Parity = USART_Parity_No;                             //������żУ��
    USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //Ӳ��������ʧ��
    USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx;                 //���͡�����ʹ��
    //����USART2ʱ��
    USART_ClockInitStruct.USART_Clock = USART_Clock_Disable;     //ʱ�ӵ͵�ƽ�
    USART_ClockInitStruct.USART_CPOL = USART_CPOL_Low;           //SLCK������ʱ������ļ���->�͵�ƽ
    USART_ClockInitStruct.USART_CPHA = USART_CPHA_2Edge;         //ʱ�ӵڶ������ؽ������ݲ���
    USART_ClockInitStruct.USART_LastBit = USART_LastBit_Disable; //���һλ���ݵ�ʱ�����岻��SCLK���

    USART_Init(USART2, &USART_InitStructure);
    USART_ClockInit(USART2, &USART_ClockInitStruct);

    //ʹ��USART2�����ж�
    USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);
    //ʹ��USART2
    USART_Cmd(USART2, ENABLE);
}

u8 TxBuffer[256];
u8 TxCounter = 0;
u8 count = 0;
void DrvUart2SendBuf(unsigned char *DataToSend, u8 data_num)
{
    u8 i;
    for (i = 0; i < data_num; i++)
    {
        TxBuffer[count++] = *(DataToSend + i);
    }

    if (!(USART2->CR1 & USART_CR1_TXEIE))
    {
        USART_ITConfig(USART2, USART_IT_TXE, ENABLE); //�򿪷����ж�
    }
}
u8 U2RxDataTmp[100];
u8 U2RxInCnt = 0;
u8 U2RxoutCnt = 0;
void drvU2GetByte(u8 data)
{
	U2RxDataTmp[U2RxInCnt++] = data;
	if(U2RxInCnt >= 100)
		U2RxInCnt = 0;
}
void drvU2DataCheck(void)
{
	while(U2RxInCnt!=U2RxoutCnt)
	{
		U2GetOneByte(U2RxDataTmp[U2RxoutCnt++]);
		if(U2RxoutCnt >= 100)
			U2RxoutCnt = 0;
	}
}
void Usart2_IRQ(void)
{
    u8 com_data;

    if (USART2->SR & USART_SR_ORE) //ORE�ж�
    {
        com_data = USART2->DR;
    }

    //�����ж�
    if (USART_GetITStatus(USART2, USART_IT_RXNE))
    {
        USART_ClearITPendingBit(USART2, USART_IT_RXNE); //����жϱ�־
        com_data = USART2->DR;
		drvU2GetByte(com_data);
    }
    //���ͣ�������λ���ж�
    if (USART_GetITStatus(USART2, USART_IT_TXE))
    {
        USART2->DR = TxBuffer[TxCounter++]; //дDR����жϱ�־
        if (TxCounter == count)
        {
            USART2->CR1 &= ~USART_CR1_TXEIE; //�ر�TXE�������жϣ��ж�
        }
    }
}

//====uart3
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
void DrvUart3Init(u32 br_num)
{
    USART_InitTypeDef USART_InitStructure;
    USART_ClockInitTypeDef USART_ClockInitStruct;
    NVIC_InitTypeDef NVIC_InitStructure;
    GPIO_InitTypeDef GPIO_InitStructure;

    USART_StructInit(&USART_InitStructure);

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); //����USART2ʱ��
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);

    //�����ж����ȼ�
    NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);

    GPIO_PinAFConfig(GPIOB, GPIO_PinSource10, GPIO_AF_USART3);
    GPIO_PinAFConfig(GPIOB, GPIO_PinSource11, GPIO_AF_USART3);

    //����PD5��ΪUSART2��Tx
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
    GPIO_Init(GPIOB, &GPIO_InitStructure);
    //����PD6��ΪUSART2��Rx
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
    GPIO_Init(GPIOB, &GPIO_InitStructure);

    //����USART2
    //�жϱ�������
    USART_InitStructure.USART_BaudRate = br_num;                                    //�����ʿ���ͨ������վ����
    USART_InitStructure.USART_WordLength = USART_WordLength_8b;                     //8���
    USART_InitStructure.USART_StopBits = USART_StopBits_1;                          //��֡��β����1��ֹͣλ
    USART_InitStructure.USART_Parity = USART_Parity_No;                             //������żУ��
    USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //Ӳ��������ʧ��
    USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx;                 //���͡�����ʹ��
    //����USART2ʱ��
    USART_ClockInitStruct.USART_Clock = USART_Clock_Disable;     //ʱ�ӵ͵�ƽ�
    USART_ClockInitStruct.USART_CPOL = USART_CPOL_Low;           //SLCK������ʱ������ļ���->�͵�ƽ
    USART_ClockInitStruct.USART_CPHA = USART_CPHA_2Edge;         //ʱ�ӵڶ������ؽ������ݲ���
    USART_ClockInitStruct.USART_LastBit = USART_LastBit_Disable; //���һλ���ݵ�ʱ�����岻��SCLK���

    USART_Init(USART3, &USART_InitStructure);
    USART_ClockInit(USART3, &USART_ClockInitStruct);

    //ʹ��USART2�����ж�
    USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);
    //ʹ��USART2
    USART_Cmd(USART3, ENABLE);
}

u8 Tx3Buffer[256];
u8 Tx3Counter = 0;
u8 count3 = 0;
void DrvUart3SendBuf(unsigned char *DataToSend, u8 data_num)
{
    u8 i;
    for (i = 0; i < data_num; i++)
    {
        Tx3Buffer[count3++] = *(DataToSend + i);
    }
    if (!(USART3->CR1 & USART_CR1_TXEIE))
    {
        USART_ITConfig(USART3, USART_IT_TXE, ENABLE); //�򿪷����ж�
    }
}
u8 U3RxDataTmp[100];
u8 U3RxInCnt = 0;
u8 U3RxoutCnt = 0;
void drvU3GetByte(u8 data)
{
	U3RxDataTmp[U3RxInCnt++] = data;
	if(U3RxInCnt >= 100)
		U3RxInCnt = 0;
}
void drvU3DataCheck(void)
{
	while(U3RxInCnt!=U3RxoutCnt)
	{
		U3GetOneByte(U3RxDataTmp[U3RxoutCnt++]);
		if(U3RxoutCnt >= 100)
			U3RxoutCnt = 0;
	}
}
void Usart3_IRQ(void)
{
    u8 com_data;

    if (USART3->SR & USART_SR_ORE) //ORE�ж�
        com_data = USART3->DR;

    //�����ж�
    if (USART_GetITStatus(USART3, USART_IT_RXNE))
    {
        USART_ClearITPendingBit(USART3, USART_IT_RXNE); //����жϱ�־
        com_data = USART3->DR;
		drvU3GetByte(com_data);
    }
    //���ͣ�������λ���ж�
    if (USART_GetITStatus(USART3, USART_IT_TXE))
    {
        USART3->DR = Tx3Buffer[Tx3Counter++]; //дDR����жϱ�־
        if (Tx3Counter == count3)
        {
            USART3->CR1 &= ~USART_CR1_TXEIE; //�ر�TXE�������жϣ��ж�
        }
    }
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
//====uart4
void DrvUart4Init(u32 br_num)
{
    USART_InitTypeDef USART_InitStructure;
    //USART_ClockInitTypeDef USART_ClockInitStruct;
    NVIC_InitTypeDef NVIC_InitStructure;
    GPIO_InitTypeDef GPIO_InitStructure;

    USART_StructInit(&USART_InitStructure);

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART4, ENABLE); //����USART2ʱ��
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);

    //�����ж����ȼ�
    NVIC_InitStructure.NVIC_IRQChannel = UART4_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_UART4_P;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_UART4_S;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);

    GPIO_PinAFConfig(GPIOA, GPIO_PinSource0, GPIO_AF_UART4);
    GPIO_PinAFConfig(GPIOA, GPIO_PinSource1, GPIO_AF_UART4);

    //����PC12��ΪUART5��Tx
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
    GPIO_Init(GPIOA, &GPIO_InitStructure);
    //����PD2��ΪUART5��Rx
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
    GPIO_Init(GPIOA, &GPIO_InitStructure);

    //����UART5
    //�жϱ�������
    USART_InitStructure.USART_BaudRate = br_num;                                    //�����ʿ���ͨ������վ����
    USART_InitStructure.USART_WordLength = USART_WordLength_8b;                     //8���
    USART_InitStructure.USART_StopBits = USART_StopBits_1;                          //��֡��β����1��ֹͣλ
    USART_InitStructure.USART_Parity = USART_Parity_No;                             //������żУ��
    USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //Ӳ��������ʧ��
    USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx;                 //���͡�����ʹ��
    USART_Init(UART4, &USART_InitStructure);

    //ʹ��UART5�����ж�
    USART_ITConfig(UART4, USART_IT_RXNE, ENABLE);
    //ʹ��USART5
    USART_Cmd(UART4, ENABLE);
}
u8 Tx4Buffer[256];
u8 Tx4Counter = 0;
u8 count4 = 0;
void DrvUart4SendBuf(unsigned char *DataToSend, u8 data_num)
{
    u8 i;
    for (i = 0; i < data_num; i++)
    {
        Tx4Buffer[count4++] = *(DataToSend + i);
    }

    if (!(UART4->CR1 & USART_CR1_TXEIE))
    {
        USART_ITConfig(UART4, USART_IT_TXE, ENABLE); //�򿪷����ж�
    }
}
u8 U4RxDataTmp[100];
u8 U4RxInCnt = 0;
u8 U4RxoutCnt = 0;
void drvU4GetByte(u8 data)
{
	U4RxDataTmp[U4RxInCnt++] = data;
	if(U4RxInCnt >= 100)
		U4RxInCnt = 0;
}
void drvU4DataCheck(void)
{
	while(U4RxInCnt!=U4RxoutCnt)
	{
		U4GetOneByte(U4RxDataTmp[U4RxoutCnt++]);
		if(U4RxoutCnt >= 100)
			U4RxoutCnt = 0;
	}
}
void Uart4_IRQ(void)
{
    u8 com_data;

    if (UART4->SR & USART_SR_ORE) //ORE�ж�
    {
        com_data = UART4->DR;
    }
    //�����ж�
    if (USART_GetITStatus(UART4, USART_IT_RXNE))
    {
        USART_ClearITPendingBit(UART4, USART_IT_RXNE); //����жϱ�־
        com_data = UART4->DR;
        drvU4GetByte(com_data);
    }

    //���ͣ�������λ���ж�
    if (USART_GetITStatus(UART4, USART_IT_TXE))
    {
        UART4->DR = Tx4Buffer[Tx4Counter++]; //дDR����жϱ�־
        if (Tx4Counter == count4)
        {
            UART4->CR1 &= ~USART_CR1_TXEIE; //�ر�TXE�������жϣ��ж�
        }
    }
}

//====uart5
void DrvUart5Init(u32 br_num)
{
    USART_InitTypeDef USART_InitStructure;
    //USART_ClockInitTypeDef USART_ClockInitStruct;
    NVIC_InitTypeDef NVIC_InitStructure;
    GPIO_InitTypeDef GPIO_InitStructure;

    USART_StructInit(&USART_InitStructure);

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART5, ENABLE); //����USART2ʱ��
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);

    //�����ж����ȼ�
    NVIC_InitStructure.NVIC_IRQChannel = UART5_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_UART5_P;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_UART5_S;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);

    GPIO_PinAFConfig(GPIOC, GPIO_PinSource12, GPIO_AF_UART5);
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource2, GPIO_AF_UART5);

    //����PC12��ΪUART5��Tx
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
    GPIO_Init(GPIOC, &GPIO_InitStructure);
    //����PD2��ΪUART5��Rx
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
    GPIO_Init(GPIOD, &GPIO_InitStructure);

    //����UART5
    //�жϱ�������
    USART_InitStructure.USART_BaudRate = br_num;                                    //�����ʿ���ͨ������վ����
    USART_InitStructure.USART_WordLength = USART_WordLength_8b;                     //8���
    USART_InitStructure.USART_StopBits = USART_StopBits_1;                          //��֡��β����1��ֹͣλ
    USART_InitStructure.USART_Parity = USART_Parity_No;                             //������żУ��
    USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //Ӳ��������ʧ��
    USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx;                 //���͡�����ʹ��
    USART_Init(UART5, &USART_InitStructure);

    //ʹ��UART5�����ж�
    USART_ITConfig(UART5, USART_IT_RXNE, ENABLE);
    //ʹ��USART5
    USART_Cmd(UART5, ENABLE);
}
u8 Tx5Buffer[256];
u8 Tx5Counter = 0;
u8 count5 = 0;
void DrvUart5SendBuf(unsigned char *DataToSend, u8 data_num)
{
    u8 i;
    for (i = 0; i < data_num; i++)
    {
        Tx5Buffer[count5++] = *(DataToSend + i);
    }

    if (!(UART5->CR1 & USART_CR1_TXEIE))
    {
        USART_ITConfig(UART5, USART_IT_TXE, ENABLE); //�򿪷����ж�
    }
}
u8 U5RxDataTmp[100];
u8 U5RxInCnt = 0;
u8 U5RxoutCnt = 0;
void drvU5GetByte(u8 data)
{
	U5RxDataTmp[U5RxInCnt++] = data;
	if(U5RxInCnt >= 100)
		U5RxInCnt = 0;
}
void drvU5DataCheck(void)
{
	while(U5RxInCnt!=U5RxoutCnt)
	{
		U5GetOneByte(U5RxDataTmp[U5RxoutCnt++]);
		if(U5RxoutCnt >= 100)
			U5RxoutCnt = 0;
	}
}
void Uart5_IRQ(void)
{
    u8 com_data;
	
    if (UART5->SR & USART_SR_ORE) //ORE�ж�
    {
        com_data = UART5->DR;
    }
    //�����ж�
    if (USART_GetITStatus(UART5, USART_IT_RXNE))
    {
        USART_ClearITPendingBit(UART5, USART_IT_RXNE); //����жϱ�־   
        com_data = UART5->DR;
        drvU5GetByte(com_data);
    }

    //���ͣ�������λ���ж�
    if (USART_GetITStatus(UART5, USART_IT_TXE))
    {
        UART5->DR = Tx5Buffer[Tx5Counter++]; //дDR����жϱ�־
        if (Tx5Counter == count5)
        {
            UART5->CR1 &= ~USART_CR1_TXEIE; //�ر�TXE�������жϣ��ж�
        }
    }
}



void DrvUartDataCheck(void)
{
	drvU1DataCheck();
	drvU2DataCheck();
	drvU3DataCheck();
	drvU4DataCheck();
	drvU5DataCheck();
}
/******************* (C) COPYRIGHT 2014 ANO TECH *****END OF FILE************/

总结

这套凌霄运动源码，真正值得学习的地方不是“怎么把一个起飞函数调通”，而是它已经把任务飞行拆成了比较清晰的几层：调度层、任务状态机层、控制层、感知接入层、协议下发层。

如果你准备做自己的二次开发，最实用的切入顺序就是：

1. 先读 main.c -> Ano_Scheduler.c -> My_control.c，搞清楚主链路。
2. 再读 coordinate_control.c，搞清楚速度是怎么给出来的。
3. 再读 mid360_receive.c / Drv_AnoOf.c / Drv_Uart.c，搞清楚外部设备是怎么接进来的。
4. 最后再按自己的任务需要改 My_control.h / My_control.c。

照这个顺序读，工程就不会乱；照这个顺序改，二次开发也最稳。

200字摘要

本文围绕凌霄运动源码的 STM32F407 版本，系统梳理了这套自主飞行源码的真实运行链路。文章从项目结构入手，明确 Headware 才是当前任务逻辑核心，随后分析了 main.c、Ano_Scheduler.c、My_control.c、coordinate_control.c、mid360_receive.c、Drv_AnoOf.c、Drv_Uart.c 等关键文件，说明任务调度、坐标控制、外设接入和飞控协议下发是如何协同工作的。同时给出了二次开发时最应该修改的入口，包括航点坐标、任务状态机、控制参数、串口协议和外部设备接入方法，并结合 Jetson、激光雷达、摄像头等场景说明了扩展思路。最后附上重要源码全文，便于单篇文章直接发布和持续学习。