Ardupilot之Throw mode代码分析

只是进行了代码的注释，分析也注释当中，（代码很短，通过几个状态就能很好的理清逻辑关系了，刚开始没必要深究代码的实现，知道实现了哪些功能，并如何调用即可）没有上飞机测试，现在只是纸上谈兵，将来有飞机测试之后再来更新心得，并好好地梳理一下结构。

废话少说，代码如下：

#include "Copter.h"

#if MODE_THROW_ENABLED == ENABLED

// throw_init - initialise throw controller
bool Copter::ModeThrow::init(bool ignore_checks)
{
#if FRAME_CONFIG == HELI_FRAME
    // do not allow helis to use throw to start 不允许直升机抛飞
    return false;
#endif

    // do not enter the mode when already armed or when flying 已解锁或飞行时不进入Throw模式
    if (motors->armed()) {
        return false;
    }

    // init state
    stage = Throw_Disarmed;
    nextmode_attempted = false;

    return true;
}

// runs the throw to start controller
// should be called at 100hz or more
void Copter::ModeThrow::run()
{
    /* Throw State Machine
    Throw_Disarmed - motors are off  未解锁
    Throw_Detecting -  motors are on and we are waiting for the throw 电机解锁 等待被扔
    Throw_Uprighting -  被扔出去了，并向上飞the throw has been detected and the copter is being uprighted
    Throw_HgtStabilise - 定高the copter is kept level and  height is stabilised about the target height
    Throw_PosHold 定点- the copter is kept at a constant position and height
    */

    // Don't enter THROW mode if interlock will prevent motors running
    if (!motors->armed() && motors->get_interlock()) {          //电机一旦解锁便进入下一个判断 否则将状态设为Throw_Disarmed
        // state machine entry is always from a disarmed state
        stage = Throw_Disarmed;

    } else if (stage == Throw_Disarmed && motors->armed()) {  //若电机解锁 则向上位机发送waiting for throw，并将状态设为Throw_Detecting（等待被扔）
        gcs().send_text(MAV_SEVERITY_INFO,"waiting for throw");
        stage = Throw_Detecting;

    } else if (stage == Throw_Detecting && throw_detected()){  //状态为等待被扔，且被检测到，向地面站发送throw detected - uprighting，将状态设为被扔出去了，并向上飞
        gcs().send_text(MAV_SEVERITY_INFO,"throw detected - uprighting");
        stage = Throw_Uprighting;

        // Cancel the waiting for throw tone sequence
        AP_Notify::flags.waiting_for_throw = false;

    } else if (stage == Throw_Uprighting && throw_attitude_good()) { //状态为已被扔且 姿态处于正常 向地面站发送uprighted - controlling height，并将状态设为Throw_HgtStabilise（定高）
        gcs().send_text(MAV_SEVERITY_INFO,"uprighted - controlling height");
        stage = Throw_HgtStabilise;

        // initialize vertical speed and acceleration limits 初始化垂直速度和加速度限制
        // use brake mode values for rapid response   使用制动模式值进行快速响应
        pos_control->set_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z);//设置最大爬升和下降速率
        pos_control->set_accel_z(BRAKE_MODE_DECEL_RATE); //设置垂直加速度

        // initialise the demanded height to 3m above the throw height 将要求的高度初始化为距离投掷高度3米
        // we want to rapidly clear surrounding obstacles
        if (g2.throw_type == ThrowType_Drop) {         //若向下扔  设置目标高度在解锁时下面一米
            pos_control->set_alt_target(inertial_nav.get_altitude() - 100);  //设定在家上的目标高度（cm）
        } else {
            pos_control->set_alt_target(inertial_nav.get_altitude() + 300); //向上抛，设置目标高度在解锁时上三米
        }

        // set the initial velocity of the height controller demand to the measured velocity if it is going up
        //如果飞机正在上升，则将高度控制器的初始速度设置为当前速度
        // if it is going down, set it to zero to enforce a very hard stop  //如果飞机下落，那么将Z轴速度置0
        pos_control->set_desired_velocity_z(fmaxf(inertial_nav.get_velocity_z(),0.0f));//取最大值，一旦飞机下落get_velocity_z()将为负值

        // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
        copter.set_auto_armed(true);//设置自动解锁

    } else if (stage == Throw_HgtStabilise && throw_height_good()) { //若状态为定高且高度误差在0.5米以内，将则将状态设为定点模式
        gcs().send_text(MAV_SEVERITY_INFO,"height achieved - controlling position");
        stage = Throw_PosHold;

        // initialise the loiter target to the curent position and velocity
        loiter_nav->clear_pilot_desired_acceleration(); //加速度归零
        loiter_nav->init_target(); // 初始化当前模式下的速度与位置

        // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
        copter.set_auto_armed(true);//设置自动解锁
    } else if (stage == Throw_PosHold && throw_position_good()) { //一切就绪，等待模式切换
        if (!nextmode_attempted) {    //在初始化时 nextmode_attempted = false；
            switch (g2.throw_nextmode) { //参数序列
                case AUTO:  //自动
                case GUIDED://引导模式
                case RTL://返回目标点
                case LAND://着陆
                case BRAKE://刹车 需要GPS支持，一旦执行，不接受飞行操控信号
                case LOITER://悬停
                    set_mode((control_mode_t)g2.throw_nextmode.get(), MODE_REASON_THROW_COMPLETE);//将模式设置到切换的模式
                    break;
                default:
                    // do nothing
                    break;
            }
            nextmode_attempted = true;
        }
    }

    // Throw State Processing  //状态处理
    switch (stage) {

    case Throw_Disarmed:

        // prevent motors from rotating before the throw is detected unless enabled by the user
        //非用户启用，否则在检测到抛出之前，请防止电机旋转
        if (g.throw_motor_start == 1) {
            motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);//设置假脱机状态  解锁时电机开始自旋
        } else {
            motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); //所有电机停止
        }

        // demand zero throttle (motors will be stopped anyway) and continually reset the attitude controller
        //0油门
        attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
        break;

    case Throw_Detecting:

        // prevent motors from rotating before the throw is detected unless enabled by the user
        if (g.throw_motor_start == 1) {
            motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
        } else {
            motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
        }

        // Hold throttle at zero during the throw and continually reset the attitude controller
        //在投掷过程中保持油门为零，并不断重置姿态控制器
        attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);

        // Play the waiting for throw tone sequence to alert the user
        AP_Notify::flags.waiting_for_throw = true;

        break;

    case Throw_Uprighting:

        // set motors to full range
        motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

        // demand a level roll/pitch attitude with zero yaw rate
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);

        // output 50% throttle and turn off angle boost to maximise righting moment
        attitude_control->set_throttle_out(0.5f, false, g.throttle_filt);

        break;

    case Throw_HgtStabilise:

        // set motors to full range
        motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

        // call attitude controller
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);

        // call height controller
        pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
        pos_control->update_z_controller();

        break;

    case Throw_PosHold:

        // set motors to full range
        motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

        // run loiter controller
        loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);

        // call attitude controller
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), 0.0f);

        // call height controller
        pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
        pos_control->update_z_controller();

        break;
    }

    // log at 10hz or if stage changes  记录飞行日志
    uint32_t now = AP_HAL::millis();
    if ((stage != prev_stage) || (now - last_log_ms) > 100) { //状态改变 或 频率大于10Hz
        prev_stage = stage;
        last_log_ms = now;
        const float velocity = inertial_nav.get_velocity().length();
        const float velocity_z = inertial_nav.get_velocity().z;
        const float accel = copter.ins.get_accel().length();
        const float ef_accel_z = ahrs.get_accel_ef().z;
        const bool throw_detect = (stage > Throw_Detecting) || throw_detected();
        const bool attitude_ok = (stage > Throw_Uprighting) || throw_attitude_good();
        const bool height_ok = (stage > Throw_HgtStabilise) || throw_height_good();
        const bool pos_ok = (stage > Throw_PosHold) || throw_position_good();
        DataFlash_Class::instance()->Log_Write(
            "THRO",
            "TimeUS,Stage,Vel,VelZ,Acc,AccEfZ,Throw,AttOk,HgtOk,PosOk",
            "s-nnoo----",
            "F-0000----",
            "QBffffbbbb",
            AP_HAL::micros64(),
            (uint8_t)stage,
            (double)velocity,
            (double)velocity_z,
            (double)accel,
            (double)ef_accel_z,
            throw_detect,
            attitude_ok,
            height_ok,
            pos_ok);
    }
}

