PX4飞控之位置控制（1）整体架构

位置控制是无人机飞控的核心算法之一，一方面根据commander中的flag标志位和Navigator中提供的航点信息进行控制（自主模式下），另一方面得到期望姿态角（setpoint）的四元数信息，给到姿态控制模块进行姿态控制。本文重点PX4飞控的位置控制的代码整体架构（mc_pos_control）,具体的控制算法将在后续文章中陆续奉上。
位置控制模块的主函数：task_main()
1.订阅结构体orb_subscribe（）。
2.参数更新parameters_update(true)
3.while (!_task_should_exit){}进入位置控制循环
（1）加锁解锁控制参数重置
if (_control_mode.flag_armed && !was_armed)则重置位置和高度sp，速度sp归零。
_control_mode.flag_armed=armed.armed;飞机启动后为真。while初始定义时bool was_armed = false;初始化为假，意味着在飞机启动时进行位置和速度sp的重置。
注：根据加锁和解锁的判断，保证重新解锁后时位置参数被重置。
（2）获取当前位置和速度信息，作为反馈信息，进行之后的PID控制

if (PX4_ISFINITE(_local_pos.x) &&
    PX4_ISFINITE(_local_pos.y) &&
    PX4_ISFINITE(_local_pos.z)) {
    _pos(0) = _local_pos.x;
    _pos(1) = _local_pos.y;
    if (_params.alt_mode == 1 &&_local_pos.dist_bottom_valid) {
        _pos(2) = -_local_pos.dist_bottom;
    } else {
        _pos(2) = _local_pos.z;
    }
}//更新当前位置坐标
if (PX4_ISFINITE(_local_pos.vx) &&
    PX4_ISFINITE(_local_pos.vy) &&
    PX4_ISFINITE(_local_pos.vz)) {
    _vel(0) = _local_pos.vx;
    _vel(1) = _local_pos.vy;
    if (_params.alt_mode == 1 && _local_pos.dist_bottom_valid) {
        _vel(2) = -_local_pos.dist_bottom_rate;
    } else {
        _vel(2) = _local_pos.vz;
    }
}

注：_params.alt_mode 是MPC_ALT_MODE，默认为0，代表高度跟随，1代表地形跟随。地形跟随模式下，垂直方向上的参考基准为地面。
（3）位置sp
根据commander中不同的标志位选择不同的控制方式。
control_manual(dt);//手动模式
control_offboard(dt);//外部模式
control_auto(dt);//自动模式
得到_pos_sp和yaw_sp.
在control_auto中
起飞时_do_reset_alt_pos_flag = false;其它情况为真。貌似只有在manual中用到。从auto切换到manual时。
进行起落架控制
4.速度SP
（1）非手动控制下，怠速模式下，不进行控制
姿态角的俯仰、横滚为0，航向为当前航向，油门为0.

_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _yaw;
_att_sp.thrust = 0.0f;

（2）手动控制，落地之后
对位置和高度sp、三个方向上的积分量进行重置，同时姿态角sp为0，油门sp为0，航向角为当前角度。

_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _yaw;
_att_sp.thrust = 0.0f;

注：落地之后，为防止手动打杆翻机，将姿态角和油门都归零，航向角保持不变。
（3）正常情况的水平位置控制：
①_run_pos_control为真时，（初始化为真，只有在手动模式中和offboard中会赋值为假）
水平位置P控制，得到水平速度sp

_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);

②目标跟随模式下
目标水平速度：ft_vel(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy, 0);
if (cos_ratio > 0) {

    ft_vel *= (cos_ratio);//目标速度在无人setpoint速度方向上投影
    ft_vel += ft_vel.normalized() * 1.5f;} 

实际上ft_vel =ft_vel * (cos_ratio)+ft_vel * (cos_ratio).normalized() * 1.5（比投影速度多一点）

_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);

二者取较大的速度
③当前目标位置无效，速度有效时，速度sp就等于航点设定速度

_vel_sp(0) = _pos_sp_triplet.current.vx;
_vel_sp(1) = _pos_sp_triplet.current.vy;

（4）正常情况的高度位置控制

_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);

（5）速度限幅（在control auto 中也有进行限幅的控制）
水平方向的和速度，不能大于水平方向上的最大速度，大于之后，对水平的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;
                }

垂直方向上的速度，在最大上升速度和最小下降速度之间。
（6）对于需要reset的情况，位置、高度、速度重置（在mission等自主模式中不会触发）

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;
}

（7）降落速度控制（见下一篇博客）
（8）起飞速度控制（见下一篇博客）
（9）加速度最大值对速度sp的限幅

水平：

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);
}

垂直：

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);
}

5.油门sp
（1）积分项
（2）加速度控制时，直接根据目标航点加速度得到油门值
非加速度控制，利用PID参数得到thrust sp。（对于mission模式，都是非加速度控制）

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;
}

（3）起飞、降落油门控制
（4）最大加速度限幅
水平：

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;
}

（5）重力补偿（垂直油门）

if (_control_mode.flag_control_climb_rate_enabled && !_control_mode.flag_control_velocity_enabled) {
    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;
}

（6）总油门限幅，加速度超过最大值thrust_abs > thr_max
垂直方向上的 推力向上，同时超过最大值 thr_max时（0.9），水平方向上为0，垂直方向上等于最大值。
垂直方向上的推力向上，没有超过最大值 thr_max时，优先保证垂直推力，得到水平上的最大推力合力，对水平进行限幅。
垂直方向上加速度向下时，则同时对三个方向上进行等比例限幅
（7）更新积分项
速度控制有效同时没有进行推力限幅时。
由速度误差得到积分值，水平和垂直两个方向。
6.姿态角sp与四元数
（1）body_z = -thrust_sp / thrust_abs;机体z轴与推力轴重合，
求机体坐标系三个轴单位向量在地面坐标系下的坐标
先求body_z= -thrust_sp / thrust_abs;
（2）机体坐标系y轴在水平面上的投影向量

y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);

由偏航角求得，
叉乘得到机体坐标系的x轴向量

body_x = y_C % body_z;

（3）机体x轴和机体z轴叉乘的到真正的机体y轴向量。

body_y = body_z % body_x;

（4）这三个向量组成旋转矩阵R=euler
_att_sp.roll_body = euler(0);_att_sp.pitch_body = euler(1);
得到四元数，给到姿态sp
7.手动控制下的姿态角sp

_att_sp.yaw_sp_move_rate = _manual.r * yaw_rate_max;
yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
_att_sp.roll_body = _manual.y * _params.man_roll_max;
_att_sp.pitch_body = -_manual.x * _params.man_pitch_max;//x为正时，摇杆往前推，俯仰角为负