Ardupilot四元数姿态控制函数attitude_controller_run_quat解析

在开始阅读本文之前，建议可以先看一下前面几篇博文，当然你基础好直接看也是没事的~
详解APM的开方控制器sqrt_controller
Ardupilot倾转分离函数thrust_heading_rotation_angles
Ardupilot前馈及平滑函数input_euler_angle_roll_pitch_yaw解析

源码解析

以n系简称NED坐标系，b系简称机体坐标系，tb表示期望姿态，cb表示当前姿态。

// 根据姿态误差计算得出期望角速率
void AC_AttitudeControl::attitude_controller_run_quat()
{
   
   
	// 检索四元数车辆姿态
	// 获取到当前姿态的四元数并保存到attitude_vehicle_quat
    Quaternion attitude_vehicle_quat;
    _ahrs.get_quat_body_to_ned(attitude_vehicle_quat);

	// 计算姿态误差
	// _attitude_target_quat：输入参数，在n系下期望姿态的四元数，旋转方向tb->n
	// attitude_vehicle_quat：输入参数，在n系下当前姿态的四元数，旋转方向cb->n
	// attitude_error_vector：传入传出参数，姿态误差向量
	// _thrust_error_angle：传入传出参数，期望z轴和当前z轴的误差（详见轴角分离），即推力方向上的偏差角
	// 该函数通过前两个输入的四元数参数计算得到各轴的姿态偏差
	// 经过此函数后得到的参数均在b系下
    Vector3f attitude_error_vector;
    thrust_heading_rotation_angles(_attitude_target_quat, attitude_vehicle_quat, attitude_error_vector, _thrust_error_angle);

	// P控制器，输入为姿态误差，输出为期望角速率
	// 函数内部根据是否开启了sqrt_controller来区分计算各通道的期望角速率_rate_target_ang_vel
	// 是，则使用开方调整过的P控制器
	// 否，则使用常规的Kp*error
    _rate_target_ang_vel = update_ang_vel_target_from_att_error(attitude_error_vector);

	// 对P控制器输出叠加前馈项，尝试确保侧倾和俯仰误差随车身框架而不是参考框架旋转
	// x轴的前馈项 = y轴的姿态误差（限幅后的）*陀螺仪获取的z轴角速度
	// y轴的前馈项 = x轴的姿态误差（限幅后的）*陀螺仪获取的z轴角速度
	// 注意此处并没有对z轴的期望角速率进行处理
    // todo: this should probably be a matrix that couples yaw as well.
    _rate_target_ang_vel.x += constrain_float(attitude_error_vector.y, -M_PI / 4, M_PI / 4) * _ahrs.get_gyro().z;
    _rate_target_ang_vel.y += -constrain_float(attitude_error_vector.x, -M_PI / 4, M_PI / 4) * _ahrs.get_gyro().z;

	// 期望角速率限幅
    ang_vel_limit(_rate_target_ang_vel, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max