浅谈APM系列-----throttle_loop

SCHED_TASK(throttle_loop, 50, 75)

位置：X:\ardupilot\ArduCopter\ArduCopter.cpp

// throttle_loop - should be run at 50 hz
// ---------------------------
void Copter::throttle_loop()
{
    // update throttle_low_comp value (controls priority of throttle vs attitude control)（控制油门与姿态控制的优先级）
    update_throttle_thr_mix();

    // check auto_armed status
    update_auto_armed();

#if FRAME_CONFIG == HELI_FRAME
    // update rotor speed
    heli_update_rotor_speed_targets();

    // update trad heli swash plate movement
    heli_update_landing_swash();
#endif

    // compensate for ground effect (if enabled)
    update_ground_effect_detector();
}

X:\ardupilot\ArduCopter\land_detector.cpp

// update_throttle_thr_mix - 
// sets motors throttle_low_comp value depending upon vehicle state low values favour pilot/autopilot throttle over attitude control, 
//  high values favour attitude control over throttle has no effect when throttle is above hover throttle
void Copter::update_throttle_thr_mix()
{
#if FRAME_CONFIG != HELI_FRAME
    // if disarmed or landed prioritise throttle
    if(!motors->armed() || ap.land_complete) {
        attitude_control->set_throttle_mix_min();
        return;
    }

    if (flightmode->has_manual_throttle()) {//true,飞手手动控制油门
        // manual throttle
        if(channel_throttle->get_control_in() <= 0) {
	// throttle vs attitude control prioritisation used when landing 
	//(higher values mean we prioritise attitude control over throttle)
            attitude_control->set_throttle_mix_min();
        } else {
	// throttle vs attitude control prioritisation used when using manual throttle 
	// (higher values mean we prioritise attitude control over throttle)
            attitude_control->set_throttle_mix_man();
        }
    } else {//自驾仪自动控制油门
        // autopilot controlled throttle

        // check for aggressive flight requests - requested roll or pitch angle below 15 degrees（检查是否有攻击性的飞行请求-要求的滚动或倾斜角度低于15度）
        const Vector3f angle_target = attitude_control->get_att_target_euler_cd();
        bool large_angle_request = (norm(angle_target.x, angle_target.y) > LAND_CHECK_LARGE_ANGLE_CD);// 1500 -> maximum angle target to be considered landing

        // check for large external disturbance - angle error over 30 degrees（检查大的外部干扰-角度误差超过30度）
        const float angle_error = attitude_control->get_att_error_angle_deg();//返回目标推力矢量和当前推力矢量之间的夹角。
        bool large_angle_error = (angle_error > LAND_CHECK_ANGLE_ERROR_DEG);//30 ->  maximum angle error to be considered landing

        // check for large acceleration - falling or high turbulence(检查是否有大的加速度-下降或高湍流)
        Vector3f accel_ef = ahrs.get_accel_ef_blended();
        accel_ef.z += GRAVITY_MSS;//Z方向，抵消重力
        bool accel_moving = (accel_ef.length() > LAND_CHECK_ACCEL_MOVING);//3 -> maximum acceleration after subtracting gravity

        // check for requested decent
        bool descent_not_demanded = pos_control->get_desired_velocity().z >= 0.0f;

        if ( large_angle_request || large_angle_error || accel_moving || descent_not_demanded) {
            // throttle vs attitude control prioritisation used during active flight
            // (higher values mean we prioritise attitude control over throttle)
            attitude_control->set_throttle_mix_max();
        } else {
            // throttle vs attitude control prioritisation used when landing 
            //(higher values mean we prioritise attitude control over throttle)
            attitude_control->set_throttle_mix_min();
        }
    }
#endif
}

---

 

X:\ardupilot\ArduCopter\system.cpp

// update_auto_armed - update status of auto_armed flag
void Copter::update_auto_armed()
{
    // disarm checks（上锁检查）
    if(ap.auto_armed){
        // if motors are disarmed, auto_armed should also be false
        // 如果电机上锁，自动解锁应该是FALSE
        if(!motors->armed()) {
            set_auto_armed(false);
            return;
        }
        // if in stabilize or acro flight mode and throttle is zero, auto-armed should become false
        // 如果在自稳和特技飞行模式并且油门是0，自动解锁应该变成FALSE
        if(flightmode->has_manual_throttle() && ap.throttle_zero && !failsafe.radio) {
            set_auto_armed(false);
        }
#if FRAME_CONFIG == HELI_FRAME
        // if helicopters are on the ground, and the motor is switched off, auto-armed should be false
        // so that rotor runup is checked again before attempting to take-off
        if(ap.land_complete && !motors->rotor_runup_complete()) {
            set_auto_armed(false);
        }
#endif // HELI_FRAME
    }else{
        // arm checks（解锁检查）
        
#if FRAME_CONFIG == HELI_FRAME
        // for tradheli if motors are armed and throttle is above zero and the motor is started, auto_armed should be true
        if(motors->armed() && !ap.throttle_zero && motors->rotor_runup_complete()) {
            set_auto_armed(true);
        }
#else
        // if motors are armed and throttle is above zero auto_armed should be true
        // 如果电机是解锁的并且油门在0之上，自动解锁应该是TRUE
        // if motors are armed and we are in throw mode, then auto_ermed should be true
        // 如果电机解锁并且处于抛飞模式，自动解锁为TRUE
        if(motors->armed() && (!ap.throttle_zero || control_mode == THROW)) {
            set_auto_armed(true);
        }
#endif // HELI_FRAME
    }
}

