應用程序的入口，“extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[])”，在這個函數裏就實現了，這個程序是否已近啓動，如果沒有啓動就會註冊函數來啓動。
聲明FixedwingAttitudeControl是主函數
int fw_att_control_main(int argc, char *argv[])
{
return FixedwingAttitudeControl::main(argc, argv);
}
一,剛開始就是一些初始化設置和消息訂閱
FixedwingAttitudeControl::FixedwingAttitudeControl() :
導入參數
_parameter_handles.XXXX=param_find("XXXX")
消息的訂閱：
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));//姿態
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));//姿態設定點
_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));//着陸深測
_battery_status_sub = orb_subscribe(ORB_ID(battery_status));//電池模式
_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));//速度設定
聲明瞭取消訂閱的方法
FixedwingAttitudeControl::~FixedwingAttitudeControl()//取消訂閱
參數更新:
FixedwingAttitudeControl::parameters_update()
二.接下來就是以check的方式獲取訂閱消息的值：
FixedwingAttitudeControl::vehicle_control_mode_poll()//控制模式判定
FixedwingAttitudeControl::vehicle_manual_poll()//彈體手動判定
FixedwingAttitudeControl::vehicle_attitude_setpoint_poll()//彈體姿態設定判定
FixedwingAttitudeControl::vehicle_rates_setpoint_poll()//彈體速度設定判定
FixedwingAttitudeControl::global_pos_poll()//彈體位置設定判定
FixedwingAttitudeControl::vehicle_status_poll()//彈體狀態設置判定
FixedwingAttitudeControl::vehicle_land_detected_poll()//彈體着陸狀態判定
FixedwingAttitudeControl::get_airspeed_and_scaling// 更新空速
//獲取所有傳感器和狀態數據的初始更新
//_poll()函數是輪詢,內部的代碼是檢測是否更新,若更新了數據,就copy
orb_check(_vehicle_xxxx_sub, &vehicle_xxxx_updated)
然後if判定vehicle_xxxx_updated真僞
以阻塞等待方式檢查參數是否更新，以及當前的飛機狀態。阻塞等待這種獲取訂閱消息的方式
三.姿態控制
FixedwingAttitudeControl::run()
fds[0.fd]爲輪詢的姿態數據,其events是爲了檢測姿態是否發生改變
* wakeup source */
px4_pollfd_struct_t fds[1];
/* Setup of loop */
fds[0].fd = _att_sub;
fds[0].events = POLLIN;//fds[0.fd]爲
這個if函數就是姿態控制運行的開始，如果姿態發生了改變，就運行這個if:
/* only run controller if attitude changed只有在姿態改變時才運行控制器 */
if (fds[0].revents & POLLIN) {
- 那麼要知道姿態是否發生改變，那麼就需要知道當前的姿態，這通過獲取當前姿態的消息，然後得到四元數，轉化爲旋轉矩陣，進而求得姿態角：
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
/* get current rotation matrix and euler angles from control state quaternions
matrix::Dcmf R = matrix::Quatf(_att.q);
if (_vehicle_status.is_vtol && _parameters.vtol_type == vtol_type::TAILSITTER) {
matrix::Dcmf R_adapted = R; //modified rotation matrix修正旋轉矩陣
/* move z to x */
R_adapted(0, 0) = R(0, 2);
R_adapted(1, 0) = R(1, 2);
R_adapted(2, 0) = R(2, 2);
/* move x to z */
R_adapted(0, 2) = R(0, 0);
R_adapted(1, 2) = R(1, 0);
R_adapted(2, 2) = R(2, 0);
/* change direction of pitch (convert to right handed system) */
R_adapted(0, 0) = -R_adapted(0, 0);
R_adapted(1, 0) = -R_adapted(1, 0);
R_adapted(2, 0) = -R_adapted(2, 0);
/* fill in new attitude data */
R = R_adapted;
/* lastly, roll- and yawspeed have to be swaped */
float helper = _att.rollspeed;
_att.rollspeed = -_att.yawspeed;
_att.yawspeed = helper;
}
matrix::Eulerf euler_angles(R);//矩陣：歐拉-歐拉角（r）；
2.由於姿態控制算法在一些模式下面是不會估算姿態設定點的，所以要確認這些標誌。
_att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled;//判斷垂尾，用於自主起飛。
3./* lock integrator until control is started 鎖定積分器直到控制啓動*/
bool lock_integrator = !(_vcontrol_mode.flag_control_rates_enabled && !_vehicle_status.is_rotary_wing);
4./故障保護的簡單處理：如果故障保護開啓，則展開降落傘。*/
5.接下來是襟翼的設定，
control_flaps(deltaT)
在姿態運行的開始已經聲明瞭float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; 當前時間減去上次運行的時間
if (deltaT > 1.0f) {//防止太大增量
deltaT = 0.01f;
}
襟翼分爲手動控制和自動控制
/* default flaps to center 默認襟翼到中心*/
float flap_control = 0.0f;
/* map flaps by default to manual if valid 如果有效，默認地圖襟翼爲手動*/
if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled
&& fabsf(_parameters.flaps_scale) > 0.01f) {
flap_control = 0.5f * (_manual.flaps + 1.0f) * _parameters.flaps_scale;
} else if (_vcontrol_mode.flag_control_auto_enabled
&& fabsf(_parameters.flaps_scale) > 0.01f) {
switch (_att_sp.apply_flaps) {
case vehicle_attitude_setpoint_s::FLAPS_OFF : flap_control = 0.0f; // no flaps無襟翼
break;
case vehicle_attitude_setpoint_s::FLAPS_LAND : flap_control = 1.0f * _parameters.flaps_scale *
_parameters.flaps_land_scale; // 着陸襟翼landing flaps
break;
case vehicle_attitude_setpoint_s::FLAPS_TAKEOFF : flap_control = 1.0f * _parameters.flaps_scale *
_parameters.flaps_takeoff_scale; // take-off flaps起飛襟翼
break;
}
}
// move the actual control value continuous with time, full flap travel in 1sec隨時間連續移動實際控制值，1秒內全襟翼行程
if (fabsf(_flaps_applied - flap_control) > 0.01f) {
_flaps_applied += (_flaps_applied - flap_control) < 0 ? dt : -dt;
} else {
_flaps_applied = flap_control;
}
接着襟副翼的設定
/* default flaperon to center 默認襟副翼到中心*/
float flaperon_control = 0.0f;
/* map flaperons by default to manual if valid 如果有效，默認將Flaperons映射爲手動*/
if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled
&& fabsf(_parameters.flaperon_scale) > 0.01f) {
flaperon_control = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale;
} else if (_vcontrol_mode.flag_control_auto_enabled
&& fabsf(_parameters.flaperon_scale) > 0.01f) {
flaperon_control = (_att_sp.apply_flaps == vehicle_attitude_setpoint_s::FLAPS_LAND) ? 1.0f *
_parameters.flaperon_scale : 0.0f;
}
// move the actual control value continuous with time, full flap travel in 1sec
//隨時間連續移動實際控制值，1秒內全襟翼行程
if (fabsf(_flaperons_applied - flaperon_control) > 0.01f) {
_flaperons_applied += (_flaperons_applied - flaperon_control) < 0 ? dt : -dt;
} else {
_flaperons_applied = flaperon_control;
}
6.決定姿態是自穩還是完全手動控制
if (_vcontrol_mode.flag_control_rates_enabled) {
在這個if語句裏面執行的主要有判斷空速是否有效，如果無效設定空速爲參數設定，如果有效設定空速爲測量或者計算的空速。
通過vehicle_global_position來計算飛機的地面速度。
如果自穩模式下面，我們需要通過遙控器來產生姿態設定點；
計算機體座標系下飛機的速度；
如果飛機在地面或是多旋翼飛機（但不轉換vtol），重置積分器。
旋轉矩陣進而求得姿態角 */；
struct ECL_ControlData control_input = {};
準備姿態控制器運行需要的參數:
control_input.roll = euler_angles.phi();
control_input.pitch = euler_angles.theta();
control_input.yaw = euler_angles.psi();
control_input.body_x_rate = _att.rollspeed;
control_input.body_y_rate = _att.pitchspeed;
control_input.body_z_rate = _att.yawspeed;
control_input.roll_setpoint = _att_sp.roll_body;
control_input.pitch_setpoint = _att_sp.pitch_body;
control_input.yaw_setpoint = _att_sp.yaw_body;
control_input.airspeed_min = _parameters.airspeed_min;
control_input.airspeed_max = _parameters.airspeed_max;
control_input.airspeed = airspeed;
control_input.scaler = airspeed_scaling;
control_input.lock_integrator = lock_integrator;
control_input.groundspeed = groundspeed;
control_input.groundspeed_scaler = groundspeed_scaler;
reset body angular rate limits on mode change重置模式更改時的車身角速率限制
Run attitude controllers 運行姿態控制器*/
if (_vcontrol_mode.flag_control_attitude_enabled) {
前面的判斷就是爲運行姿態控制器所準備的。
_roll_ctrl.control_attitude(control_input);
_pitch_ctrl.control_attitude(control_input);
_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
_wheel_ctrl.control_attitude(control_input);
上面的代碼計算目標與當前姿態的角度誤差值,對於roll和pitch是計算角度誤差，然後算出角速率，對於yaw速率的計算是，假設在沒有側向力的情況下,通過計算可以得到相應的yaw速率
更新速率控制器的輸入數據
control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate();
control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate();
//運行姿態率控制器，需要從上面獲得所需姿態，增加配平
手動模式下的附加手動方向舵控制
如果節氣門有限且未檢測到發動機故障，則節氣門通過
按電池狀態縮放作用力
延遲發佈速率設定值（分析時，執行器發佈如下）
* 只有一次可用
從橫搖控制輸出到橫擺控制輸出添加前饋
這可以用來抵消飛機滾動時的不利偏航效應。
actuators.control[actuator_controls_s::INDEX_YAW] += _parameters.roll_to_yaw_ff * math::constrain(
_actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f);
整個姿態控制流程主要這些,具體的算法還要繼續研究