ardupilot/arduplane/takeoff 起飞模式分析（2）-mode_takeoff.cpp

#include "mode.h"
#include "Plane.h"

/*
  起飞模式参数
 */
const AP_Param::GroupInfo ModeTakeoff::var_info[] = {
    // @参数: ALT
    // @显示名称: Takeoff mode altitude
    // @描述: 起飞模式下的目标高度
    // @范围: 0 200
    // @增量: 1
    // @单位: m
    // @优先级: 标准
    AP_GROUPINFO("ALT", 1, ModeTakeoff, target_alt, 50),

    // @参数: LVL_ALT
    // @显示名称: Takeoff mode altitude level altitude
    // @描述: 起飞模式下 无人机机翼保持水平的高度，也就是说，在该高度下，无人机禁止滚转
    // @范围: 0 50
    // @增量: 1
    // @单位: m
    // @优先级: 标准
    AP_GROUPINFO("LVL_ALT", 2, ModeTakeoff, level_alt, 20),

    // @参数: LVL_PITCH
    // @显示名称: Takeoff mode altitude initial pitch
    // @描述: 初始爬升至TKOFF_LVL_ALT的目标俯仰角度
    // @范围: 0 30
    // @增量: 1
    // @单位: deg
    // @优先级: 标准
    AP_GROUPINFO("LVL_PITCH", 3, ModeTakeoff, level_pitch, 15),

    // @参数: DIST
    // @显示名称: Takeoff mode distance
    // @描述: 这是无人机相对起飞位置开始定点盘旋的距离，方位与位于起飞方向相同（发动机启动时飞机所面对的方向）
    // @范围: 0 500
    // @增量: 1
    // @单位: m
    // @优先级: 标准
    AP_GROUPINFO("DIST", 4, ModeTakeoff, target_dist, 200),
    
    AP_GROUPEND
};

ModeTakeoff::ModeTakeoff() :
    Mode()
{
    AP_Param::setup_object_defaults(this, var_info);
}

bool ModeTakeoff::_enter()
{
    takeoff_started = false;

    return true;
}

void ModeTakeoff::update()
{
    if (!takeoff_started) {
        // 判断是否已经在飞行，如果是则跳过自动起飞阶段
        if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && plane.ahrs.groundspeed() > 3) {
            gcs().send_text(MAV_SEVERITY_INFO, "Takeoff skipped - circling");
            plane.prev_WP_loc = plane.current_loc;
            plane.next_WP_loc = plane.current_loc;
            takeoff_started = true;
            plane.set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
        }
    }

    if (!takeoff_started) {
        //设置目标位置，距离起飞点1.5倍盘旋半径，高度为TKOFF_ALT
        const float dist = target_dist;
        const float alt = target_alt;
        const float direction = degrees(plane.ahrs.yaw);

        start_loc = plane.current_loc;
        plane.prev_WP_loc = plane.current_loc;
        plane.next_WP_loc = plane.current_loc;
        plane.next_WP_loc.alt += alt*100.0;
        plane.next_WP_loc.offset_bearing(direction, dist);

        plane.crash_state.is_crashed = false;

        plane.auto_state.takeoff_pitch_cd = level_pitch * 100;

        plane.set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_TAKEOFF);

        if (!plane.throttle_suppressed) {
            gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm at %.1fm to %.1f deg",
                            alt, dist, direction);
            takeoff_started = true;
        }
    }

    //如果高度到达TKOFF_LVL_ALT或通过目标位置，则认为完成了初始水平起飞。如果无法爬升，目标位置的检查将避免永远飞行的情况
    if (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF &&
        (plane.current_loc.alt - start_loc.alt >= level_alt*100 ||
         start_loc.get_distance(plane.current_loc) >= target_dist)) {
        //达到设定高度，重新计算方位以应对无人机在初始起飞时没有罗盘或初始罗盘较差的情况
        float direction = start_loc.get_bearing_to(plane.current_loc) * 0.01;
        float dist_done = start_loc.get_distance(plane.current_loc);
        const float dist = target_dist;

        plane.next_WP_loc = plane.current_loc;
        plane.next_WP_loc.offset_bearing(direction, MAX(dist-dist_done, 0));
        plane.next_WP_loc.alt = start_loc.alt + target_alt*100.0;

        plane.set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
        
#if AP_FENCE_ENABLED
        plane.fence.auto_enable_fence_after_takeoff();
#endif
    }

    if (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF) {
        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 100.0);
        plane.takeoff_calc_roll();
        plane.takeoff_calc_pitch();
    } else {
        plane.calc_nav_roll();
        plane.calc_nav_pitch();
        plane.calc_throttle();
    }
}

void ModeTakeoff::navigate()
{

    //0表示使用WP_LOITER_RAD
    plane.update_loiter(0);
}