bool Copter::ModeThrow::throw_detected() //投掷检测
{
    // Check that we have a valid navigation solution
    nav_filter_status filt_status = inertial_nav.get_filter_status();
    if (!filt_status.flags.attitude || !filt_status.flags.horiz_pos_abs || !filt_status.flags.vert_pos) {
        return false;
    }

    // Check for high speed (>500 cm/s)
    bool high_speed = inertial_nav.get_velocity().length() > THROW_HIGH_SPEED;

    // check for upwards or downwards trajectory (airdrop) of 50cm/s
    //检查向上或向下的轨迹(空投)是否为50cm/s
    bool changing_height;
    if (g2.throw_type == ThrowType_Drop) {
        changing_height = inertial_nav.get_velocity().z < -THROW_VERTICAL_SPEED;
    } else {
        changing_height = inertial_nav.get_velocity().z > THROW_VERTICAL_SPEED;
    }

    // Check the vertical acceleraton is greater than 0.25g
    bool free_falling = ahrs.get_accel_ef().z > -0.25 * GRAVITY_MSS;

    // Check if the accel length is < 1.0g indicating that any throw action is complete and the copter has been released
    bool no_throw_action = copter.ins.get_accel().length() < 1.0f * GRAVITY_MSS;

    // High velocity or free-fall combined with increasing height indicate a possible air-drop or throw release
    bool possible_throw_detected = (free_falling || high_speed) && changing_height && no_throw_action;

    // Record time and vertical velocity when we detect the possible throw
    if (possible_throw_detected && ((AP_HAL::millis() - free_fall_start_ms) > 500)) {
        free_fall_start_ms = AP_HAL::millis();
        free_fall_start_velz = inertial_nav.get_velocity().z;
    }

    // Once a possible throw condition has been detected, we check for 2.5 m/s of downwards velocity change in less than 0.5 seconds to confirm
    //一旦检测到可能的抛出条件，我们会在0.5秒内检查2.5 m/s的向下速度变化来确认
    bool throw_condition_confirmed = ((AP_HAL::millis() - free_fall_start_ms < 500) && ((inertial_nav.get_velocity().z - free_fall_start_velz) < -250.0f));

    // start motors and enter the control mode if we are in continuous freefall
    //启动电机，进入控制模式，如果我们处于连续自由落体状态
    if (throw_condition_confirmed) {
        return true;
    } else {
        return false;
    }
}

bool Copter::ModeThrow::throw_attitude_good()
{
    // Check that we have uprighted the copter 检查一下我们是否已将直升机扶正
    const Matrix3f &rotMat = ahrs.get_rotation_body_to_ned();
    return (rotMat.c.z > 0.866f); // is_upright
}

bool Copter::ModeThrow::throw_height_good()
{
    // Check that we are within 0.5m of the demanded height //高度误差 <0.5m
    return (pos_control->get_alt_error() < 50.0f);
}

bool Copter::ModeThrow::throw_position_good()
{
    // check that our horizontal position error is within 50cm //水平面误差小于0.5m
    return (pos_control->get_horizontal_error() < 50.0f);
}
#endif