今天我们来看下新版PX4（1.8.2版本,2018年11月21日）的姿态控制方法。首先下载该版本的代码。
官方下载地址：https://github.com/PX4/Firmware/releases
百度网盘：https://pan.baidu.com/s/18NiROf7Gsa7Zsbj7VoonVA
解压文件后，定位到Firmware-master\src\modules\mc_att_control\mc_att_control_main.cpp
函数void MulticopterAttitudeControl::control_attitude()就是新版PX4的姿态控制函数。
vehicle_attitude_setpoint_poll();
_thrust_sp = _v_att_sp.thrust;首先获取期望姿态和期望推力。
/* prepare yaw weight from the ratio between roll/pitch and yaw gains */
Vector3f attitude_gain = _attitude_p;
const float roll_pitch_gain = (attitude_gain(0) + attitude_gain(1)) / 2.f;
const float yaw_w = math::constrain(attitude_gain(2) / roll_pitch_gain, 0.f, 1.f);
attitude_gain(2) = roll_pitch_gain;然后获取姿态控制的P值。求roll和pitch的P值的平均值roll_pitch_gain，使用yaw的P值除以roll_pitch_gain得到航向控制的比重 yaw_w，最后把roll_pitch_gain赋值给航向的P（没理解这一点，估计是要削弱航向期望值，但是又不想虚弱航向角速度的期望值，所以用yaw_w算出一个航向变化更小的期望姿态，在算期望角速度的时候乘上roll_pitch_gain得到未削弱前的角速度。）
/* get estimated and desired vehicle attitude */
Quatf q(_v_att.q);
Quatf qd(_v_att_sp.q_d);
/* ensure input quaternions are exactly normalized because acosf(1.00001) == NaN */
q.normalize();
qd.normalize();然后求出期望姿态和实际姿态的四元数，并且把四元数进行归一化处理。
/* calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch */
Vector3f e_z = q.dcm_z();
Vector3f e_z_d = qd.dcm_z();
Quatf qd_red(e_z, e_z_d);求期望姿态和实际姿态的Z轴，构建一个从实际姿态Z轴旋转到期望姿态Z轴的四元数qd_red（大地坐标系）。
if (abs(qd_red(1)) > (1.f - 1e-5f) || abs(qd_red(2)) > (1.f - 1e-5f)) {
/* In the infinitesimal corner case where the vehicle and thrust have the completely opposite direction,
* full attitude control anyways generates no yaw input and directly takes the combination of
* roll and pitch leading to the correct desired yaw. Ignoring this case would still be totally safe and stable. */
qd_red = qd;
} else {
/* transform rotation from current to desired thrust vector into a world frame reduced desired attitude */
qd_red *= q;
}然后判断qd_red(1)和qd_red(2)。我们知道，qd_red在构建的时候已经进行了归一化（在类构建的时候），所以实际就是qd_red(1)=1或者qd_red(2)=1的情况。对应的四元数为（0,1,0,0）和（0,0,1,0)。根据标准的四元数的构建，四元数与旋转角度以及旋转轴有关。设旋转角度为α，旋转轴为单位向量（x,y,z）。则可以创建标准四元数q
q0=cos(α/2)
q1=xsin(α/2)
q2=ysin(α/2)
q3=zsin(α/2)
当cos(α/2)为0时，可知α为±180°，故此时旋转轴应为（1,0,0）,或者（0,1,0）,可知，当旋转轴为x轴或者y轴时，航向角是不会发生转动的，故此时可以直接把期望姿态作为过渡姿态使用。所以把期望姿态qd直接赋值给过渡姿态qd_red。
如果是其他正常情况，直接qd_red=qd_red*q即可得到过渡姿态qd_red。因为两个四元数相乘就是旋转的叠加，即先旋转q，然后再旋转qd_red（一定要注意顺序，四元数乘法的位置是不能互换的）。至此，我们已经得到了过渡姿态的四元数qd_red（大地坐标系）。
/* mix full and reduced desired attitude */
Quatf q_mix = qd_red.inversed() * qd;
q_mix *= math::signNoZero(q_mix(0));
/* catch numerical problems with the domain of acosf and asinf */
q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f);
q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f);
qd = qd_red * Quatf(cosf(yaw_w * acosf(q_mix(0))), 0, 0, sinf(yaw_w *asinf(q_mix(3))));然后计算期望姿态qd在过渡姿态中的映射q_mix，即期望姿态在过渡姿态坐标系中的四元数，这种方法有点类似于转置旋转矩阵乘某个向量可以得到该向量在机体坐标系的坐标。然后判断q_mix(0)的正负号，然后用q_mix直接乘该符号（0的时候返回1）。这一步是为了将四元数的旋转都变为-180°到正180°这个区间。我们知道，正常的旋转角是0到360。已知某旋转轴为V，那么绕其正方向旋转270°与绕-V正方向旋转90°的结果是一样的。然后对q_mix(0)和q_mix(3)进行限幅，因为等下要求反正弦和反余弦。
然后用公式Quatf(cosf(yaw_w * acosf(q_mix(0))), 0, 0, sinf(yaw_w * asinf(q_mix(3))));得出一个航向角乘了航向比重yaw_w的期望姿态（位于过渡姿态坐标系），acosf(q_mix(0)等于航向角的半角，乘yaw_w在求cosf,就是把航向角缩小而已。为什么q_mix(1)和q_mix(2)是0呢？因为过渡姿态与期望姿态的Z轴是重合的，所以在过渡姿态坐标系中，期望姿态与过渡姿态的旋转轴必然与Z轴平行。即旋转轴的x,y都是0。用qd_red乘上期望姿态位于过渡姿态坐标系的四元数即把期望姿态转换到大地坐标系中。至此我们需要用来作控制的期望姿态的四元数qd（大地坐标系）我们已经求出。
/* quaternion attitude control law, qe is rotation from q to qd */
Quatf qe = q.inversed() * qd;
/* using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle)
* also taking care of the antipodal unit quaternion ambiguity */
Vector3f eq = 2.f * math::signNoZero(qe(0)) * qe.imag();
/* calculate angular rates setpoint */
_rates_sp = eq.emult(attitude_gain);
/* Feed forward the yaw setpoint rate.
* yaw_sp_move_rate is the feed forward commanded rotation around the world z-axis,
* but we need to apply it in the body frame (because _rates_sp is expressed in the body frame).
* Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed)
* and multiply it by the yaw setpoint rate (yaw_sp_move_rate).
* This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame
* such that it can be added to the rates setpoint.
*/
_rates_sp += q.inversed().dcm_z() * _v_att_sp.yaw_sp_move_rate;然后q.inversed()*qd得到期望姿态在实际姿态坐标系的四元数qe，qe就是实际姿态到期望姿态的旋转。然后求qe在实际姿态坐标系下的三轴旋转角eq。eq的x,y,z对应x,y,z轴的旋转角。首先qe.imag()是qe的虚部。对应（xsin(α/2),ysin(α/2),zsin(α/2)）,我们知道，在旋转角度α比较小的情况下，α/2≈sin(α/2)。如下图
即qe.imag()=（xα/2,yα/2,zα/2），然后乘以2就可以得到（xα,yα,zα），这个向量的意思是把旋转角α分解到机体坐标系的三个轴上，因为旋转是可以分解的，它也遵循正交分解。最后乘上qe(0)的正负号原因也是一样的。绕某一旋转轴正方向旋转270°与绕其反向旋转轴正方向旋转90°结果是一样的。最后乘上姿态控制比例系数attitude_gain即可得到期望角速度（机体坐标系）。
/* limit rates */
for (int i = 0; i < 3; i++) {
if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
!_v_control_mode.flag_control_manual_enabled) {
_rates_sp(i) = math::constrain(_rates_sp(i), -_auto_rate_max(i), _auto_rate_max(i));
} else {
_rates_sp(i) = math::constrain(_rates_sp(i), -_mc_rate_max(i), _mc_rate_max(i));
}
}
}在程序的最后，根据是自动控制还是手动控制对期望角速度进行限幅。得到期望角速度（机体坐标系）之后，只要简单的与陀螺仪的角速度进行对比，做一个简单的PID控制，就可以得到无人机x,y,z轴的控制输出。