PX4的姿态控制中，并没有提到姿态的期望值是如何计算得出的，在前面的位置控制的，我们已经从期望推力得到了期望姿态，但是却没有提到姿态模式下如何获取期望姿态，下面我们就来分析一下1.5.4版本的PX4位置控制最后一段——手动模式下姿态的获取。
1.期望航向的获取
首先，要判断是否是手动控制模式，手动控制模式包括手动（角速度控制），姿态，定高，定点等模式，凡是期望值获取来自遥控器的都是手动控制模式，然后还需要允许姿态控制，基本只有手动模式（角速度控制）不允许姿态控制。
接着判断是否需要重置航向yaw，只有手动模式切入到姿态模式或者其他模式需要重置航向。如果无人机在地上，或者无人机处于不定高模式同时油门低于0.1（0-1）时，不允许控制航向。如果允许控制航向，首先计算航向速率允许的最大值，比较手动模式下最大角速度和无人机最大角速度，选更小的值，这两个值默认是200度每秒。然后计算最大的角度偏差，它是用最大角速度除以航向的比例系数逆推出，如果都使用默认值可以得到最大角度偏差为71.4度。然后用杆量（范围是-1到1）乘上最大角速度，航向的期望值就是当前期望值加上刚才的杆量值，偏差就是期望值减去当前航向，如果偏差大于最大允许的角度偏差（刚才的71.4），就把期望值放到期望姿态中去，直到偏差在合理范围内再放进去。
/* generate attitude setpoint from manual controls */
if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) {
/* reset yaw setpoint to current position if needed */
if (reset_yaw_sp) {
reset_yaw_sp = false;
_att_sp.yaw_body = _yaw;
}
/* do not move yaw while sitting on the ground */
else if (!_vehicle_land_detected.landed &&
!(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.1f)) {
/* we want to know the real constraint, and global overrides manual */
const float yaw_rate_max = (_params.man_yaw_max < _params.global_yaw_max) ? _params.man_yaw_max :
_params.global_yaw_max;
const float yaw_offset_max = yaw_rate_max / _params.mc_att_yaw_p;
_att_sp.yaw_sp_move_rate = _manual.r * yaw_rate_max;
float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
float yaw_offs = _wrap_pi(yaw_target - _yaw);
// If the yaw offset became too big for the system to track stop
// shifting it, only allow if it would make the offset smaller again.
if (fabsf(yaw_offs) < yaw_offset_max ||
(_att_sp.yaw_sp_move_rate > 0 && yaw_offs < 0) ||
(_att_sp.yaw_sp_move_rate < 0 && yaw_offs > 0)) {
_att_sp.yaw_body = yaw_target;
}
}2.期望推力、俯仰、横滚的获取
首先计算手动控制下的期望推力，判断无人机是否允许定高控制，如果不是，则通过油门曲线函数throttle_curve获取对应的推力值，然后与最大推力比较，取较小值。如果无人机在飞行中，则需要同时限制推力的最小值。
然后求期望俯仰和横滚，当无人机不处于速度控制时，期望横滚等于横滚杆量（-1到1）乘与最大允许横滚（默认35度），期望俯仰也是同样的。得到期望横滚和俯仰还不够，我们还需要获取期望姿态对应的四元数或者旋转矩阵。
首先判断_params.opt_recover这个参数，该参数用于修复俯仰较弱的VTOL的俯仰与横滚，默认为0。然后计算航向误差，计算期望横滚俯仰下的机体坐标系Z轴，然后把该Z轴逆旋转航向回到的位置。我们知道，PX4的旋转是先转航向在转俯仰，最后是横滚，首先求期望俯仰横滚的Z轴，这时候是没有进行航向旋转的，然后反向旋转航向，得到的就是当前航向下的期望横滚和俯仰。最简单是解释就是，先保证俯仰横滚，航向能不能保证就看天意了。如果航向能保证，那么期望姿态就是正确的，如果航向不能保证，那么起码飞控能保证横滚俯仰的正确性，这个是由于VTOL的特性决定的。
然后不管横滚、俯仰有没有被矫正，直接把期望的航向，俯仰，横滚组合求出期望姿态的四元数，然后放到期望姿态中去。
/* control throttle directly if no climb rate controller is active */
if (!_control_mode.flag_control_climb_rate_enabled) {
float thr_val = throttle_curve(_manual.z, _params.thr_hover);
_att_sp.thrust = math::min(thr_val, _manual_thr_max.get());
/* enforce minimum throttle if not landed */
if (!_vehicle_land_detected.landed) {
_att_sp.thrust = math::max(_att_sp.thrust, _manual_thr_min.get());
}
}
/* control roll and pitch directly if no aiding velocity controller is active */
if (!_control_mode.flag_control_velocity_enabled) {
_att_sp.roll_body = _manual.y * _params.man_roll_max;
_att_sp.pitch_body = -_manual.x * _params.man_pitch_max;
/* only if optimal recovery is not used, modify roll/pitch */
if (_params.opt_recover <= 0) {
// construct attitude setpoint rotation matrix. modify the setpoints for roll
// and pitch such that they reflect the user's intention even if a yaw error
// (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix
// from the pure euler angle setpoints will lead to unexpected attitude behaviour from
// the user's view as the euler angle sequence uses the yaw setpoint and not the current
// heading of the vehicle.
// calculate our current yaw error
float yaw_error = _wrap_pi(_att_sp.yaw_body - _yaw);
// compute the vector obtained by rotating a z unit vector by the rotation
// given by the roll and pitch commands of the user
math::Vector<3> zB = {0, 0, 1};
math::Matrix<3, 3> R_sp_roll_pitch;
R_sp_roll_pitch.from_euler(_att_sp.roll_body, _att_sp.pitch_body, 0);
math::Vector<3> z_roll_pitch_sp = R_sp_roll_pitch * zB;
// transform the vector into a new frame which is rotated around the z axis
// by the current yaw error. this vector defines the desired tilt when we look
// into the direction of the desired heading
math::Matrix<3, 3> R_yaw_correction;
R_yaw_correction.from_euler(0.0f, 0.0f, -yaw_error);
z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp;
// use the formula z_roll_pitch_sp = R_tilt * [0;0;1]
// to calculate the new desired roll and pitch angles
// R_tilt can be written as a function of the new desired roll and pitch
// angles. we get three equations and have to solve for 2 unknowns
_att_sp.pitch_body = asinf(z_roll_pitch_sp(0));
_att_sp.roll_body = -atan2f(z_roll_pitch_sp(1), z_roll_pitch_sp(2));
}
/* copy quaternion setpoint to attitude setpoint topic */
matrix::Quatf q_sp = matrix::Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body);
memcpy(&_att_sp.q_d[0], q_sp.data(), sizeof(_att_sp.q_d));
_att_sp.q_d_valid = true;
}3.发布期望值
首先根据脚架开关判断是否需要收起落架或者放起落架。
然后如果不允许手动控制或者高度控制，基本就可以锁定是手动模式了，就把reset_yaw_sp置0，下次进入可以重置航向。
然后如果是_control_mode.flag_control_offboard_enabled模式并且不允许位置、速度、加速度控制，就不发布期望姿态，否则就开始发布期望姿态。
最后，计算标志位reset_int_z_manual。
if (_manual.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON &&
!_vehicle_land_detected.landed) {
_att_sp.landing_gear = 1.0f;
} else if (_manual.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
_att_sp.landing_gear = -1.0f;
}
_att_sp.timestamp = hrt_absolute_time();
} else {
reset_yaw_sp = true;
_att_sp.yaw_sp_move_rate = 0.0f;
}
/* update previous velocity for velocity controller D part */
_vel_prev = _vel;
/* publish attitude setpoint
* Do not publish if offboard is enabled but position/velocity/accel control is disabled,
* in this case the attitude setpoint is published by the mavlink app. Also do not publish
* if the vehicle is a VTOL and it's just doing a transition (the VTOL attitude control module will generate
* attitude setpoints for the transition).
*/
if (!(_control_mode.flag_control_offboard_enabled &&
!(_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_acceleration_enabled))) {
if (_att_sp_pub != nullptr) {
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
} else if (_attitude_setpoint_id) {
_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
}
}
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled
&& !_control_mode.flag_control_climb_rate_enabled;
}