PX4姿态控制基础

1. 无人机姿态控制代码实现的四个步骤

订阅数据 （飞机现姿态，飞机期望姿态）
PID 外环计算（作用于姿态误差，角速度）
作用：产生期望角速度，并发布出去
PID 内环计算 （角加速度，减少噪声）
发布数据 （混控器矩阵，控制电机转速）

2. 解耦合控制

受制于传统多旋翼无人机结构限制，电机对pitch和roll的控制力矩较大，yaw的控制力矩较小，所以需要解耦控制。
（对其Z轴旋转后再进行yaw旋转）

orb_subscribe: 订阅外部数据

parameters_update : 对默认参数进行更新

px4_poll :阻塞等待px4_poll()配置阻塞时间100ms（uORB模型的函数API）。然后是打开MAVLINK协议，记录数据。如果poll失败，直接使用关键词continue从头开始运行（注意while和continue的组合使用）。其中的usleep(10000)函数属于线程级睡眠函数，使当前线程挂起。

四旋翼（姿态、位置）控制算法：

姿态估计 Attitude_estimator_q
位置估计 position_estimator_inav
姿态控制 mc_att_control
位置控制 mc_pos_control