1.16版本的PX4混控器改版了，Pipeline如下图： 其中用于计算各执行器分配量的代码位于src/lib/control_allocation/control_allocation。

控制分配器

控制分配器有两个版本，通过参数CA_METHOD控制。

伪逆法控制分配器

角度控制器

角度控制器主要代码位于src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp中，其核心为update函数。

角度控制采取了倾转分离的策略，倾斜（俯仰、偏航）的控制能力较强，而转向（偏航）的控制能力较弱，因此采用了不同的控制增益，将转向的控制量按比例缩小。

首先计算当前姿态和期望姿态分别对应的z轴方向（世界坐标系）：

// calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch
const Vector3f e_z = q.dcm_z();
const Vector3f e_z_d = qd.dcm_z();

多旋翼控制器结构

整体控制器结构如下

各模块分别位于：

• Position Control, Velocity Control, Acceleration&Yaw to Attitude: src\modules\mc_pos_control
• Angle Control: src\modules\mc_att_control
• Angular Rate Control: src\modules\mc_rate_control
• Mixer: src\modules\control_allocator