下面是最后一个函数调用了。

// compensate for ground effect (if enabled)（如果使能的话，弥补地面效应）
update_ground_effect_detector();

x:\ardupilot\ArduCopter\baro_ground_effect.cpp

void Copter::update_ground_effect_detector(void)
{
    if(!g2.gndeffect_comp_enabled || !motors->armed()) {//电机没解锁或者没有使能地形匹配
        // disarmed - disable ground effect and return
        gndeffect_state.takeoff_expected = false;
        gndeffect_state.touchdown_expected = false;
        ahrs.setTakeoffExpected(gndeffect_state.takeoff_expected);
        ahrs.setTouchdownExpected(gndeffect_state.touchdown_expected);
        return;
    }

    // variable initialization
    uint32_t tnow_ms = millis();
    float xy_des_speed_cms = 0.0f;
    float xy_speed_cms = 0.0f;
    float des_climb_rate_cms = pos_control->get_desired_velocity().z;// desired velocity in cm/s

    if (pos_control->is_active_xy()) {// is_active_xy - returns true if the xy position controller has been run very recently
        Vector3f vel_target = pos_control->get_vel_target();//获取目标速度
        vel_target.z = 0.0f;//将Z方向的目标速度设置为0
        xy_des_speed_cms = vel_target.length();//XY方向的速度和
    }

    // position_ok - returns true if the horizontal absolute position is ok and home position is set
    // optflow_position_ok - returns true if optical flow based position estimate is ok
    if (position_ok() || optflow_position_ok()) {
        Vector3f vel = inertial_nav.get_velocity();
        vel.z = 0.0f;
        xy_speed_cms = vel.length();
    }

    // takeoff logic（起飞逻辑）

    // if we are armed and haven't yet taken off（如果已经解锁，还没有起飞）
    if (motors->armed() && ap.land_complete && !gndeffect_state.takeoff_expected) {
        gndeffect_state.takeoff_expected = true;
    }

    // if we aren't taking off yet, reset the takeoff timer, altitude and complete flag（如果我们还没有起飞，重置起飞计时器，高度和完成标志）
    const bool throttle_up = flightmode->has_manual_throttle() && channel_throttle->get_control_in() > 0;
    if (!throttle_up && ap.land_complete) {//貌似起飞之后ap.land_complete会发生改变，下面的代码就不执行了
        gndeffect_state.takeoff_time_ms = tnow_ms;//在有油门，并且没有起飞的情况下，一直更新
        gndeffect_state.takeoff_alt_cm = inertial_nav.get_altitude();
    }

    // if we are in takeoff_expected and we meet the conditions for having taken off
    // end the takeoff_expected state（如果我们在预期的情况下我们会满足起飞的条件）
    if (gndeffect_state.takeoff_expected && (tnow_ms-gndeffect_state.takeoff_time_ms > 5000 || inertial_nav.get_altitude()-gndeffect_state.takeoff_alt_cm > 50.0f)) {// 时间超过5秒或者高度超过50厘米
        gndeffect_state.takeoff_expected = false;//修改标志位
    }

    // landing logic（着陆逻辑）

    Vector3f angle_target_rad = attitude_control->get_att_target_euler_cd() * radians(0.01f);
    bool small_angle_request = cosf(angle_target_rad.x)*cosf(angle_target_rad.y) > cosf(radians(7.5f));
    bool xy_speed_low = (position_ok() || optflow_position_ok()) && xy_speed_cms <= 125.0f;
    bool xy_speed_demand_low = pos_control->is_active_xy() && xy_des_speed_cms <= 125.0f;
    bool slow_horizontal = xy_speed_demand_low || (xy_speed_low && !pos_control->is_active_xy()) || (control_mode == ALT_HOLD && small_angle_request);

    bool descent_demanded = pos_control->is_active_z() && des_climb_rate_cms < 0.0f;
    bool slow_descent_demanded = descent_demanded && des_climb_rate_cms >= -100.0f;
    bool z_speed_low = fabsf(inertial_nav.get_velocity_z()) <= 60.0f;
    bool slow_descent = (slow_descent_demanded || (z_speed_low && descent_demanded));

    gndeffect_state.touchdown_expected = slow_horizontal && slow_descent;//是否检测到着陆,两个必须都是TRUE，着陆才完成

    // Prepare the EKF for ground effect if either takeoff or touchdown is expected.（如果预期起飞或着陆，准备EKF的地面效果）
    ahrs.setTakeoffExpected(gndeffect_state.takeoff_expected);
    ahrs.setTouchdownExpected(gndeffect_state.touchdown_expected);
}

今天就先到这里吧。

---

未完。。。。。