# 【飞控】姿态控制-开方控制器和倾斜角转加速度函数

_vel_target.z = AC_AttitudeControl::sqrt_controller(_pos_error.z, _p_pos_z.kP(), _accel_z_cms, _dt);

float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord_lim, float dt)
{
float correction_rate;
if (is_negative(second_ord_lim) || is_zero(second_ord_lim)) {
//二阶限制为零或负数 在这为 _accel_z_cms
correction_rate = error*p;
} else if (is_zero(p)) {
//p为0 在这_p_pos_z.kP()=1
if (is_positive(error)) {
correction_rate = safe_sqrt(2.0f*second_ord_lim*(error));
} else if (is_negative(error)) {
correction_rate = -safe_sqrt(2.0f*second_ord_lim*(-error));
} else {
correction_rate = 0.0f;
}
} else {//zing-note:正常进这个函数
float linear_dist = second_ord_lim/sq(p);
if (error > linear_dist) {//误差非常大
correction_rate = safe_sqrt(2.0f*second_ord_lim*(error-(linear_dist/2.0f)));
} else if (error < -linear_dist) {//误差非常小
correction_rate = -safe_sqrt(2.0f*second_ord_lim*(-error-(linear_dist/2.0f)));
} else {//正常状态
correction_rate = error*p;
}
}
if (!is_zero(dt)) {
return constrain_float(correction_rate, -fabsf(error)/dt, fabsf(error)/dt);
} else {
return correction_rate;
}
}

_vel_target.z=_pos_error.z*_p_pos_z.kP()

void AC_PosControl::lean_angles_to_accel(float& accel_x_cmss, float& accel_y_cmss) const
{
accel_x_cmss = (GRAVITY_MSS * 100) * (-_ahrs.cos_yaw() * _ahrs.sin_pitch() * _ahrs.cos_roll() - _ahrs.sin_yaw() * _ahrs.sin_roll()) / MAX(_ahrs.cos_roll()*_ahrs.cos_pitch(), 0.5f);
accel_y_cmss = (GRAVITY_MSS * 100) * (-_ahrs.sin_yaw() * _ahrs.sin_pitch() * _ahrs.cos_roll() + _ahrs.cos_yaw() * _ahrs.sin_roll()) / MAX(_ahrs.cos_roll()*_ahrs.cos_pitch(), 0.5f);
}

$$R_B^NF_b=\left[\begin{matrix} \times \\\times \\-mg \end{matrix}\right]$$

$$\left[\begin{matrix} \times & \times & \cos\psi\sin\theta\cos\phi+\sin\psi\sin\phi \\ \times & \times & \sin\psi\sin\theta\cos\phi-\cos\psi\sin\phi \\ \times & \times & \cos\psi\sin\theta \end{matrix} \right]\left[\begin{matrix} 0 \\0 \\f \end{matrix}\right] =\left[\begin{matrix} \times \\\times \\-mg \end{matrix}\right]$$

$$F_b=\left[\begin{matrix} 0 \\0 \\\frac{-mg}{\cos\theta\cos\phi} \end{matrix}\right]$$

$$\left[\begin{matrix} F_Nx \\F_Ny \\F_Nz \end{matrix}\right] =\left[\begin{matrix} \times & \times & \cos\psi\sin\theta\cos\phi+\sin\psi\sin\phi \\ \times & \times & \sin\psi\sin\theta\cos\phi-\cos\psi\sin\phi \\ \times & \times & \times \end{matrix}\right] \left[\begin{matrix} 0 \\0 \\\frac{-mg}{\cos\theta\cos\phi} \end{matrix}\right]$$

$$\begin{matrix} F_Nx=(-mg)*(\sin\theta\cos\psi/\cos\theta+\sin\psi\sin\phi/\cos\theta\cos\phi)\\ F_Ny=(-mg)*(\sin\theta\sin\psi/\cos\theta-\cos\psi\sin\phi/\cos\theta\cos\phi) \end{matrix}$$

3 is dependent on there being a large lean angle and angle boost running. If we are in ACRO and at a large lean angle then you are probably holding altitude and the Fbz is probably closer to -mg/(cos_theta * cos_rho). Using the result of 3 will also have the effect of the aircraft going through the unconstrained acceleration part of the Fbz curve on it's way to the low angle causing a large acceleration as it levels. So 3 is not a good option in general.

1 and 2 are reasonable and both will result in a good starting point. 2 will result in a twitch as you say but due to the acceleration limiting in most navigation modes 1 will to. In any case we have to limit the angle to something to prevent the cos terms going to zero so we need some version of 2.

For now I will leave it using the 2 version until we update the output of the navigation to a full 3D acceleration controller.

https://github.com/ArduPilot/ardupilot/issues/7267