PX4飞控之导航及任务架构

感谢博主的分享，原文地址：http://blog.youkuaiyun.com/zhenglongf/article/details/77775658

本文重点介绍PX4飞控的Navigator和mission控制框架和逻辑。Navigator导航部分是无人机自主飞行控制的核心所在，其中包括自主起飞、自主降落、自主返航、自主任务以及GPS失效保护等各个部分。搞懂这个部分有助于理解无人机的自主飞行过程，闲话少说，上干货。 
PX4飞控的整体架构参见Px4源码框架结构图 
 
Navigator模块主要功能在于确定任务类型、地理围栏、失效保护，把任务航点更新后送给位置控制器。其中task_main函数是主函数。 
 
在task_main 函数中，主要分为如下几个部分： 
 
当commander中的传输来的导航模式为mission时，依次运行mission.cpp中的三个函数：初始化on_activation()、主函数on_active()、退出函数on_inactive()。 
对于mission模式的主函数on_active()，代码逻辑如下： 
 
mission的主要功能在于对航点数组pos_sp_triplet更新和赋值，航点数组包括previous、current、next三个航点，这一功能由函数set_mission_item()来实现，主要的逻辑为先从SD卡中读取航点信息read_mission_item()赋值给结构体mission_item，然后再将当前航点复制给pos_sp_triplet.current 
 
每当一个航点任务完成is_mission_item_reached()返回值为true，则指针++，读取SD卡中的下一个航点。而判断航点的是否完成由三个判断条件： 
 
航向控制由函数heading_sp_update()实现，将航向setpoint计算赋值给pos_sp_triplet.current.yaw,给到pos控制环，进行航向控制。 
 
至此，导航完成了航点经纬高、航点类型、航向等信息的更新。而位置控制环以pos_sp_triplet为目标进行控制。 
注：航点数组中的高度为绝对高度。