经过control_manual(dt)或者control_auto(dt)的处理，我们已经得到了实际控制用的期望位置，而我们期望输出的是期望姿态，下面我们就继续分析Mc_pos_control里的task_main()如何将期望位置转化为期望姿态。
转化过程遵循：期望位置→期望速度→期望加速度（用于限制速度）→期望速度→期望推力→期望姿态
首先程序已经调用了control_manual(dt)或者control_auto(dt)，函数运行完成后会返回1487行继续运行。通过判断是否手动控制以及是否关闭航向控制的标志决定姿态控制是否控制航向。一般只有在VTOL的起飞降落阶段（即垂起的多轴阶段）需要关闭航向控制，如果VTOL的航向轴的动力强的话，也可以不关闭航向控制。
/* weather-vane mode for vtol: disable yaw control */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.disable_mc_yaw_control == true) {
_att_sp.disable_mc_yaw_control = true;
} else {
/* reset in case of setpoint updates */
_att_sp.disable_mc_yaw_control = false;
}
1.IDLE和手动GPS降落的处理
如果，飞控处于IDLE模式，清空期望姿态，期望俯仰0°，期望横滚0°，期望推力0，期望航向为当前航向，然后发布期望姿态。如果无人机当前位于手控并且已经降落，处理方式和IDLE模式差不多。
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
/* idle state, don't run controller and set zero thrust */
R.identity();
matrix::Quatf qd = R;
memcpy(&_att_sp.q_d[0], qd.data(), sizeof(_att_sp.q_d));
_att_sp.q_d_valid = true;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _yaw;
_att_sp.thrust = 0.0f;
_att_sp.timestamp = hrt_absolute_time();
/* publish attitude setpoint */
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);
}
} else if (_control_mode.flag_control_manual_enabled
&& _vehicle_land_detected.landed) {
/* don't run controller when landed */
_reset_pos_sp = true;
_reset_alt_sp = true;
_do_reset_alt_pos_flag = true;
_mode_auto = false;
reset_int_z = true;
reset_int_xy = true;
R.identity();
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _yaw;
_att_sp.thrust = 0.0f;
_att_sp.timestamp = hrt_absolute_time();
/* publish attitude setpoint */
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);
}
2.FOLLOW_TARGET模式下的速度处理
如果不是1中的两种情况，那么就是正常计算期望速度，期望速度等于位置差乘上位置控制的P。注意_run_pos_control这个变量，这个变量在control_manual里面会进行赋值，决定是采用遥杆直接给期望速度还是计算出来。
然后计算速度的有效值，显然，正常的航线任务速度是无效的，因为航线任务的速度是位置计算出来的，不是直接由任务赋值的，velocity_valid只用于FOLLOW_TARGET任务。如果FOLLOW_TARGET任务有期望位置，那么就混合期望位置计算出的期望速度和任务里的期望速度，并且与望位置计算出的期望速度比较选择绝对值较大的速度作为真正的期望速度。如果FOLLOW_TARGET任务没有期望位置，那么就把任务里的速度赋值给期望速度。
} else {
/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
if (_run_pos_control) {
_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
}
// guard against any bad velocity values
bool velocity_valid = PX4_ISFINITE(_pos_sp_triplet.current.vx) &&
PX4_ISFINITE(_pos_sp_triplet.current.vy) &&
_pos_sp_triplet.current.velocity_valid;
// do not go slower than the follow target velocity when position tracking is active (set to valid)
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
velocity_valid &&
_pos_sp_triplet.current.position_valid) {
math::Vector<3> ft_vel(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy, 0);
float cos_ratio = (ft_vel * _vel_sp) / (ft_vel.length() * _vel_sp.length());
// only override velocity set points when uav is traveling in same direction as target and vector component
// is greater than calculated position set point velocity component
if (cos_ratio > 0) {
ft_vel *= (cos_ratio);
// min speed a little faster than target vel
ft_vel += ft_vel.normalized() * 1.5f;
} else {
ft_vel.zero();
}
_vel_sp(0) = fabs(ft_vel(0)) > fabs(_vel_sp(0)) ? ft_vel(0) : _vel_sp(0);
_vel_sp(1) = fabs(ft_vel(1)) > fabs(_vel_sp(1)) ? ft_vel(1) : _vel_sp(1);
// track target using velocity only
} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
velocity_valid) {
_vel_sp(0) = _pos_sp_triplet.current.vx;
_vel_sp(1) = _pos_sp_triplet.current.vy;
}3.速度的限幅
如果进行高度控制，由高度位置差计算垂向期望速度。然后对平飞速度进行限幅，默认最大平飞速度是8m/s。然后对垂向速度进行限幅，最大上升速度默认是1.5m/s，最大下降速度默认是0.5m/s。如果不允许水平、垂向的位置控制，将相关标志位置一，如果不允许速度控制，将实际速度作为上一期望速度。清零水平期望速度。如果不允许垂向速度控制，将垂向期望速度清零。
if (_run_alt_control) {
_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
}
/* make sure velocity setpoint is saturated in xy*/
float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) +
_vel_sp(1) * _vel_sp(1));
if (vel_norm_xy > _params.vel_max(0)) {
/* note assumes vel_max(0) == vel_max(1) */
_vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy;
_vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy;
}
/* make sure velocity setpoint is saturated in z*/
if (_vel_sp(2) < -1.0f * _params.vel_max_up) {
_vel_sp(2) = -1.0f * _params.vel_max_up;
}
if (_vel_sp(2) > _params.vel_max_down) {
_vel_sp(2) = _params.vel_max_down;
}
if (!_control_mode.flag_control_position_enabled) {
_reset_pos_sp = true;
}
if (!_control_mode.flag_control_altitude_enabled) {
_reset_alt_sp = true;
}
if (!_control_mode.flag_control_velocity_enabled) {
_vel_sp_prev(0) = _vel(0);
_vel_sp_prev(1) = _vel(1);
_vel_sp(0) = 0.0f;
_vel_sp(1) = 0.0f;
control_vel_enabled_prev = false;
}
if (!_control_mode.flag_control_climb_rate_enabled) {
_vel_sp(2) = 0.0f;
}
3.起飞或者降落的速度控制
对于降落的速度控制比较简单，如果不是手动模式，任务当前期望点有效，当前任务类型为降落，直接把期望的Z轴速度覆盖为0.5m/s。
如果是起飞的话，首先判断_takeoff_jumped，该标志位为true时即代表飞机已经飞起来了。在一个正常的任务流程中，正常飞机都是未解锁的，此时_takeoff_jumped为false，然后需要自主起飞时，如果无人机Z轴速度小于1.5/2=0.75m/s时，推力_takeoff_thrust_sp会自动累加，一秒就能加到0.5，然后清零期望速度。这个起飞期望推力，在更后面的代码中会把期望速度计算的推力给覆盖掉，即当无人机刚起飞，但速度小于0.75m/s时，我们只控制推力，且推力是累加的。
当飞机Z轴速度已经大于0.75m/s了，就开始进入速度控制环节，计算当前推力与实际速度计算的推力的差，并赋值到thrust_int中，该步是为了平滑推力变化，如当速度达到0.75m/s时，实际有一个推力。如果不平滑推力，在进入速度控制后，期望速度是1.5m/s，而实际速度在0.75m/s左右，固定就会有一个PID计算的推力，这个推力与刚才进行推力控制的推力相差很大，那么在切换的状态中，飞机的推力变化就会很大。然后再把期望Z轴速度直接覆盖掉，赋值相关标志位。
如果不是起飞状态，那么就清零_takeoff_jumped和_takeoff_thrust_sp。
/* use constant descend rate when landing, ignore altitude setpoint */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
_vel_sp(2) = _params.land_speed;
}
/* special thrust setpoint generation for takeoff from ground */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
&& _control_mode.flag_armed) {
// check if we are not already in air.
// if yes then we don't need a jumped takeoff anymore
if (!_takeoff_jumped && !_vehicle_land_detected.landed && fabsf(_takeoff_thrust_sp) < FLT_EPSILON) {
_takeoff_jumped = true;
}
if (!_takeoff_jumped) {
// ramp thrust setpoint up
if (_vel(2) > -(_params.tko_speed / 2.0f)) {
_takeoff_thrust_sp += 0.5f * dt;
_vel_sp.zero();
_vel_prev.zero();
} else {
// copter has reached our takeoff speed. split the thrust setpoint up
// into an integral part and into a P part
thrust_int(2) = _takeoff_thrust_sp - _params.vel_p(2) * fabsf(_vel(2));
thrust_int(2) = -math::constrain(thrust_int(2), _params.thr_min, _params.thr_max);
_vel_sp_prev(2) = -_params.tko_speed;
_takeoff_jumped = true;
reset_int_z = false;
}
}
if (_takeoff_jumped) {
_vel_sp(2) = -_params.tko_speed;
}
} else {
_takeoff_jumped = false;
_takeoff_thrust_sp = 0.0f;
}4.对速度的加速度限制，发布速度期望值
然后开始计算加速度，加速度是当前期望速度与上一期望速度的微分，如果水平加速度超过5m/s²，就限制当前的速度期望值。如果垂向加速度超过了10m/s²，那么就限制垂向期望速度，然后开始发布期望速度的值。
// limit total horizontal acceleration
math::Vector<2> acc_hor;
acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt;
acc_hor(1) = (_vel_sp(1) - _vel_sp_prev(1)) / dt;
if ((acc_hor.length() > _params.acc_hor_max) & !_reset_pos_sp) {
acc_hor.normalize();
acc_hor *= _params.acc_hor_max;
math::Vector<2> vel_sp_hor_prev(_vel_sp_prev(0), _vel_sp_prev(1));
math::Vector<2> vel_sp_hor = acc_hor * dt + vel_sp_hor_prev;
_vel_sp(0) = vel_sp_hor(0);
_vel_sp(1) = vel_sp_hor(1);
}
// limit vertical acceleration
float acc_v = (_vel_sp(2) - _vel_sp_prev(2)) / dt;
if ((fabsf(acc_v) > 2 * _params.acc_hor_max) & !_reset_alt_sp) {
acc_v /= fabsf(acc_v);
_vel_sp(2) = acc_v * 2 * _params.acc_hor_max * dt + _vel_sp_prev(2);
}
_vel_sp_prev = _vel_sp;
_global_vel_sp.vx = _vel_sp(0);
_global_vel_sp.vy = _vel_sp(1);
_global_vel_sp.vz = _vel_sp(2);
/* publish velocity setpoint */
if (_global_vel_sp_pub != nullptr) {
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);
} else {
_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp);
}5.速度控制的积分重置
根据相关标志位判断，是否需要清零垂向速度控制的积分项，然后查看水平速度控制的积分项是否需要清零，如果需要也清零。
if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_acceleration_enabled) {
/* reset integrals if needed */
if (_control_mode.flag_control_climb_rate_enabled) {
if (reset_int_z) {
reset_int_z = false;
float i = _params.thr_min;
if (reset_int_z_manual) {
i = _params.thr_hover;
if (i < _params.thr_min) {
i = _params.thr_min;
} else if (i > _params.thr_max) {
i = _params.thr_max;
}
}
thrust_int(2) = -i;
}
} else {
reset_int_z = true;
}
if (_control_mode.flag_control_velocity_enabled) {
if (reset_int_xy) {
reset_int_xy = false;
thrust_int(0) = 0.0f;
thrust_int(1) = 0.0f;
}
} else {
reset_int_xy = true;
}
6.平滑推力变化
如果原来无人机除以非速度控制模式，那么切换的时候需要平滑期望速度。用无人机当期使用的推力_att_sp.thrust分解到大地坐标系的x,y,z上，然后减去积分、微分，反推出速度差出来。
/* velocity error */
math::Vector<3> vel_err = _vel_sp - _vel;
// check if we have switched from a non-velocity controlled mode into a velocity controlled mode
// if yes, then correct xy velocity setpoint such that the attitude setpoint is continuous
if (!control_vel_enabled_prev && _control_mode.flag_control_velocity_enabled) {
matrix::Dcmf Rb = matrix::Quatf(_att_sp.q_d[0], _att_sp.q_d[1], _att_sp.q_d[2], _att_sp.q_d[3]);
// choose velocity xyz setpoint such that the resulting thrust setpoint has the direction
// given by the last attitude setpoint
_vel_sp(0) = _vel(0) + (-Rb(0,
2) * _att_sp.thrust - thrust_int(0) - _vel_err_d(0) * _params.vel_d(0)) / _params.vel_p(0);
_vel_sp(1) = _vel(1) + (-Rb(1,
2) * _att_sp.thrust - thrust_int(1) - _vel_err_d(1) * _params.vel_d(1)) / _params.vel_p(1);
_vel_sp(2) = _vel(2) + (-Rb(2,
2) * _att_sp.thrust - thrust_int(2) - _vel_err_d(2) * _params.vel_d(2)) / _params.vel_p(2);
_vel_sp_prev(0) = _vel_sp(0);
_vel_sp_prev(1) = _vel_sp(1);
_vel_sp_prev(2) = _vel_sp(2);
control_vel_enabled_prev = true;
// compute updated velocity error
vel_err = _vel_sp - _vel;
}7.计算期望推力
然后开始计算期望推力，推力是计算速度的PID值累加得到的，如果是起飞任务且无人机Z轴速度还比较低的时候，直接使用推力控制，并清零XY的期望推力。然后计算推力的最小值。
/* thrust vector in NED frame */
math::Vector<3> thrust_sp;
if (_control_mode.flag_control_acceleration_enabled && _pos_sp_triplet.current.acceleration_valid) {
thrust_sp = math::Vector<3>(_pos_sp_triplet.current.a_x, _pos_sp_triplet.current.a_y, _pos_sp_triplet.current.a_z);
} else {
thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int;
}
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
&& !_takeoff_jumped && !_control_mode.flag_control_manual_enabled) {
// for jumped takeoffs use special thrust setpoint calculated above
thrust_sp.zero();
thrust_sp(2) = -_takeoff_thrust_sp;
}
if (!_control_mode.flag_control_velocity_enabled && !_control_mode.flag_control_acceleration_enabled) {
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
}
if (!_control_mode.flag_control_climb_rate_enabled && !_control_mode.flag_control_acceleration_enabled) {
thrust_sp(2) = 0.0f;
}
/* limit thrust vector and check for saturation */
bool saturation_xy = false;
bool saturation_z = false;
/* limit min lift */
float thr_min = _params.thr_min;
if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) {
/* don't allow downside thrust direction in manual attitude mode */
thr_min = 0.0f;
}
8.下降推力处理
首先对速度和加速度进行互补滤波，然后判断当前任务是否是降落任务，再进一步根据速度和加速度判断是否处于降落阶段，当无人机处于降落阶段且Z轴速度小于0.1m/s时，判断其已降落，直接把最大推力设为0，显然，这是一种很危险的行为，所以代码中也有退出机制，如果加速度大于4m/s²，或者速度大于1m/s，那么就重新把推力上限恢复到最大值。
然后，限制输出的最小推力。然后如果允许速度控制或者加速度控制，通过最大倾斜角度限制水平推力。PX4默认的最大倾斜角度为45°，当Z轴推力固定时，假设无人机需要倾斜到最大角度，那么水平推力的大小是被限制的。想要增大水平推力，可以增大Z轴推力,但是这样飞机Z轴控不住；或者增加倾斜角度。代码意思也很明确，优先保证Z轴推力输出。
float thrust_abs = thrust_sp.length();
float tilt_max = _params.tilt_max_air;
float thr_max = _params.thr_max;
/* filter vel_z over 1/8sec */
_vel_z_lp = _vel_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * _vel(2);
/* filter vel_z change over 1/8sec */
float vel_z_change = (_vel(2) - _vel_prev(2)) / dt;
_acc_z_lp = _acc_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * vel_z_change;
/* adjust limits for landing mode */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
/* limit max tilt and min lift when landing */
tilt_max = _params.tilt_max_land;
if (thr_min < 0.0f) {
thr_min = 0.0f;
}
/* descend stabilized, we're landing */
if (!_in_landing && !_lnd_reached_ground
&& (float)fabs(_acc_z_lp) < 0.1f
&& _vel_z_lp > 0.5f * _params.land_speed) {
_in_landing = true;
}
/* assume ground, cut thrust */
if (_in_landing
&& _vel_z_lp < 0.1f) {
thr_max = 0.0f;
_in_landing = false;
_lnd_reached_ground = true;
}
/* once we assumed to have reached the ground always cut the thrust.
Only free fall detection below can revoke this
*/
if (!_in_landing && _lnd_reached_ground) {
thr_max = 0.0f;
}
/* if we suddenly fall, reset landing logic and remove thrust limit */
if (_lnd_reached_ground
/* XXX: magic value, assuming free fall above 4m/s2 acceleration */
&& (_acc_z_lp > 4.0f
|| _vel_z_lp > 2.0f * _params.land_speed)) {
thr_max = _params.thr_max;
_in_landing = false;
_lnd_reached_ground = false;
}
} else {
_in_landing = false;
_lnd_reached_ground = false;
}
/* limit min lift */
if (-thrust_sp(2) < thr_min) {
thrust_sp(2) = -thr_min;
saturation_z = true;
}
if (_control_mode.flag_control_velocity_enabled || _control_mode.flag_control_acceleration_enabled) {
/* limit max tilt */
if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) {
/* absolute horizontal thrust */
float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
if (thrust_sp_xy_len > 0.01f) {
/* max horizontal thrust for given vertical thrust*/
float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);
if (thrust_sp_xy_len > thrust_xy_max) {
float k = thrust_xy_max / thrust_sp_xy_len;
thrust_sp(0) *= k;
thrust_sp(1) *= k;
saturation_xy = true;
}
}
}
}9.推力补偿和推力限制
如果允许Z轴速度控制但是不允许水平速度控制（即定高但是不控水平位置），需要进行推力补偿。R（2，2）是重力向量在大地坐标系Z轴上的映射，是机体坐标系Z轴与大地坐标系Z轴的夹角的余弦值。TILT_COS_MAX等于0.7，即大概对应45°角。放大系数可以按照公式进行计算。然后对推力的最大值进行抑制，默认最大推力是0.9，又可以分为三种情况
1.Z轴推力向上，且Z轴推力超过了最大推力，此时直接把水平推力置0，垂直推力限制为最大推力，有限保证Z轴。
2.Z轴推力向上，但是没有超过最大推力，此时，限制水平推力，但是不抑制垂直推力。
3.Z轴推力朝下，此时直接按比例限制三轴推力。
可知，PX4的想法是，当飞机飞行时，优先保证高度不会掉（即此时飞机需要往上飞），水平位置控制可以等下在控。
if (_control_mode.flag_control_climb_rate_enabled && !_control_mode.flag_control_velocity_enabled) {
/* thrust compensation when vertical velocity but not horizontal velocity is controlled */
float att_comp;
if (_R(2, 2) > TILT_COS_MAX) {
att_comp = 1.0f / _R(2, 2);
} else if (_R(2, 2) > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _R(2, 2) + 1.0f;
saturation_z = true;
} else {
att_comp = 1.0f;
saturation_z = true;
}
thrust_sp(2) *= att_comp;
}
/* limit max thrust */
thrust_abs = thrust_sp.length(); /* recalculate because it might have changed */
if (thrust_abs > thr_max) {
if (thrust_sp(2) < 0.0f) {
if (-thrust_sp(2) > thr_max) {
/* thrust Z component is too large, limit it */
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
thrust_sp(2) = -thr_max;
saturation_xy = true;
saturation_z = true;
} else {
/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
float thrust_xy_max = sqrtf(thr_max * thr_max - thrust_sp(2) * thrust_sp(2));
float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
float k = thrust_xy_max / thrust_xy_abs;
thrust_sp(0) *= k;
thrust_sp(1) *= k;
saturation_xy = true;
}
} else {
/* Z component is negative, going down, simply limit thrust vector */
float k = thr_max / thrust_abs;
thrust_sp *= k;
saturation_xy = true;
saturation_z = true;
}
thrust_abs = thr_max;
}10.推力转化为姿态
首先，计算推力积分，如果允许速度控制且xy平面的推力积分不饱和，就累加到推力积分中，如果允许垂直速度控制且Z轴积分不饱和，就计算Z轴积分。但是程序也限制了Z轴积分大于0的情况。这是因为在降落阶段，当飞机靠近地面的时候，由于地面效应的存在，电机提供的升力会提升，如果允许Z轴推力积分大于0，那么电机就会减小推力，然后无人机会在地面弹跳。
然后就开始把推力转化为姿态，但是有一个问题，那就是推力的方向是与实际推力的正方向是反的。比如，期望推力是（-1,0,0），对于我们的理解就是，推力是电机提供的，正常都是向上的，对于期望推力是（-1,0,0）对应的姿态的Z轴应该是（1,0,0）才对，因为我们的坐标系向下为正，所以可以看到，期望推力的方向与期望姿态的Z轴是刚好反向的，所以在计算期望姿态时，需要把推力反向。然后推力除以推力的模长是为了把期望姿态的Z轴的坐标归一化处理。然后求向量y_C，y_C可以理解为未进行横滚俯仰旋转时的Y轴，这又可以说到欧拉角的旋转了。PX4对于欧拉角的旋转是先绕Z轴旋转，然后是绕旋转后得到的Y轴Y'，最后绕旋转后得到的X轴X''。对于一个已确定的欧拉角来说，在旋转的第二步时，绕Y'旋转，这个时候是Y轴是不动的，此时Y'轴与Y轴的夹角就是航向角，没错，这个y_C代表的就是Y'。最后绕X‘’旋转时，X轴是不动，而且可以想象一下，在最后一个旋转中，Y',Y'',Z''都垂直于X''轴。所以先求出y_C，已知Y是（0,1,0），y_C与Y轴的夹角为航向角，画一下就可以得到y_C的公式了，然后由于y_C,Z''都垂直于X''轴，Z''其实就是推力，所以直接用y_C和推力叉乘就可以得到X‘’轴，然后在用X‘’轴和推力叉乘就可以得到Y''轴，至此，期望的机体坐标系我们就求出来了。需要区分的是，当Z''轴即推力在水平下方，在水平面上，在水平面上方需要对X''轴进行处理。当Z‘’轴在水平下方时，这是最正常的姿态，正常求出X‘’。当Z''轴在水平上方时，此时Z‘’轴的z坐标小于0（Z轴向下为正），这相当于飞机翻过来了，对于多轴来说，这种动作是做不出来的，但是对于直升机来说，是可以做出倒飞的动作的。对于这样一个推力，可以有两个对应的姿态，一个是机头朝后，相当于俯仰转了超过90°，另一种是机头超前，相当于横滚转了超过90°。对于操作飞手来说，机头超前更有利于控制。所以当Z‘’轴在水平上方时，求出的X''需要反向，即采用机头朝前的姿态去解释。最后就是当Z''轴在水平面时，此时，欧拉角会产生万向节死锁，无人机航向与横滚是重叠的，为了便于控制，就全部归为X''轴固定朝下，即X''轴为（0,0,1）。显然当Z''和X''确定之后，Y''轴就是唯一的，使用Z''和X''叉乘即可得出。然后把求出的机体坐标系放到矩阵R中，再从R中转化出四元数q_sp，放到期望姿态中去，最后从旋转矩阵R中求出俯仰和横滚，用于记录。
如果不允许手动控制，直接把期望姿态置0。然后计算期望推力在各个轴上上的期望加速度，然后记录期望姿态的时间。
/* update integrals */
if (_control_mode.flag_control_velocity_enabled && !saturation_xy) {
thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt;
thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt;
}
if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) {
thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;
/* protection against flipping on ground when landing */
if (thrust_int(2) > 0.0f) {
thrust_int(2) = 0.0f;
}
}
/* calculate attitude setpoint from thrust vector */
if (_control_mode.flag_control_velocity_enabled || _control_mode.flag_control_acceleration_enabled) {
/* desired body_z axis = -normalize(thrust_vector) */
math::Vector<3> body_x;
math::Vector<3> body_y;
math::Vector<3> body_z;
if (thrust_abs > SIGMA) {
body_z = -thrust_sp / thrust_abs;
} else {
/* no thrust, set Z axis to safe value */
body_z.zero();
body_z(2) = 1.0f;
}
/* vector of desired yaw direction in XY plane, rotated by PI/2 */
math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);
if (fabsf(body_z(2)) > SIGMA) {
/* desired body_x axis, orthogonal to body_z */
body_x = y_C % body_z;
/* keep nose to front while inverted upside down */
if (body_z(2) < 0.0f) {
body_x = -body_x;
}
body_x.normalize();
} else {
/* desired thrust is in XY plane, set X downside to construct correct matrix,
* but yaw component will not be used actually */
body_x.zero();
body_x(2) = 1.0f;
}
/* desired body_y axis */
body_y = body_z % body_x;
/* fill rotation matrix */
for (int i = 0; i < 3; i++) {
R(i, 0) = body_x(i);
R(i, 1) = body_y(i);
R(i, 2) = body_z(i);
}
/* copy quaternion setpoint to attitude setpoint topic */
matrix::Quatf q_sp = R;
memcpy(&_att_sp.q_d[0], q_sp.data(), sizeof(_att_sp.q_d));
_att_sp.q_d_valid = true;
/* calculate euler angles, for logging only, must not be used for control */
matrix::Eulerf euler = R;
_att_sp.roll_body = euler(0);
_att_sp.pitch_body = euler(1);
/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
} else if (!_control_mode.flag_control_manual_enabled) {
/* autonomous altitude control without position control (failsafe landing),
* force level attitude, don't change yaw */
R = matrix::Eulerf(0.0f, 0.0f, _att_sp.yaw_body);
/* copy quaternion setpoint to attitude setpoint topic */
matrix::Quatf q_sp = R;
memcpy(&_att_sp.q_d[0], q_sp.data(), sizeof(_att_sp.q_d));
_att_sp.q_d_valid = true;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
}
11.发布期望值
然后填充位置期望的信息，发布期望值，如果位置信息是关闭的，即位置，高度，速度，爬升速度，加速度控制都是关闭的，直接给重置标志赋值为true，当下次进入位置控制时，要刷新期望值和积分。
/* fill local position, velocity and thrust setpoint */
_local_pos_sp.timestamp = hrt_absolute_time();
_local_pos_sp.x = _pos_sp(0);
_local_pos_sp.y = _pos_sp(1);
_local_pos_sp.z = _pos_sp(2);
_local_pos_sp.yaw = _att_sp.yaw_body;
_local_pos_sp.vx = _vel_sp(0);
_local_pos_sp.vy = _vel_sp(1);
_local_pos_sp.vz = _vel_sp(2);
/* publish local position setpoint */
if (_local_pos_sp_pub != nullptr) {
orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);
} else {
_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
}
} else {
/* position controller disabled, reset setpoints */
_reset_alt_sp = true;
_reset_pos_sp = true;
_do_reset_alt_pos_flag = true;
_mode_auto = false;
reset_int_z = true;
reset_int_xy = true;
control_vel_enabled_prev = false;
/* store last velocity in case a mode switch to position control occurs */
_vel_sp_prev = _vel;
}