學習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來控制姿態，下面的還需要研究一下。先這樣吧。
以上內容均屬自己理解，有錯誤之處見諒，共同討論。