竞赛无人机搭积木式编程
——以2021年电赛G题植保无人机国奖标准完整复现为例学习
首先我们需要了解下自动飞行任务执行过程几组关键变量的用法与实际作用效果：
flight_subtask_cnt用于控制飞行任务中具体执行哪一个子线程，比如我们需要对偏航角度进行控制顺时针旋转90度，我们可以将转动过程拆解成两个子线程，第一个线程为设置目标偏航角度，第二个任务为执行偏航控制并实时检测偏航控制完成与否，每一个子线程完成后都会对flight_subtask_cnt计数器自加，下次运行任务时会自动进入下一线程中继续执行未完成的子线程。
新手对任务进行拆分时，实现某一控制任务可能存在着多种实现方法，比如在本案例中为了实现偏航逆时针旋转90度，我们完成可以不用让yaw_ctrl_mode工作在CLOCKWISE模式，直接运用绝对偏航角模式AZIMUTH ，在第一步给定期望时，用当前偏航角度WP_AHRS.Yaw减去90作为偏航期望同样可以实现上述控制效果，具体过程大家可以根据自己掌握的熟练程度来针对性的设计。
flight_global_cnt用于子线程中判断是否满足某些条件的计数器，比如当判断条件为位置偏差时，通过flight_global_cnt的值可以用于判断航点位置是否抵达。
execute_time_ms用于子线程设置子线程的执行时间，比如在上一子线程执行完毕后，对下一子线程执行时间进行设置，在接下来执行下一子线程的过程中，会对execute_time_ms进行自减，execute_time_ms减到0时表示执行时间完毕，用户可以通过判断execute_time_ms是否为0来决策进入下一子任务。
根据上面的讲解，我们知道flight_global_cnt与execute_time_ms都可以用于决策子线程是否执行完毕，通常简单一点的子线程只需要用二者之一就可以，特殊一点的情况比如在某一子任务内需要结合视觉处理，但是由于自己写的视觉代码识别失败、现场某些不可控因素或者自己设计的搜索路径太过冗杂，导致在有限的时间内，程序不能及时完成本阶段子线程，但是竞赛比赛赛题时间上是有要求的，所以为了保险起见，我们可以在判断位置、视觉标记是否完成的基础上，加上合理的最大执行时间限定，当子线程处理超时会强制进入下一线程或者直接返航降落。
IO控制函数
void Laser_Light_Work(_laser_light *light)
IO控制初始化过程和赋值用法如下：
了解了上述自主飞行任务设计关键要点后，我们以2021年全国大学生电子设计竞赛中G题植保飞行器题目要求为例，编写自动飞行任务函数完成比赛内容。
- 第一阶段——自动起飞到航巡高度
uint8_t Auto_Takeoff(float target)//自动起飞到某一高度
函数输入参数target为目标高度，自动起飞任务分为两个线程，第一步为记录当前3维位置信息，作为导航初始原点位置。并且通过导航控制函数设置期望目标位置。第二步为实时检测高度偏差值，连续2S满足位置偏差在10cm以内后，函数返回值置1后，自动起飞到目标高度任务完成，用法参照Developer_Mode.c开发者模式中case 11用法，自主起飞任务完成后会进入case 12执行航点遍历作业任务。
- 第二阶段——航点遍历作业任务
void Agriculture_UAV_Closeloop(void)
第一步生成航点轨迹数组，记录当前高度height，并发布第一个目标航点位置0,0,height)，接着会在第二线程中判断水平位置误差，来确定完成水平航点任务与否。检测悬停目标完成后会发布21号区域位置航点。
第二步无人机飞向21号作业点，同时实时检测无人机是否到达21号航点，达到后判断底部视觉OPENMV识别底部颜色情况，当无人机正下方为农作物时，会设置激光笔闪烁打点。随即会依次发布28、27、20、19、26、25、...、1、2、3、4航点任务，飞机会依次飞到对用航点上见结合OPENMV识别情况决策激光笔打点与否。
第三步4号区域作业点遍历完毕之后，会发布导航原点坐标，飞到起飞点正上方后，判断是否对准，位置对准后飞机原地降落，直至落到地面后触发地面检测条件后会自动给无人机上锁停桨。
- 发挥部分
void Agriculture_UAV_Innovation(void)//发挥部分
发挥题目部分的前三个阶段和基础部分一致，故不再赘述，我们直接从4号区域播撒完毕开始解析，首先飞机会降低巡航高度，到达巡航高度后会进入下一子进程。
到达目标高度后，飞机会飞到14号作业区域，达到后无人机会采用激光雷达去识别塔杆的距离和角度方位。这里飞向14号区域的目的是使得无人机飞到赛场中心位置，避免比赛现场人员靠得太近而造成塔杆误检或者额外增加不必要的逻辑处理过程。
在无人机到达14作业区域后，无人机会根据检测到的塔杆方位、距离信息，首先控制无人机机头对准塔杆，然后再靠近塔杆，控制无人机中心与塔杆的最近距离为50cm。此过程过程中前向机器视觉OPENMV会实时检测是否有识别到条形码信息，对准和靠近过程中只要有识别到条码信息，就会提前结束当前子线程。
识别到条形码信息后，无人机会连续两次闪烁条形码表征的id信息，两次闪烁的间隔时间为3S。
首次闪烁激光笔的过程中，飞机会向后回退到14号作业区域，有的同学可能会好奇，为什么这里并没有再次发布14号区域航点，只有基本自动飞行支持函数，为什么会有自动退回到刚开始对准机头的位置的控制效果呢？这里还是需要回到上一子线程分析：
在34子线程中，我们有对target_position.x，target_position.y进行赋值，并发布航点位置。控制完毕后会进入35子线程中。无人机在机头对准塔杆的过程中，开始时target_yaw_err会大于5°，所以在下方位置-速度控制部分首先会执行②中位置控制部分内容，由于fix_flag初始值为0，故OpticalFlow_Control_Pure(fix_flag)控制效果就是原地悬停，悬停锁定的位置即为34子线程中发布的14号作业区。
接着无人机会继续转动直到机头与塔杆的角度偏差小于5°，此时无人机水平方向会进入③中的水平速度控制模式，无人机前后速度期望来源于前后方向的位置偏差计算出的位置比例控制的输出值；左右方向速度期望为0，即希望无人机左右不发生调整。
在无人机各项参数稳定的前提下，一次完整的机头对准—靠近塔杆控制过程中，有且只会存在一次从②→①的过程，即不存在飞机在靠近的过程中，机头与塔杆之间的方向角不会再次出现大于5°的情况。当且只存在一次只对准塔杆（大角度偏差）→对准的同时靠近塔杆（小角度偏差）时，即不存在②→①→②这一过程，全程位置期望不会被刷新掉，即位置期望仍为34子线程中的位置刷新值，这种情况下当识别到条形码后，进入36子线程后，就可以实现自动回退到14号作业区的效果。当然此处完全可以用位置导航函数Horizontal_Navigation重新发布一个14号作业点坐标，实现主动回退到14作业区。
在第二次闪烁条形码id值后，无人机会飞到4号作业区正下方的区块(200,-50,height)上，然后飞到起飞点正下方的区块(0,-50,height)上。
到达起飞点正下方的区块(0,-50,height)后，无人机会根据条形码id值计算出降落点并发布降落点坐标，无人机会飞到降落点上方后自动降落到底面完成整个飞行任务。
在熟练掌握本案例中的各个API用法前提下，用户进行二次开发可以把机载计算机端当做一个能提供高精度位置信息的传感器来使用就可以，新手用户可以实现不用在机载计算机下编写代码的情况下，就可以完成比赛中的所有内容。在有一定基础积累后，就可以考虑在机载计算机下用ROS系统做导航、避障、路径规划去实现比赛内容，解决问题的途径会更多，处理起来也会更加灵活，另外利用机载计算机端单目摄像头OPENCV处理视觉问题会比传统OPENMV更加强大、高效。
电赛G题植保无人机国奖标准方案学习样例
电赛G题植保无人机国奖标准方案学习样例——NC360深度开源竞赛无人机开发平台_哔哩哔哩_bilibili
2021年电赛G题—植保无人机实现例程—NC360深度开源竞赛无人机开发平台_哔哩哔哩_bilibili
NC360深度开源竞赛无人机开发平台
