PX4自主返航（RTL）控制逻辑

本文基于PX4飞控1.5.5版本，分析导航模块中自护返航模式的控制逻辑和算法。

自主返航模式和导航中的其他模式一样，在Navigator_main函数中一旦触发case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:任务指令，导航模式_navigation_mode = &_rtl;即进入自主返航模式。

依次执行初始化函数RTL::on_activation()、主函数RTL::on_active()、退出函数RTL::on_inactive()。

1.首先是初始化函数的控制逻辑：on_activation()

（1）将当前位置设为_mission_item,再赋值给当前任务航点。

set_current_position_item(&_mission_item);
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);

保证第一个航点为当前位置，从而实现爬升等一系列动作。
（2）根据当前位置到home点的距离，当前高度等参数选择进入哪种子模式。
①如果已经落地，则进入落地状态

_rtl_state = RTL_STATE_LANDED;

②如果距离home点比较远，同时当前高度小于返航高度，则先爬升

_rtl_state = RTL_STATE_CLIMB;

③其他情况下，直接以当前高度飞往home点_rtl_state = RTL_STATE_RETURN;
注：主要针对当前高度大于返航高度的情况

2.主函数on_active()

主函数的功能在于返航状态切换，一个子返航模式完成后，进入下一个模式，顺序进行。然后设定对应的任务航点信息set_rtl_item()，然后进行航点位置控制。
RTL控制逻辑：
（1）爬升模式RTL_STATE_CLIMB
从当前位置爬升到指定高度climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();经纬度不变。任务类型为航点任务_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
航点：指定返航高度、当前经纬度、不进行航向控制
（2）返回模式RTL_STATE_RETURN
在指定返回高度上到达home点上方
当距离home点的距离home_dist < _param_rtl_min_dist（默认5m），航向转为home点初始航向；
如果距离较远，同时上一航点有效，则航向为从上一航点指向home点；上一航点无效则航向从当前位置指向home点。

if (home_dist < _param_rtl_min_dist.get()) {
    _mission_item.yaw = _navigator->get_home_position()->yaw;
} else {
    if (pos_sp_triplet->previous.valid) {
    /* if previous setpoint is valid then use it to calculate heading to home */
        _mission_item.yaw = get_bearing_to_next_waypoint(
        pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
        _mission_item.lat, _mission_item.lon);
    } else {
    /* else use current position */
        _mission_item.yaw = get_bearing_to_next_waypoint(
        navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
        _mission_item.lat, _mission_item.lon);
    }
}

航点：home点经纬度，返航高度，进行航向控制
（3）RTL_STATE_DESCEND
降落，home点经纬度，高度为指定的落地高度_param_descend_alt

_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();

_param_land_delay如果在-0.1到0.1之间，则悬停loiter；否则直降降落landing（默认为-1，直接降落）
航点：home点经纬度，高度：落地高度QGC默认2.5m，航向为home点的原始航向
（4）RTL_STATE_LOITER
loiter：时间航向都不变，只是具有一定的悬停时间
在descend的航点处悬停一定时间，航向为home点的原始航向
loiter之后再land，
在land之前都是位置航点控制_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
（5）RTL_STATE_LAND
land：经纬度为home点的经纬度，高度为0（海拔）set_land_item
航点：航向为home处的原始航向，经纬度为home点高度，高度为海拔0m，任务模式为NAV_CMD_LAND，参看pos控制
在is_mission_item_reached中NAV_CMD_LAND
返回 _navigator->get_land_detected()->landed，因此在此处进行是否已落地检测，落地后即为真，从而进入下一个任务landed