学习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 4 /* get current rotation matrix and euler angles from control state quaternions */ 5 math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); 6 _R = q_att.to_dcm(); 7 8 math::Vector<3> euler_angles; 9 euler_angles = _R.to_euler(); 10 _roll = euler_angles(0); 11 _pitch = euler_angles(1); 12 _yaw = euler_angles(2);
由于姿态控制算法在一些模式下面是不会估算姿态设定点的，所以要确认这些标志。
_att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled;//判断垂尾，用于自主起飞。
判断是否安全故障，如果安全故障开启，就会设定降落伞。
接下来是襟翼的设定，分为手动控制和自动控制
1 float flap_control = 0.0f;
2
3 /* map flaps by default to manual if valid */
4 if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled
5 && fabsf(_parameters.flaps_scale) > 0.01f) {
6 flap_control = 0.5f * (_manual.flaps + 1.0f) * _parameters.flaps_scale;
7
8 } else if (_vcontrol_mode.flag_control_auto_enabled
9 && fabsf(_parameters.flaps_scale) > 0.01f) {
10 flap_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f;
11 }
12
13 // move the actual control value continuous with time, full flap travel in 1sec
14 if (fabsf(_flaps_applied - flap_control) > 0.01f) {
15 _flaps_applied += (_flaps_applied - flap_control) < 0 ? deltaT : -deltaT;
16
17 } else {
18 _flaps_applied = flap_control;
19 }
然后是襟副翼的设定，分为手动控制和自动控制
1 if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled
2 && fabsf(_parameters.flaperon_scale) > 0.01f) {
3 flaperon_control = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale;
4
5 } else if (_vcontrol_mode.flag_control_auto_enabled
6 && fabsf(_parameters.flaperon_scale) > 0.01f) {
7 flaperon_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f;
8 }
9
10 // move the actual control value continuous with time, full flap travel in 1sec
11 if (fabsf(_flaperons_applied - flaperon_control) > 0.01f) {
12 _flaperons_applied += (_flaperons_applied - flaperon_control) < 0 ? deltaT : -deltaT;
13
14 } else {
15 _flaperons_applied = flaperon_control;
16 }
接下来就是通过订阅的消息来判断是否能够控制姿态
也就是这一行代码：
1 if (_vcontrol_mode.flag_control_attitude_enabled)
在这个if语句里面执行的主要有判断空速是否有效，如果无效设定空速为参数设定，如果有效设定空速为测量或者计算的空速。
通过vehicle_global_position来计算飞机的地面速度。
如果自问模式下面，我们需要通过遥控器来产生姿态设定点；
计算机体坐标系下飞机的速度；
准备姿态控制器运行需要的参数；
1 control_input.roll = _roll; 2 control_input.pitch = _pitch; 3 control_input.yaw = _yaw; 4 control_input.roll_rate = _ctrl_state.roll_rate; 5 control_input.pitch_rate = _ctrl_state.pitch_rate; 6 control_input.yaw_rate = _ctrl_state.yaw_rate; 7 control_input.speed_body_u = speed_body_u; 8 control_input.speed_body_v = speed_body_v; 9 control_input.speed_body_w = speed_body_w; 10 control_input.acc_body_x = _accel.x; 11 control_input.acc_body_y = _accel.y; 12 control_input.acc_body_z = _accel.z; 13 control_input.roll_setpoint = roll_sp; 14 control_input.pitch_setpoint = pitch_sp; 15 control_input.yaw_setpoint = yaw_sp; 16 control_input.airspeed_min = _parameters.airspeed_min; 17 control_input.airspeed_max = _parameters.airspeed_max; 18 control_input.airspeed = airspeed; 19 control_input.scaler = airspeed_scaling; 20 control_input.lock_integrator = lock_integrator; 21 control_input.groundspeed = groundspeed; 22 control_input.groundspeed_scaler = groundspeed_scaler;
前面的判断就是为运行姿态控制器所准备的。
1 _roll_ctrl.control_attitude(control_input); 2 _pitch_ctrl.control_attitude(control_input); 3 _yaw_ctrl.control_attitude(control_input); 4 _wheel_ctrl.control_attitude(control_input);
上面就是计算目标与当前姿态的角度误差值,对于roll和pitch是计算角度误差，然后算出角速率，对于yaw速率的计算是，假设在没有侧向力的情况下,通过计算可以得到相应的yaw速率：
就是通过如下计算公式得到：
以及：
、
计算得到相应的yaw速率；
由于滚转，俯仰和偏航速率是在地面坐标系下，因此，要通过坐标转换转换到机体坐标系下，也就是：
通过上述转换就把角速率转换到机体坐标系下了，接下来是通过误差，运用pid来控制姿态，下面的还需要研究一下。先这样吧。
以上内容均属自己理解，有错误之处见谅，共同讨论。