姿态控制流程图

从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