void attitude_control_angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle_ef, float pitch_angle_ef, float yaw_rate_ef, float smoothing_gain)
{
float rate_ef_desired;
float rate_change_limit;
Vector3f angle_ef_error;
smoothing_gain = constrain_float(smoothing_gain,1.0f,50.0f);
if ((_accel_roll_max > 0.0f) && _rate_bf_ff_enabled)
{
rate_change_limit = _accel_roll_max * _att_ct_dt;
//111111这里的_angle_ef_target.x还是上一轮计算的结果，那么这个rate_ef_desired 应该是上一次目标到这一次遥感之间的速率吧？
rate_ef_desired = sqrt_controller(roll_angle_ef-_angle_ef_target.x, smoothing_gain, _accel_roll_max);
_rate_ef_desired.x = constrain_float(rate_ef_desired, _rate_ef_desired.x-rate_change_limit, _rate_ef_desired.x+rate_change_limit);
update_ef_roll_angle_and_error(_rate_ef_desired.x, &angle_ef_error, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX);
} else
{
_angle_ef_target.x = roll_angle_ef;
angle_ef_error.x = wrap_180_cd_float(_angle_ef_target.x - _ahrs.roll_sensor);
angle_ef_error.x =constrain_float(angle_ef_error.x,-glo.angle_max,glo.angle_max);
_rate_ef_desired.x = 0;
}
_angle_ef_target.x = constrain_float(_angle_ef_target.x, -glo.angle_max, glo.angle_max);
//省略对pitch 和yaw的处理
frame_conversion_ef_to_bf(&angle_ef_error, &_angle_bf_error);
update_rate_bf_targets();
if (_rate_bf_ff_enabled)
{
frame_conversion_ef_to_bf(&_rate_ef_desired, &_rate_bf_desired);
//22222为什么这里的_rate_bf_target要加上_rate_bf_desired？_rate_bf_desired不已经是速率了吗？还要加上这个干吗？
_rate_bf_target.x += _rate_bf_desired.x;//*0.1f;
_rate_bf_target.y += _rate_bf_desired.y;//*0.1f;
_rate_bf_target.z += _rate_bf_desired.z;
} else
{
_rate_ef_desired.x=0;
_rate_ef_desired.y=0;
frame_conversion_ef_to_bf(&_rate_ef_desired, &_rate_bf_desired);
_rate_bf_target.x += _rate_bf_desired.x;
_rate_bf_target.y += _rate_bf_desired.y;
_rate_bf_target.z += _rate_bf_desired.z;
}
}
谁能帮着解释一下11111和22222处，谢谢！
|
楼主
标题置顶
标题高亮
