PX4-航点到达判定代码分析

这里主要分析源代码https://github.com/PX4/Firmware/blob/master/src/modules/navigator/mission_block.cpp

里面的is_mission_item_reached 判断航点是否到达的代码，代码分析如下：

航点到达判断

当飞行器的高度误差小于爬升高度误差时进入航点飞行模式

航点到达判断流程如下所示：

1、如果为特殊飞行模式，比如空闲模式、录像模式、相机设置模式，直接判定返回
2、检测到起飞 且 路径点未到达  进入以下判定
    2.1 如果不是旋翼 且 飞行模式为航点飞行模式 进入以下判定
        2.1.1 如果水平误差小于2*NAV_LOITER_RAD 且 高度误差大于2*NAV_FW_ALT_RAD
                进入盘旋模式 LOITER飞行模式
        2.1.2 如果水平误差小于1.2*NAV_LOITER_RAD 且 高度误差小于NAV_FW_ALT_RAD
                盘旋上升完成，重新进入定点飞行模式
    2.2 如果是旋翼 且 模式为起飞
        如果飞行器高度 大于 航点高度减NAV_FW_ALT_RAD 
            则认为航点已经到达
    2.3 其他飞行器起飞模式
        如果距离误差小于NAV_ACC_RAD 且 垂直误差小于NAC_FW_ALT_RAD
            则认为航点已经到达
    2.4 不是旋翼 且 模式为空闲飞行模式
        如果距离误差小于1.2*NAV_ACC_RAD 且 垂直误差小于NAC_FW_ALT_RAD
            则认为航点已经到达
    2.5 不是旋翼 且 模式为空闲盘旋上升模式
        如果距离误差小于1.2*NAV_ACC_RAD 且 垂直误差小于NAC_FW_ALT_RAD
            则认为航点已经到达
    2.6 如果是 DELAY 模式
        直接认为到达
    2.7 其他模式 (正常航点飞行模式)
        计算 mission_accp_rad
        距离误差小于mission_accp_rad 且 垂直误差小于NAC_FW_ALT_RAD
            则航点已到达
3、路径点已到达 且 飞机偏航角未到达 进入以下判定
    计算航向误差
    如果误差在阈值范围内 且 航向不需要保持
        则航向角已到达
4、路径点已到达 且 偏航角已到达 进入以下判定
    检查飞机在航点轨道内是否足够长
5、非以上情况 
    返回false 未到达