AC_PosControl.cpp的AC_PosControl::set_alt_target_from_climb_rate_ff函数代码分析

void AC_PosControl::set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend)

函数作用：使用爬升率作为前馈，调节高度。需要连续调用，高度变化不会超过上下速度限制，不会超过电机限制，着陆期间，运行电机拉力下降的足够慢。

函数全局调用情况如下：18处出现，包括函数定义和函数调用

 函数声明情况如下：

函数声明类情况如下：

函数流程图如下：

函

void AC_PosControl::set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend)

{

    // calculated increased maximum acceleration if over speed如果超速，计算最大加速度

    float accel_z_cms = _accel_z_cms;

    if (_vel_desired.z < _speed_down_cms && !is_zero(_speed_down_cms)) {

        accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _speed_down_cms;

    }

    if (_vel_desired.z > _speed_up_cms && !is_zero(_speed_up_cms)) {

        accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _speed_up_cms;

    }

    accel_z_cms = constrain_float(accel_z_cms, 0.0f, 750.0f);

    // jerk_z is calculated to reach full acceleration in 1000ms.Z轴的速度变化

    float jerk_z = accel_z_cms * POSCONTROL_JERK_RATIO;

    float accel_z_max = MIN(accel_z_cms, safe_sqrt(2.0f*fabsf(_vel_desired.z - climb_rate_cms)*jerk_z));

    _accel_last_z_cms += jerk_z * dt;

    _accel_last_z_cms = MIN(accel_z_max, _accel_last_z_cms);

    float vel_change_limit = _accel_last_z_cms * dt;

    _vel_desired.z = constrain_float(climb_rate_cms, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit);

    _flags.use_desvel_ff_z = true;

    // adjust desired alt if motors have not hit their limits执行高度变化

    // To-Do: add check of _limit.pos_down?

    if ((_vel_desired.z<0 && (!_motors.limit.throttle_lower || force_descend)) || (_vel_desired.z>0 && !_motors.limit.throttle_upper && !_limit.pos_up)) {

        _pos_target.z += _vel_desired.z * dt;

    }

    // do not let target alt get above limit保证高度不超过限制

    if (_alt_max > 0 && _pos_target.z > _alt_max) {

        _pos_target.z = _alt_max;

        _limit.pos_up = true;

        // decelerate feed forward to zero减速到零

        _vel_desired.z = constrain_float(0.0f, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit);

    }

}

数原始代码如下：