PX4飞控之自主起飞Takeoff控制逻辑

本文主要以PX4飞控1.5.5版本为例，介绍Navigator中自主起飞（Takeoff）算法控制逻辑。
注：mission任务中的自主起飞与此模块不同。
Takeoff与导航中的其他模块类似，主要由由初始化函数on_activation()、主线程on_active()、退出函数on_inactive()组成，但真正发挥起飞作用的是函数set_takeoff_position()。
1.set_takeoff_position()：
（1）起飞高度设定
①　最小起飞高度min_abs_altitude，默认值为home点高度+_param_min_alt.get()，默认参数为2.5m，可在地面站中进行设定

const float min_abs_altitude = _navigator->get_home_position()->alt + _param_min_alt.get();

②　起飞高度

if (rep->current.valid && PX4_ISFINITE(rep->current.alt)) {
    abs_altitude = rep->current.alt;
    if (abs_altitude < min_abs_altitude) {
        abs_altitude = min_abs_altitude;
        mavlink_log_critical(_navigator->get_mavlink_log_pub(),
         "Using minimum takeoff altitude: %.2f m", (double)_param_min_alt.get());
    }
} else {
    abs_altitude = min_abs_altitude;
    mavlink_log_info(_navigator->get_mavlink_log_pub(),
     "Using minimum takeoff altitude: %.2f m", (double)_param_min_alt.get());
}

注：如果在地面站中设定了takeoff点，则进行比较取较小值作为起飞高度，否则直接用最小起飞高度。
③比较起飞高度和当前高度　
if (abs_altitude < _navigator->get_global_position()->alt) {
abs_altitude = _navigator->get_global_position()->alt;
mavlink_log_critical(_navigator->get_mavlink_log_pub(),
“Already higher than takeoff altitude”);
}
注：如果当前位置高度大于起飞高度，则起飞高度为当前位置高度。防止在空中切换自主起飞时，飞机降落。
④　设定起飞的高度和经纬度

set_takeoff_item(&_mission_item, abs_altitude);

对mission进行高度修正，mission的经纬度为当前经纬度。
⑤　把mission赋值给current航点

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

（2）　航向

pos_sp_triplet->current.yaw = _navigator->get_home_position()->yaw;

注：起飞时的航向等于home点的航向
（3）经纬度、航向修正

if (rep->current.valid) {
    if (PX4_ISFINITE(rep->current.yaw)) {
        pos_sp_triplet->current.yaw = rep->current.yaw;
    }
    if (PX4_ISFINITE(rep->current.lat) && PX4_ISFINITE(rep->current.lon)) {
            pos_sp_triplet->current.lat = rep->current.lat;
            pos_sp_triplet->current.lon = rep->current.lon;
    }
    memset(rep, 0, sizeof(*rep));
}

如果有takeoff航点，则设定takeoff航点的经纬度、航向作为起飞经纬度和航向。
2.达到起飞高度、航向后
当前任务完成is_mission_item_reached()（航点航向和时间都完成）只进入一次，因为进入一次以后get_mission_result()->finished 为真，就不再进入该模式。

else if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) {
    navigator->get_mission_result()->finished = true;
    _navigator->set_mission_result_updated();
    set_loiter_item(&_mission_item);
    struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
    mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
    _navigator->set_position_setpoint_triplet_updated();
}

注：起飞任务完成后，在当前位置悬停