从vehicle_attitude_setpoint中获取设定姿态值，存储到变量_v_att_sp中（包括期望三轴角度、期望四元数、油门量、起落架等）
从control_state中提取当前角速度rates
extern \"C\
由当前旋转矩阵 R、(rx ry 0)、θ经过罗德里格变换得到只重合z轴所需的旋转矩阵 R_rp
将_att_control和_thrust_sp等数据通过变量_actuators.control公告及发布到主题actuator_controls中
更新所订阅主题的其他参数
否
将积分项∑(I*Δ*dt) 通过变量_rates_int公告及发布到主题mc_att_ctrl_status中
θ90°
e_R = (θ*rx，θ*ry，ψ)
函数 start()
采用角度控制
偏航权重yaw_w=R_sp[2][2] * R_sp[2][2]
函数结束
将_rates_sp和_thrust_sp数据通过变量_v_rates_sp公告及发布到主题vehicle_rates_setpoint中
函数parameters_update() 主要进行各种参数配置，包括：PID和反馈各项系数；不同模式下角速度限制；角速度/角度控制模式的阈值等各项配置。初始的配置参数储存在mc_pos_control_params.c文件中，在构造函数中将其读取出来
从_v_att_sp中取出油门量，存储到变量_thrust_sp中
_rates_sp取为期望三轴角度_thrust_sp取值为期望油门量
几点说明：1. 本函数的主要思路是输入当前姿态和期望姿态，计算得出三轴的控制量，发布到actuator_controls中。2. 角度控制环中将偏航角与俯仰滚转角分离，主要原因是偏航角的响应要比其他两轴慢。欢迎多批评指正!460864915@qq.com
_rates_sp 的范围限制处理
从control_state中取出当前四元数，得到当前旋转矩阵 R
函数control_attitude(dt)角度控制环
从vehicle_attitude_setpoint中获取设定姿态值，存储到变量_v_att_sp中
是
mc_att_control_main.cpp工作流程图
使能了比率控制模式
配置阻塞时间（100ms）
是否使能了rattitude控制模式
获取上次迭代中的当前角速度rates(n-1)和期望角速度_rate_sp(n-1)
pitch或者roll方向的遥控输入是否超过阈值
_task_should_exit==ture
R_rp[*][0] % R_sp[*][0]得到旋转向量，从而得到yaw方向的旋转角度ψ
采用rattitude控制
k=cosθ*cosθ*yaw_w
读取control_state中的数据：control_state中的数据由姿态解算发布，包括三轴位置、速度、加速度、角速度、四元数、空速等
限制dt在[2ms，20ms]之间
积分饱和处理
RT * ( R_sp[*][3] % R[*][3] )得到使得期望坐标系和当前坐标系z轴重合所需的旋转，在机体坐标系下的的旋转轴向量e_R（包含旋转轴方向和旋转角大小）
perf_end(_loop_perf)该类函数主要用于对程序运行状态进行评估监控
计算两次迭代之间的时间 dt，单位为秒
函数内部
perf_begin(_loop_perf)
_rates_sp和_thrust_sp的值完全由遥控给出
是否为尾坐式飞机
函数 mc_att_control_main()
RT * R_sp得到的结果转换为四元数，取虚部得到(q1 q2 q3)
by 蜗牛拉火车
由e_R得到旋转轴单位向量 (rx ry rz) 和旋转角度θ。可以证明rz=0
e_R=θ*(rx ry 0)
control_attitude_rates(dt)角速度控制环
进入While()大循环
从_v_att_sp中取出期望四元数，得到期望旋转矩阵 R_sp
订阅以下主题：vehicle_attitude_setpoint vehicle_rates_setpointcontrol_state vehicle_control_modeparameter_update manual_control_setpointactuator_armed vehicle_status multirotor_motor_limits
_rates_sp = e_R * P其中：P为角度的比例项系数
期望角速度减去当前角速度得到角速度差：Δ= _rates_sp - rates
由函数calcOptimalRates()得到_rates_sp
设定油门衰减因子tpa（当油门大于某一值时，减小控制的比例项系数）
函数 task_main()
控制量：_att_control=Δ*tpa*P+ D*(rates(n-1)-rates)/dt+ ∑(I*Δ*dt)+ FF*_rates_sp
