一、消息订阅
二.获取订阅消息值
三.姿态控制
前面的判断是为运行姿态控制器准备的。
应用程序的入口，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]为 /* 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::TAISITTER) {
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);整个姿态控制流程主要这些,具体的算法还要继续研究