应用程序的入口，“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);整个姿态控制流程主要这些,具体的算法还要继续研究