Avoidance Adjusted Climbrate

// 输入z轴位置控制P，且得到加速度z ，目标攀爬率和时间间隔。
avoid.adjust_velocity_z(pos_control->get_pos_z_p().kP(), pos_control->get_accel_z(), target_rate, G_Dt);

// adjust vertical climb rate so vehicle does not break the vertical fence
void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float dt)
{
    // 若是没开启则立即退出，目前是开启的
    if (_enabled == AC_AVOID_DISABLED) {
        return;
    }
    
    // 若是飞机高度没有速度或者在下降则不进入此函数
    if (climb_rate_cms <= 0.0f) {
        return;
    }

    // 限制避障加速度
    float accel_cmss_limited = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX);

    bool limit_alt = false;
    float alt_diff = 0.0f;   // 从限制的高度到飞行器距离(正的意味着，处于限制高度下面)。

    // 由于在这里的fence没开启，则次if不进入
    if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0 && (_fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) > 0) {
        // calculate distance from vehicle to safe altitude
        float veh_alt;
        _ahrs.get_relative_position_D_home(veh_alt);
        // _fence.get_safe_alt_max() is UP, veh_alt is DOWN:
        alt_diff = _fence.get_safe_alt_max() + veh_alt;
        limit_alt = true;
    }
  

    // calculate distance to (e.g.) optical flow altitude limit
    // 限制高度。
    float alt_limit;
    float curr_alt;
    if (_ahrs.get_hgt_ctrl_limit(alt_limit) &&
        _ahrs.get_relative_position_D_origin(curr_alt)) {
        // alt_limit is UP, curr_alt is DOWN:
        const float ctrl_alt_diff = alt_limit + curr_alt;
        if (!limit_alt || ctrl_alt_diff < alt_diff) {
            alt_diff = ctrl_alt_diff;
            limit_alt = true;
        }
    }

    // 获得避障传感器距离 ，此时要是距离小于_margin,则limit_alt置位。
    float proximity_alt_diff;
    if (_proximity.get_upward_distance(proximity_alt_diff)) {
        proximity_alt_diff -= _margin; //减去警戒值AVOID_MARGIN，此值和水平方向共用一个，可以去设置个特定用于高度上的
        if (!limit_alt || proximity_alt_diff < alt_diff) {
            alt_diff = proximity_alt_diff;
            limit_alt = true;
        }
    }

    // limit climb rate
    if (limit_alt) {
        // do not allow climbing if we've breached the safe altitude
        if (alt_diff <= 0.0f) {
            climb_rate_cms = MIN(climb_rate_cms, 0.0f);
            return;
        }

        // limit climb rate
        const float max_speed = get_max_speed(kP, accel_cmss_limited, alt_diff*100.0f, dt);
        climb_rate_cms = MIN(max_speed, climb_rate_cms); // 最后达到了限制的目的。
    }
}