Pixhawk之姿态控制篇（1）_源码算法分析（超级有料）
一、开篇
姿态控制篇终于来了、来了、来了~~~
心情爽不爽？愉悦不愉悦？开心不开心？
喜欢的话就请我吃顿饭吧，哈哈。
其实这篇blog一周前就应该写的，可惜被上一篇blog霸占了。但是也不算晚，整理了很多算法基础知识，使得本篇blog更充实。一人之力总是有限的，难免有不足之处，大家见谅，有写的不好的地方劳烦指正。看到标题了吧，属于连载篇，所以后续还会有相关问题的补充的。
三、实验平台
Software Version：PX4Firmware
Hardware Version：pixhawk
IDE：eclipse Juno （Windows）
四、基础知识
1、写在前面
无人机控制部分主要分为两个部分，姿态控制部分和位置控制部分；位置控制可用远程遥控控制，而姿态控制一般由无人机系统自动完成。姿态控制是非常重要的，因为无人机的位置变化都是由姿态变化引起的。
下图阐释了PX4源码中的两个环路控制，分为姿态控制和位置控制。
补充：关于Pixhawk原生固件中姿态(估计/控制)和位置(估计/控制)源码的应用问题
PX4Fireware原生固件中的modules中姿态估计有多种：Attitude_estimator_ekf、Attitude_estimator_q、ekf_att_pos_estimator。
位置估计有：ekf_att_pos_estimator、local_position_estimator、position_estimator_inav
姿态控制有：fw_att_control、mc_att_control、mc_att_control_multiplatform、vtol_att_control
位置控制有：fw_pos_control_l1、fw_pos_control_l1、mc_pos_control_multiplatform
四旋翼用到以上哪些估计和控制算法呢？这部分在启动代码rc.mc_app里面有详细的说明。
默认的是：
姿态估计 Attitude_estimator_q
位置估计 position_estimator_inav
姿态控制 mc_att_control
位置控制 mc_pos_control
2、飞行控制（该部分属于理论概述）
飞行控制分为姿态控制和位置控制，该文章主讲姿态控制。
所谓姿态控制，主要就是在前期姿态解算的基础上对四旋翼飞行器进行有效的飞行控制，以达到所需要的控制效果。在这种情况下，算法要学会如何连续地做决策，并且算法的评价应该根据其所做选择的长期质量来进行。举一个具体的例子，想想无人机飞行所面临的难题：每不到一秒，算法都必须反复地选择最佳的行动控制。控制过程还是以经典的PID反馈控制器为主（在控制环路中可以添加smith预测器）。那么如何实现控制呢?
以四旋翼飞行器为例，主要就是通过改变旋翼的角速度来控制四旋翼无人机。每个旋翼产生一个推力（F1、F2、F3、F4）和一个力矩，其共同作用构成四旋翼无人机的主推力、偏航力矩、俯仰力矩和滚转力矩。在四旋翼无人机中，正对的一对旋翼旋转方向一致，另外一对与之相反，来抵消静态平稳飞行时的回转效应和气动力矩。升降以及RPY的实现不在赘述。控制对象就是四旋翼无人机，其动力学模型可以描述为：将其视为有一个力和三个力矩的三维刚体。如下给出了小角度变化条件下的四旋翼无人机的近似动力学模型：
PS：PX4的姿态控制部分使用的是roll-pitch和yaw分开控制的（是为了解耦控制行为），即tilt和torsion两个环节。感性认识一下，如下图：
根据经验所得，控制toll-pitch比控制yaw更容易实现。比如同样是实现10°的变化，roll-pitch需要60ms左右；但是yaw控制器却需要接近150ms。（上面两幅图是出自DJI某哥写的论文里面，仅作参考，结合理解Pixhawk）
控制流程：
1）、预处理：各参数的初始化。
2）、稳定roll-pitch的角速度。
3）、稳定roll-pitch的角度。
4）、稳定yaw的角速度。
5）、稳定yaw的角度。
其中在第五步中有一个yaw的前馈控制（MC_YAW_FF）：There is MC_YAW_FF parameter that controls how much of userinput need to feed forward to yaw rate controller. 0 means very slow control,controller will start to move yaw only when sees yaw position error, 1 meansvery fast responsive control, but with some overshot, controller will move yawimmediately, always keeping yaw error near zero。This parameter is not critical and can be tuned in flight, inworst case yaw responce will be sluggish or too fast. Play with FF parameter toget comfortable responce. Valid range is 0…1. Typical value is 0.8…0.9. (Foraerial video optimal value may be much smaller to get smooth responce.) Yawovershot should be not more than 2-5%。
摘自：https://pixhawk.org/users/multirotor_pid_tuning
3、 进入姿态控制源码的前期过程
首先感性认识一下姿态控制部分的框架，控制部分分为内外环控制，内环控制角速度、外环控制角度。控制过程是先根据目标姿态（target）和当前姿态（current）求出偏差角，然后通过角速度来修正这个偏差角，最终到达目标姿态。
和姿态解算算法的流程几乎类似，主要的代码流程首先就是按照C++语言的格式引用C语言的main函数，但是在该处变成了：
extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[])。
然后捏：跳转到所谓的main函数，该部分有个要注意的点，如下代码所示：即mc_att_control::g_control = new MulticopterAttitudeControl;//重点（934），new关键词应该不陌生吧，类似于C语言中的malloc，对变量进行内存分配的，即对姿态控制过程中使用到的变量赋初值。
int mc_att_control_main(int argc, char *argv[])
{
if (argc < 2) {
warnx("usage: mc_att_control {start|stop|status}");
return 1;
}
if (!strcmp(argv[1], "start")) {
if (mc_att_control::g_control != nullptr) {
warnx("already running");
return 1;
}
mc_att_control::g_control = new MulticopterAttitudeControl;//重点
if (mc_att_control::g_control == nullptr) {
warnx("alloc failed");
return 1;
}
if (OK != mc_att_control::g_control->start()) {//跳转
delete mc_att_control::g_control;
mc_att_control::g_control = nullptr;
warnx("start failed");
return 1;
}
return 0;
}
然后捏：start函数
Int MulticopterAttitudeControl::start()
{
ASSERT(_control_task == -1);
/* start the task */
_control_task = px4_task_spawn_cmd("mc_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1500,
(px4_main_t)&MulticopterAttitudeControl::task_main_trampoline,
nullptr);
if (_control_task < 0) {
warn("task start failed");
return -errno;
}
return OK;
}
其中上面有个封装了nuttx自带的生成task的任务创建函数（他把优先级什么的做了重新的define，这么做是便于代码阅读）：px4_task_spawn_cmd()，注意它的用法。其函数原型是：
px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry,
char *const argv[])第一个参数是namespace，第二个参数是选择调度策略，第三个是任务优先级，第四个是任务的栈空间大小，第五个是任务的入口函数，最后一个一般是null。
然后捏：
Void MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
{
mc_att_control::g_control->task_main();
}
再然后捏：终于到本体了。
Void MulticopterAttitudeControl::task_main(){}比较讨厌的就是为什么要封装那么多层，应该是水平不够，还没有理解此处的用意。下面就是重点了。
五、重点
1、姿态控制源码_订阅姿态控制的代码比姿态解算的代码少了不少，所以接下来分析应该会比较快。
首先还是需要通过IPC模型uORB进行订阅所需要的数据。需要注意的一个细节就是在该算法处理过程中的有效数据的用途问题，最后处理过的数据最后又被改进程自己订阅了，然后再处理，再订阅，一直处于循环状态，这就是所谓的PID反馈控制器吧，最终达到所需求的控制效果，达到控制效果以后就把一系列的控制量置0（类似于idle），该任务一直在运行，随启动脚本启动的。
/* * do subscriptions */
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));
上面这些订阅到底订阅了哪些东西呢，顾名思义，根据ORB()中的参数的名称就是知道订阅的到底用于做什么的了。这套开源代码中最优越的地方时变量的命名很好，通俗易懂。
2、 参数初始化
紧随上面的代码就是参数数据的获取，parameters主要就是我们前期定义的感兴趣的数据，在姿态控制中的这些数据都是私有数据（private），比如roll、pitch、yaw以及与它们对应的PID参数。注意区分_params_handles和_params这两种数据结构（struct类型）。
/* initialize parameters cache */
parameters_update();
函数原型欣赏：
int MulticopterAttitudeControl::parameters_update()
{
float v;
/* roll gains */
param_get(_params_handles.roll_p, &v);
_params.att_p(0) = v;
param_get(_params_handles.roll_rate_p, &v);
_params.rate_p(0) = v;
param_get(_params_handles.roll_rate_i, &v);
_params.rate_i(0) = v;
param_get(_params_handles.roll_rate_d, &v);
_params.rate_d(0) = v;
param_get(_params_handles.roll_rate_ff, &v);
_params.rate_ff(0) = v;
/* pitch gains */
省略
/* yaw gains */
省略
/* angular rate limits */
param_get(_params_handles.roll_rate_max, &_params.roll_rate_max);
_params.mc_rate_max(0) = math::radians(_params.roll_rate_max);
param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max);
_params.mc_rate_max(1) = math::radians(_params.pitch_rate_max);
param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
_params.mc_rate_max(2) = math::radians(_params.yaw_rate_max);
/* manual rate control scale and auto mode roll/pitch rate limits */
param_get(_params_handles.acro_roll_max, &v);
_params.acro_rate_max(0) = math::radians(v);
param_get(_params_handles.acro_pitch_max, &v);
_params.acro_rate_max(1) = math::radians(v);
param_get(_params_handles.acro_yaw_max, &v);
_params.acro_rate_max(2) = math::radians(v);
/* stick deflection needed in rattitude mode to control rates not angles */
param_get(_params_handles.rattitude_thres, &_params.rattitude_thres);
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
return OK;
}
重点分析一下上述代码：其中param_get()函数比较重要，特别是内部使用的lock和unlock的使用（主要就是通过sem信号量控制对某一数据的互斥访问）。
Int param_get(param_t param, void *val)
{
int result = -1;
param_lock();
const void *v = param_get_value_ptr(param);
if (val != NULL) {
memcpy(val, v, param_size(param));
result = 0;
}
param_unlock();
return result;
}
上述使用的*lock和*unlock通过sem实现互斥访问（进临界区），源码如下。
/** lock the parameter store */
static void param_lock(void)
{
//do {} while (px4_sem_wait(¶m_sem) != 0);
}
/** unlock the parameter store */
static void param_unlock(void)
{
//px4_sem_post(¶m_sem);
}
上面是开源代码中的，代码里面把lock和unlock函数都写成空函数了，那还有屁用啊。应该是由于程序开发和版本控制不是一个人，有的程序开发到一半人走了，搞版本控制的，又找不到新的人来进行开发，搁置了忘记修改回来了吧；再或者别的什么意图。
经过上述分析，该parameters_update()函数主要就是获取roll、pitch、yaw的PID参数的。并对三种飞行模式（stablize、auto、acro）下的最大姿态速度做了限制。
3、NuttX任务使能
/* wakeup source: vehicle attitude */
px4_pollfd_struct_t fds[1];
fds[0].fd = _ctrl_state_sub;
fds[0].events = POLLIN;
注意上面的fd的赋值。随后进入任务的循环函数：while (!_task_should_exit){}。都是一样的模式，在姿态解算时也是使用的该种方式。
4、阻塞等待数据 /* wait for up to 100ms for data */
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit */
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("mc att ctrl: poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(100000);
continue;
}
perf_begin(_loop_perf);
首先是px4_poll()配置阻塞时间100ms（uORB模型的函数API）。然后是打开MAVLINK协议，记录数据。如果poll失败，直接使用关键词continue从头开始运行（注意while和continue的组合使用）。其中的usleep(10000)函数属于线程级睡眠函数，使当前线程挂起。原文解释为：
“Theusleep() function will cause the calling thread to be suspended from executionuntil either the number of real-time microseconds specified by the argument'usec' has elapsed or a signal is delivered to the calling thread。”
上面最后一个perf_begin(_loop_perf)，是一个空函数，带perf开头的都是空函数，它的作用主要是“Empty function calls forroscompatibility”。
5、重点来了（获取当前姿态Current）终于到了姿态控制器了，兴奋不？别只顾着兴奋了，好好理解一下。尤其是下面的几个*poll函数，特别重要，后期算法中的很多数据都是通过这个几个*poll()函数获取的，也是uORB模型，不理解这个后去会很晕的，别说没提醒啊；代码中没有一点冗余的部分，每一个函数、每一行都是其意义所在。
/* run controller on attitude changes */
if (fds[0].revents & POLLIN) {
static uint64_t last_run = 0;
float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* guard against too small (<2ms) and too large (>20ms) dt's */
if (dt < 0.002f) {
dt = 0.002f;
} else if (dt > 0.02f) {
dt = 0.02f;
}
/* copy attitude and control state topics *///获取当前姿态数据
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
/* check for updates in other topics */
parameter_update_poll();
vehicle_control_mode_poll();
arming_status_poll();
vehicle_manual_poll();
vehicle_status_poll();
vehicle_motor_limits_poll();
注意上面的revents，要与events区分开来，两者的区别如下：
pollevent_t events; /* The input event flags */
pollevent_t revents; /* The output event flags */
首先就是判断姿态控制器的控制任务是否已经使能，然后就是检测通过hrt获取时间精度的所需时间，并且约束在2ms至20ms以内。完了，orb_copy()函数怎么用的忘记了。。。。
/**
* Fetch data from a topic.
* This is the only operation that will reset the internal marker that
* indicates that a topic has been updated for a subscriber. Once poll
* or check return indicating that an updaet is available, this call
* must be used to update the subscription.
* @param meta The uORB metadata (usually from the ORB_ID() macro)
* for the topic.
* @param handle A handle returned from orb_subscribe.
* @param buffer Pointer to the buffer receiving the data, or NULL
* if the caller wants to clear the updated flag without
* using the data.
* @return OK on success, ERROR otherwise with errno set accordingly.
*/
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
{
return uORB::Manager::get_instance()->orb_copy(meta, handle, buffer);
}
第三个参数就是为了保存通过orb_subscribe()函数订阅获得的有效数据，该部分获取的是_ctrl_state，即控制姿态的数据，数据结构如下：（包含三轴加速度、三轴速度、三轴位置、空速、四元数、roll/pitch/yaw的速率）。记住这个copy的内容，后面会用到多次。
然后就是检测数据是否已经更新，举一例说明问题。
/* check for updates in other topics */
parameter_update_poll();
vehicle_status_poll();//注意这个，后面会用到内部的数据处理结果，即发布和订阅的ID问题。
函数原型：
Void MulticopterAttitudeControl::parameter_update_poll()
{
bool updated;
/* Check if parameters have changed */
orb_check(_params_sub, &updated);
if (updated) {
struct parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update);
parameters_update();
}
}
然后捏：飞行模式判断是否是MAIN_STATE_RATTITUD模式，该模式是一种新的飞行模式，只控制角速度，不控制角度，俗称半自稳模式（小舵量自稳大舵量手动），主要用在setpoint中，航点飞行。根据介绍，这个模式只有在pitch和roll都设置为Rattitude模式时才有意义，如果yaw也设置了该模式，那么就会自动被手动模式替代了。所以代码中只做了x、y阈值的检测。官方介绍：
- RATTITUDE The pilot's inputs are passed as roll, pitch, and yaw rate commands to the autopilot if they are greater than the mode's threshold. If not the inputs are passed as roll and pitch angle commands and a yaw rate command. Throttle is passed directly to the output mixer.
/* Check if we are in rattitude（新的飞行模式，角速度模式，没有角度控制） mode and the pilot is above the threshold on pitch or roll (yaw can rotate 360 in normal att control). If both are true don't even bother running the attitude controllers */
if(_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE){
if (fabsf(_manual_control_sp.y) > _params.rattitude_thres ||
fabsf(_manual_control_sp.x) > _params.rattitude_thres){
_v_control_mode.flag_control_attitude_enabled = false;
}
}
确定飞行模式以后，根据前面的代码分析，在确定了飞行模式以后（判断当前飞行模式，通过最开始部分的*poll函数获取，还记得它么？刚才提醒过了吧），再进行姿态控制。先来代码，然后详细分析。
if (_v_control_mode.flag_control_attitude_enabled)
{
control_attitude(dt);
/* publish attitude rates setpoint */
_v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2);
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub != nullptr) {
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
} else if (_rates_sp_id) {
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
}
//}
} else {
/* attitude controller disabled, poll rates setpoint topic */
if (_v_control_mode.flag_control_manual_enabled) {
/* manual rates control - ACRO mode */
_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max);
_thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER);
/* publish attitude rates setpoint */
_v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2);
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub != nullptr) {
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
} else if (_rates_sp_id) {
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
}
} else {
/* attitude controller disabled, poll rates setpoint topic */
vehicle_rates_setpoint_poll();
_rates_sp(0) = _v_rates_sp.roll;
_rates_sp(1) = _v_rates_sp.pitch;
_rates_sp(2) = _v_rates_sp.yaw;
_thrust_sp = _v_rates_sp.thrust;
}
}
上面的代码中，初始就是control_attitude(dt)，控制数据都是由它来获取的。该函数内部做了很多的处理，控制理论基本都是在这个里面体现的，所以需要深入研究理解它才可以进一步的研究后续的算法。它的内部会通过算法处理获得控制量（目标姿态Target），即_rates_sp，一个vector<3>变量，以便后续控制使用。好了，进入正题。
首先是姿态控制(control_attitude)，然后是速度控制(control_attitude_rates)，一个个来。
6.1、control_attitude()函数(角度控制环) 获取目标姿态Target
/**
* Attitude controller.
* Input: 'vehicle_attitude_setpoint' topics (depending on mode)
* Output: '_rates_sp' vector, '_thrust_sp'
*/
Void MulticopterAttitudeControl::control_attitude(float dt)
{
vehicle_attitude_setpoint_poll();
_thrust_sp = _v_att_sp.thrust;
/* construct attitude setpoint rotation matrix */
math::Matrix<3, 3> R_sp;
R_sp.set(_v_att_sp.R_body);
/* get current rotation matrix from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
math::Matrix<3, 3> R = q_att.to_dcm();
/* all input data is ready, run controller itself */
/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch 约两倍*/
math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));
math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
/* axis and sin(angle) of desired rotation */
math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);
/* calculate angle error */
float e_R_z_sin = e_R.length();
float e_R_z_cos = R_z * R_sp_z;
/* calculate weight for yaw control */
float yaw_w = R_sp(2, 2) * R_sp(2, 2);
/* calculate rotation matrix after roll/pitch only rotation */
math::Matrix<3, 3> R_rp;
if (e_R_z_sin > 0.0f) {
/* get axis-angle representation */
float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;
e_R = e_R_z_axis * e_R_z_angle;
/* cross product matrix for e_R_axis */
math::Matrix<3, 3> e_R_cp;
e_R_cp.zero();
e_R_cp(0, 1) = -e_R_z_axis(2);
e_R_cp(0, 2) = e_R_z_axis(1);
e_R_cp(1, 0) = e_R_z_axis(2);
e_R_cp(1, 2) = -e_R_z_axis(0);
e_R_cp(2, 0) = -e_R_z_axis(1);
e_R_cp(2, 1) = e_R_z_axis(0);
/* rotation matrix for roll/pitch only rotation */
R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
} else {
/* zero roll/pitch rotation */
R_rp = R;
}
/* R_rp and R_sp has the same Z axis, calculate yaw error */
math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
if (e_R_z_cos < 0.0f) {
/* for large thrust vector rotations use another rotation method:
* calculate angle and axis for R -> R_sp rotation directly */
math::Quaternion q;
q.from_dcm(R.transposed() * R_sp);
math::Vector<3> e_R_d = q.imag();
e_R_d.normalize();
e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
/* use fusion of Z axis based rotation and direct rotation */
float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
}
/* calculate angular rates setpoint */
_rates_sp = _params.att_p.emult(e_R);
/* limit rates */
for (int i = 0; i < 3; i++) {
_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
}
/* feed forward yaw setpoint rate */
_rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;
}详细分析：首先就是通过uORB模型检测姿态数据是否已经更新。检测到更新数据以后，把数据拷贝到当前，并通过_thrust_sp = _v_att_sp.thrust把油门控制量赋值给控制变量。
然后捏：构建姿态旋转矩阵（目标状态，所谓的TargetRotation）。
/* construct attitude setpoint rotation matrix */
math::Matrix<3,3> R_sp;
R_sp.set(_v_att_sp.R_body);//不在赘述，在姿态解算时使用了同样的方法然后捏：通过控制四元数获取当前状态的旋转矩阵DCM，后面在计算误差以后旋转到b系时使用到了该处的DCM。即由姿态解算得到的有效姿态信息。
/* get current rotation matrix from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
math::Matrix<3, 3> R = q_att.to_dcm();
通过math库构建四元数；获取DCM的函数原型：无可厚非，都懂的
/*** create rotation matrix for the quaternion */
Matrix<3, 3> to_dcm(void) const {
Matrix<3, 3> R;
float aSq = data[0] * data[0];
float bSq = data[1] * data[1];
float cSq = data[2] * data[2];
float dSq = data[3] * data[3];
R.data[0][0] = aSq + bSq - cSq - dSq;
R.data[0][1] = 2.0f * (data[1] * data[2] - data[0] * data[3]);
R.data[0][2] = 2.0f * (data[0] * data[2] + data[1] * data[3]);
R.data[1][0] = 2.0f * (data[1] * data[2] + data[0] * data[3]);
R.data[1][1] = aSq - bSq + cSq - dSq;
R.data[1][2] = 2.0f * (data[2] * data[3] - data[0] * data[1]);
R.data[2][0] = 2.0f * (data[1] * data[3] - data[0] * data[2]);
R.data[2][1] = 2.0f * (data[0] * data[1] + data[2] * data[3]);
R.data[2][2] = aSq - bSq - cSq + dSq;
return R;
}
};
然后捏：取两个矩阵中的Z轴向量，即YAW-axis。
/* all input data is ready, run controller itself */
/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch 这个地方应该知道旋转按照ZYX来进行的*/
math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));
math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
然后捏：当前姿态的z轴和目标姿态的z轴的误差大小（即需要旋转的角度）并旋转到b系（即先对齐Z轴）。
/* axis and sin(angle) of desired rotation */
math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);
R_z%R_sp_z叉积，还记得这个么?在mahony算法中已经出现过一次了，就是求取误差的，本来应该z轴相互重合的，如果不是0就作为误差项。然后再左乘旋转矩阵旋转到b系。
转置源码：
Matrix3 Matrix3::transposed(void) const
{
return Matrix3(Vector3(a.x, b.x, c.x),
Vector3(a.y, b.y, c.y),
Vector3(a.z, b.z, c.z));
}
然后捏：计算姿态角度误差（姿态误差），一个数学知识背景：由公式a×b=︱a︱︱b︱sinθ，a•b=︱a︱︱b︱cosθ，这里的R_z和R_sp_z都是单位向量，模值为1，因此误差向量e_R（a×b叉积就是误差）的模就是sinθ，点积就是cosθ。
/* calculate angle error */
float e_R_z_sin = e_R.length();
float e_R_z_cos = R_z * R_sp_z;
然后捏：计算yaw的权重（不懂，谁帮忙解释一下原因。。跪谢）
/* calculate weight for yaw control */
float yaw_w = R_sp(2, 2) * R_sp(2, 2);//不懂
第一行的这个权重纯粹是因为如果不转动roll-pitch的话那应该是1，而如果转动的话，那个权重会平方倍衰减 （来自MR的解释）。
然后捏：因为多轴的yaw响应一般比roll/pitch慢了接近一倍，因此将两者解耦(需要理解解耦的目的)，先补偿roll-pitch的变化，计算R_rp。
/* calculate rotation matrix after roll/pitch only rotation */
math::Matrix<3, 3> R_rp;
if (e_R_z_sin > 0.0f) {
/* get axis-angle representation */
float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;
e_R = e_R_z_axis * e_R_z_angle;//很大的用途，下面的R_rp求取公式就是利用欧拉角计算的。
/* cross product matrix for e_R_axis */
math::Matrix<3, 3> e_R_cp;
e_R_cp.zero();
e_R_cp(0, 1) = -e_R_z_axis(2);
e_R_cp(0, 2) = e_R_z_axis(1);
e_R_cp(1, 0) = e_R_z_axis(2);
e_R_cp(1, 2) = -e_R_z_axis(0);
e_R_cp(2, 0) = -e_R_z_axis(1);
e_R_cp(2, 1) = e_R_z_axis(0);
/* rotation matrix for roll/pitch only rotation */
R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));//罗德里格旋转公式：Rodrigues rotation formula
} else {
/* zero roll/pitch rotation */
R_rp = R;
}
首先需要明确的就是上述处理过程中的DCM量都是通过欧拉角来表示的，这个主要就是考虑在控制时需要明确具体的欧拉角的大小，还有就是算法的解算过程是通过矩阵微分方程推导得到的（参考《惯性技术_邓正隆》_P148-P152以及《惯性导航_秦永元》_P342），并且在《惯性技术_邓正隆》_P154页介绍了姿态矩阵的实时解算方法。再判断两个z轴是否存在误差（e_R_z_sin> 0.0f），若存在误差则通过反正切求出该误差角度值（atan2f(e_R_z_sin,e_R_z_cos)）；然后归一化e_R_z_axis(e_R /e_R_z_sin该步计算主要就是利用e_R_z_sin=e_R.length()，往上看就是了，不会这么快就忘记了吧？！)。然后就是e_R =e_R_z_axis* e_R_z_angle了（主要就是为了误差向量用角度量表示）。
然后捏：计算yaw的误差，该误差是roll_pitch获取的z轴和目标姿态的z轴的误差。
/* R_rp and R_sp has the same Z axis, calculate yaw error */
math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
该部分同样是根据向量的叉积和点积求出误差角度的正弦和余弦，再反正切求出角度（又忘记了？回头看吧）。
上面介绍的是在小角度变化时，如果是大角度变化时（大于90°，可能性比较小，还是集中在上面的算法吧）使用如何方法处理。
if (e_R_z_cos < 0.0f) {
/* for large thrust vector rotations use another rotation method:
* calculate angle and axis for R->R_sp rotation directly */
math::Quaternion q;
q.from_dcm(R.transposed() * R_sp);
math::Vector<3> e_R_d = q.imag();
e_R_d.normalize();
e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));//不懂
/* use fusion of Z axis based rotation and direct rotation */
float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
}
上面这段代码比较好理解，主要就是由DCM获取四元数；然后把四元数的虚部取出赋值给e_R_d(e_R_d = q.imag());然后对其进行归一化处理；最后2行是先求出互补系数，再通过互补方式求取e_R。
然后捏：计算角速度变化的大小，并对其进行约束（constrain）。
/* calculate angular rates setpoint */
_rates_sp = _params.att_p.emult(e_R);
/* limit rates */
for (int i = 0; i < 3; i++) {
_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
}
/* feed forward yaw setpoint rate 因为yaw响应较慢，再加入一个前馈控制*/
_rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;
上述代码中的一个emult(e_R)的函数原型：
Matrix emult(const Matrix &other) const
{
Matrix res;
const Matrix &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j)*other(i, j);
}
}
return res;
}
所以_rates_sp = _params.att_p.emult(e_R)这句话的意思就是用att_p的每一个元素和e_R中对应位置的每一个元素相乘，结果赋值给_rates_sp角速度变量（该死的C++）。
6.2、control_attitude(dt)返回以后/* publish attitude rates setpoint */
_v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2);
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub != nullptr) {
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
} else if (_rates_sp_id) {
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
}
上面这部分代码就通过control_attitude(dt)经过一系列的算法处理过以后获取的目标内环角速度值，并通过uORB模型发布出去，包含roll-pitch-yaw、油门量和时间戳。
该处正好可以再次深入理解一下uORB模型的一些理论。上述代码涉及了orb_publish()和orb_advertise()两个函数接口，通常第一次发布有效数据之前需要使用orb_advertise()函数进行广播（类似topic register），它发布成功以后会返回一个handle供orb_publish()发布时使用，即广播之后可以使用orb_publish()进行发布新的数据。orb_advertise()发布函数有第一个参数类似ID，返回值作为handle以便区分再次使用orb_publish()时发布的是何种消息数据，即再次说明orb_publish()需要在orb_advertise()函数接口之后使用。通过查看orb_advertise()函数的代码原型可以了解到，该函数的作用就类似于把需要后续发布的主题（topic）注册一下，然后才可以进行orb_publish()。
现在最不明了的就是这个数据发布出去以后在哪订阅了该数据呢或者说给谁用呢？？？自己发布，自己订阅，生生不息息，PX4里面有很多都是自己发布然后再自己订阅的，感谢群友我是肉包子的帮助。细节说明：在task_main()的开头处就是订阅各种topics，其中就有一个_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint))订阅过程（735_linenumber），它就是在该算法执行到最后时发布的控制量数据“_v_rates_sp”（822），也就是按照前讲述的理论，自己订阅自己发布的topic，以实现循环控制。其中需要注意的就是发布时用的ID和订阅时用的不一致所迷惑了，其实它俩是一样的;因为在上述处理过程中是把ORB_ID(vehicle_rates_setpoint)赋值给_rates_sp_id它的（567），赋值过程在发布topic之前，即在vehicle_status_poll()函数内部（794）。
前面的算法都是在flag_control_attitude_enabled非零（姿态控制）的情况下实现的。紧接着，是在flag_control_attitude_enabled为零时，即转变为flag_control_manual_enabled：手动控制，方法类似，不在赘述。再接着，连手动控制都为使能时，只能poll了，并把控制量都置0。
姿态控制结束。
姿态速度控制开始。
7、姿态速度控制（角速度环）代码来也，先感性认识~~~~~~
if (_v_control_mode.flag_control_rates_enabled) {
control_attitude_rates(dt);
/* publish actuator controls */
_actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f;
_actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f;
_actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
_actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _ctrl_state.timestamp;
_controller_status.roll_rate_integ = _rates_int(0);
_controller_status.pitch_rate_integ = _rates_int(1);
_controller_status.yaw_rate_integ = _rates_int(2);
_controller_status.timestamp = hrt_absolute_time();
if (!_actuators_0_circuit_breaker_enabled) {
if (_actuators_0_pub != nullptr) {
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
perf_end(_controller_latency_perf);
} else if (_actuators_id) {
_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
}
}
/* publish controller status */
if(_controller_status_pub != nullptr) {
orb_publish(ORB_ID(mc_att_ctrl_status),_controller_status_pub, &_controller_status);
} else {
_controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);
}
}
进入上述代码首先就是control_attitude_rates(dt)，该函数的输入是前面算法处理得到的_rates_sp控制量（目标姿态），输出是_att_control控制量。其函数原型是：
Void MulticopterAttitudeControl::control_attitude_rates(float dt)
{
/* reset integral if disarmed */
if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
_rates_int.zero();
}
/* current body angular rates */
math::Vector<3> rates;
rates(0) = _ctrl_state.roll_rate;
rates(1) = _ctrl_state.pitch_rate;
rates(2) = _ctrl_state.yaw_rate;
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;//目标姿态-当前姿态
_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp - _rates_sp_prev) / dt;
_rates_sp_prev = _rates_sp;
_rates_prev = rates;
/* update integral only if not saturated on low limit and if motor commands are not saturated */
if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit ) {
for (int i = 0; i < 3; i++) {
if (fabsf(_att_control(i)) < _thrust_sp) {
float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
_att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
_rates_int(i) = rate_i;
}
}
}
}
}
主要就是通过_ctrl_state数据结构（前面说过要记住它的吧，当前姿态信息）把需要的有效数据赋值给rates，然后通过rates进行一系列的算法处理。该过程中最最最需要注意的就是这个_ctrl_state变量的获取过程，其实还是通过uORB。前面也涉及过多次，比如control_attitude()函数内部使用它构造状态四元数。
如下非常重要。。。。。。打通姿态解算和姿态控制部分。
数据获取过程：
Quaterion_CF姿态解算算法：（需要对代码有个整体把握，不然会很晕啊，还有就是关于姿态解算部分使用的CF时，在PX4Firmware/src/module/attitude_estimator_q中）。首先是通过姿态解算部分获取当前的姿态信息（Quaterion_CF），获取之后通过uORB模型发布：
/* publish to control state topic */(646)
orb_publish_auto(ORB_ID(control_state), &_ctrl_state_pub, &ctrl_state, &ctrl_inst, ORB_PRIO_HIGH);
Ekf2姿态解算算法：（还是需要对代码有个整体把握，不然还是会很晕啊，还有就是关于姿态解算部分使用的ekf2时，在PX4Firmware/src/module/ekf2中）。首先是通过姿态解算部分获取当前的姿态信息（ekf2），获取之后通过uORB模型发布：
// publish control state data(475)
if (_control_state_pub == nullptr) {
_control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
} else {
orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state);
}
关于到底使用哪种解算算法在启动脚本rc_mc_app里面涉及了关于姿态解算用什么算法的问题，里面给了一个宏，通过宏定义选取的。而且在使用四元数的互补算法和ekf2的算法里面都对结算到的姿态信息进行了发布处理，以便供姿态控制时订阅使用。
然后再姿态控制中通过uORB模型订阅：
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));(736)
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);(787)
再然后就是姿态控制量（_att_control）的获取：获取原则是由预期姿态控制获取的角速度值与通过uORB获得的角速度值做差（该部分差值代表error=target-current，_ctrl_state应该是要控制的控制量）。rates_err的获取就是通过经典的PD控制器了，然后再加个前馈。还未使用I控制器；在后面会单独使用。
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;
_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp - _rates_sp_prev) / dt;
_rates_sp_prev = _rates_sp;
_rates_prev = rates;
I控制器的使用（注意使用条件）。
/* update integral only if not saturated on low limit and if motor commands are not saturated */
if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit ) {
for (int i = 0; i < 3; i++) {
if (fabsf(_att_control(i)) < _thrust_sp) {
float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
_att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
_rates_int(i) = rate_i;
}
}
}
}
其中fabsf()的函数原型是（取绝对值）：
float fabsf(float x)
{
return ((x < 0) ? -x : x);
}
常用的几种取绝对值的函数：
int abs(int i); //处理int类型的取绝对值
double fabs(double i); //处理double类型的取绝对值
float fabsf(float i); //处理float类型的取绝对值
注意上面的fabsf(_att_control(i)) <_thrust_sp)这个判断项，符合就执行积分。这个做主要是为了安全考虑，当roll的变化值需要很大时，就停止积分项的累加以便防止积分项产生较大的误差。
别看这个_thrust_sp单单的一个控制量，其实它可麻烦了，不对整体核心的解算和控制（姿态解算姿态控制、位置解算位置控制）有个深入理解的话，很难看懂这部分。下面详细介绍一下这个控制量的获取过程，耐心看，别晕了。介绍还是需要正向介绍，在看的时候可以反向看，比较容易理解。
首先是_v_att_sp_sub =orb_subscribe(ORB_ID(vehicle_attitude_setpoint));（813），订阅所需的控制量。
然后再attitude control里面处理：_thrust_sp =_v_att_sp.thrust（653）
上面是订阅拷贝和使用部分，下面就是发布部分。
发布分为两个地方，一个是mc_pos_control和mavlink_receiver.cpp。主要考虑前者。
ID重定义：_attitude_setpoint_id= ORB_ID(vehicle_attitude_setpoint);（595）
正式发布给mc_att_control: orb_publish(_attitude_setpoint_id,_att_sp_pub,&_att_sp);(1932)
为何称为正式发布呢？主要是因为在mc_pos_control里面根据不懂的模式进行了多次发布处理，比如idle状态下这个_thrust_sp就赋值为0发布出去。这个正式发布出来的才是我们飞行控制过程中需要考虑的控制量。
补充mavlink_receiver.cpp
orb_publish(ORB_ID(vehicle_attitude_setpoint),_att_sp_pub,&_att_sp);（951）
现在发现这个规律了吧，任务间通信（IPC）都是靠的uORB，找不到来源就查ID吧。
8、发布控制量/* publish actuator controls */
_actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f;
_actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f;
_actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
_actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _ctrl_state.timestamp;
_controller_status.roll_rate_integ = _rates_int(0);
_controller_status.pitch_rate_integ = _rates_int(1);
_controller_status.yaw_rate_integ = _rates_int(2);
_controller_status.timestamp = hrt_absolute_time();
if (!_actuators_0_circuit_breaker_enabled) {
if (_actuators_0_pub != nullptr) {
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
perf_end(_controller_latency_perf);
} else if (_actuators_id) {
_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
}
}
/* publish controller status */
if(_controller_status_pub != nullptr) {
orb_publish(ORB_ID(mc_att_ctrl_status),_controller_status_pub, &_controller_status);
} else {
_controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);
}
}
}
perf_end(_loop_perf);
}
_control_task = -1;
return;
PS：一个比较有趣的东西task handle：“_control_task”
了解姿态控制任务的执行流么？可以参考这个task handle思考思考。
其实在mc_att_control里面就完全涵盖了姿态控制的内环和外环（即角速度控制、角度控制）。主要就是attitude control和attitude rate control两个部分，前者是控制角度后者是控制角速度并把控制量输入给mixer。在控制过程中是通过控制电机的速度以实现多旋翼的整体的rpy的速度，通过这个速度随时间的累加实现角度控制。
attitude_control 输入是体轴矩阵R和期望的体轴矩阵Rsp，角度环只是一个P控制，算出来之后输出的是期望的角速度值rate_sp（这一段已经完成了所需要的角度变化，并将角度的变化值转换到了需要的角速度值）。并且把加速度值直接输出给attitude rate control，再经过角速度环的pid控制，输出值直接就给mixer，然后控制电机输出了。
关于这些，主要还是需要理解这个控制过程：一方面是通过姿态解算部分获取的实时的姿态信息，并通过uORB模型把姿态信息发布出去；姿态控制部分订阅姿态解算得到的姿态信息。然后通过attitude control获取目标姿态和当前姿态的角度差值并经过算法处理得到对应的角速度值，并把这个角速度值输出给attitude rate control 最终获取到需求的控制量。输出给mixer。但是关于上述还是有一个迷惑的地方，就是在attitude control这个里面输出的是根据目标姿态计算的角速度值，然后再和attitude rate control 里面通过uORB获取的当前的角速度值做差得出角速度差值。。。。本身对这个比较懵逼。其实attitude control输出是需要达到这个误差角度时所需要的角速度值，用这个值与当前的角速度值做差，求出现在需要的角速度值而已。这个就是为什么控制角速度的原因，进而达到控制角度的效果。
本篇blog写了很久了也写了很久，收获甚多，感触甚多，愿本篇blog能给正在迷茫的你一点帮助~~~
祝愿祖国繁荣昌盛，也希望雷某案早日结束。有能力的还是移民吧~~~~
下一篇预告：欧拉旋转和罗德里格旋转公式~~~~