px4固定翼无人机姿态控制理解

　　学习px4代码也有一段时间了，所以想写一写，自己的一些学习心得吧，也算是笔记吧。

　　在px4这套代码中，每一个功能都是一个模块，例如姿态控制，也就是一个应用程序，我们可以把它添加到初始话脚本里，让它自启动。需要注意的就是在一个应用程序就是处理订阅的消息，然后发布处理过后的消息。这种消息机制就是uorb消息机制，可以找资料学习它具体实现的一个过程。

　　接下来，就逐步学习一下这个姿态控制的模块。

　　首先就是应用程序的入口，“extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[])”，在这个函数里就实现了，这个程序是否已近启动，如果没有启动就会注册函数来启动。

　　task_main()这个函数就是整个姿态控制的关键，具体姿态控制的算法就是在这里面实现。刚开始就介绍说每个应用程序会订阅相应的消息，然后处理，最后发布。

所以刚开始就是一些消息的订阅：

_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));//姿态设定点

_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));//飞机状态

_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);//加速度值
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));//控制模式

_params_sub = orb_subscribe(ORB_ID(parameter_update));//参数更新

_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));//手动控制的设定

_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));//车辆全球位置

_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));//飞机状态

_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));//着陆探测

接下来就是以check的方式获取订阅消息的值：

vehicle_setpoint_poll();
vehicle_accel_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
vehicle_status_poll();
vehicle_land_detected_poll();

接下来就是以阻塞等待方式检查参数是否更新，以及当前的飞机状态。阻塞等待这种获取订阅消息的方式，具体是怎么实现的可以查找资料进行学习。

这个if函数就是姿态控制运行的开始，如果姿态发生了改变，就运行这个if

if (fds[1].revents & POLLIN) 

那么要知道姿态是否发生改变，那么就需要知道当前的姿态，这通过获取当前姿态的消息，然后得到四元数，转化为旋转矩阵，进而求得姿态角：

 1 orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
 2 
 3