文章目录
- 前言
- 一、roll/pitch姿态/角速率控制
- 二、偏航角速率控制
- 三、主程序
前言
固件版本 PX4 1.13.2
欢迎交流学习，可加左侧名片
一、roll/pitch姿态/角速率控制
roll/pitch的姿态控制类似，这里只介绍roll姿态控制，
代码位置：
外环姿态控制
float ECL_RollController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
{如果值异常，返回上一次的结果
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll_setpoint) &&
PX4_ISFINITE(ctl_data.roll))) {
return _rate_setpoint;
}求roll姿态误差
/* Calculate the error */
float roll_error = ctl_data.roll_setpoint - ctl_data.roll;外环P控制，_tc为时间常数，可在地面站参数列表中设置FW_R_TC
/* Apply P controller: rate setpoint from current error and time constant */
_rate_setpoint = roll_error / _tc;
return _rate_setpoint;
}内环角速率控制
float ECL_RollController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data)
{判断值是否可用
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.body_x_rate) &&
PX4_ISFINITE(ctl_data.body_z_rate) &&
PX4_ISFINITE(ctl_data.yaw_rate_setpoint) &&
PX4_ISFINITE(ctl_data.airspeed_min) &&
PX4_ISFINITE(ctl_data.airspeed_max) &&
PX4_ISFINITE(ctl_data.scaler))) {
return math::constrain(_last_output, -1.0f, 1.0f);
}求角速率误差
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - ctl_data.body_x_rate;求积分，这里的ctl_data.scaler是一个缩放系数，具体的值和参数FW_ARSP_SCALE_EN有关，如果FW_ARSP_SCALE_EN为1（enable），则ctl_data.scaler的值为（参数FW_AIRSPD_TRIM/空速），也就是空速越大，积分系数越小，这是因为空速大的时候舵面的力更大。如果FW_ARSP_SCALE_EN为0（disable）,则ctl_data.scaler为1。
if (!ctl_data.lock_integrator && _k_i > 0.0f) {
/* Integral term scales with 1/IAS^2 */
float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler;抗积分饱和，负向饱和就只加正积分，正向饱和就只加负积分
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
} else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}将积分增量乘系数_k_i加到积分项，_k_i为参数FW_RR_I
/* add and constrain */
_integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max);
}PI+前馈控制得到最终输出，这个输出就是给混控的。缩放系数同上，前馈系数_k_ff对应参数FW_RR_FF，比例系数_k_p对应参数FW_RR_P。
/* Apply PI rate controller and store non-limited output */
/* FF terms scales with 1/TAS and P,I with 1/IAS^2 */
_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
_rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
+ _integrator;
return math::constrain(_last_output, -1.0f, 1.0f);
}姿态控制的当前姿态和期望姿态都是基于地理坐标系的，即正北为航向正方向，地理的水平面为横滚的0度。实际控制的时候都是作用于机体进行控制的，所以将地理系下期望的角速度，转换为机体坐标系下的期望角速度，然后调用内环角速度控制
float ECL_RollController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff)
{
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint + bodyrate_ff;
set_bodyrate_setpoint(_bodyrate_setpoint);
return control_bodyrate(dt, ctl_data);
}二、偏航角速率控制
PX4中固定翼是不控制偏航角度的，只控制偏航角速率。
代码位置
判断值是否可用
float ECL_YawController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.roll_rate_setpoint) &&
PX4_ISFINITE(ctl_data.pitch_rate_setpoint))) {
return _rate_setpoint;
}飞机没有倒飞的情况下，将横滚进行限幅
float constrained_roll;
bool inverted = false;
/* roll is used as feedforward term and inverted flight needs to be considered */
if (fabsf(ctl_data.roll) < math::radians(90.0f)) {
/* not inverted, but numerically still potentially close to infinity */
constrained_roll = math::constrain(ctl_data.roll, math::radians(-80.0f), math::radians(80.0f));
}飞机倒飞的情况下，将横滚进行限幅
else {
inverted = true;
// inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity
//note: the ranges are extended by 10 deg here to avoid numeric resolution effects
if (ctl_data.roll > 0.0f) {
/* right hemisphere */
constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f));
} else {
/* left hemisphere */
constrained_roll = math::constrain(ctl_data.roll, math::radians(-180.0f), math::radians(-100.0f));
}
}限幅
constrained_roll = math::constrain(constrained_roll, -fabsf(ctl_data.roll_setpoint), fabsf(ctl_data.roll_setpoint));没有倒飞的情况下，利用协调转弯方程计算期望的偏航角速度，协调转弯方程如下，
if (!inverted) {
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
_rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / (ctl_data.airspeed <
ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed);
}if (!PX4_ISFINITE(_rate_setpoint)) {
PX4_WARN("yaw rate sepoint not finite");
_rate_setpoint = 0.0f;
}
return _rate_setpoint;
}偏航角速率控制
判断值是否可用
float ECL_YawController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.body_y_rate) &&
PX4_ISFINITE(ctl_data.body_z_rate) &&
PX4_ISFINITE(ctl_data.pitch_rate_setpoint) &&
PX4_ISFINITE(ctl_data.airspeed_min) &&
PX4_ISFINITE(ctl_data.airspeed_max) &&
PX4_ISFINITE(ctl_data.scaler))) {
return math::constrain(_last_output, -1.0f, 1.0f);
}求角速率误差
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - ctl_data.body_z_rate;求积分，同roll角速率控制
if (!ctl_data.lock_integrator && _k_i > 0.0f) {
/* Integral term scales with 1/IAS^2 */
float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler;抗积分饱和，同roll角速率控制
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
} else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}累加积分
/* add and constrain */
_integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max);
}PI+前馈控制，同roll角速率控制
/* Apply PI rate controller and store non-limited output */
/* FF terms scales with 1/TAS and P,I with 1/IAS^2 */
_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
_rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
+ _integrator;
return math::constrain(_last_output, -1.0f, 1.0f);
}地理转机体坐标系，同roll
float ECL_YawController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff)
{
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint +
cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint + bodyrate_ff;
set_bodyrate_setpoint(_bodyrate_setpoint);
return control_bodyrate(dt, ctl_data);
}三、主程序
代码位置
构造函数，初始化一些句柄和参数，PX4固定翼的姿态控制在工作队列nav_and_controllers中运行，其运行的优先级仅次于传感器驱动
FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
_actuators_0_pub(vtol ? ORB_ID(actuator_controls_virtual_fw) : ORB_ID(actuator_controls_0)),
_actuator_controls_status_pub(vtol ? ORB_ID(actuator_controls_status_1) : ORB_ID(actuator_controls_status_0)),
_attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
// check if VTOL first
if (vtol) {
int32_t vt_type = -1;
if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) {
_is_tailsitter = (static_cast<vtol_type>(vt_type) == vtol_type::TAILSITTER);
}
}
/* fetch initial parameter values */
parameters_update();
// set initial maximum body rate setpoints
_roll_ctrl.set_max_rate(radians(_param_fw_acro_x_max.get()));
_pitch_ctrl.set_max_rate_pos(radians(_param_fw_acro_y_max.get()));
_pitch_ctrl.set_max_rate_neg(radians(_param_fw_acro_y_max.get()));
_yaw_ctrl.set_max_rate(radians(_param_fw_acro_z_max.get()));
_rate_ctrl_status_pub.advertise();
}释放性能计数器perf_counter
FixedwingAttitudeControl::~FixedwingAttitudeControl()
{
perf_free(_loop_perf);
}初始化回调函数
bool
FixedwingAttitudeControl::init()
{
if (!_att_sub.registerCallback()) {
PX4_ERR("callback registration failed");
return false;
}
return true;
}更新参数
int
FixedwingAttitudeControl::parameters_update()
{
/* pitch control parameters */
_pitch_ctrl.set_time_constant(_param_fw_p_tc.get());
_pitch_ctrl.set_k_p(_param_fw_pr_p.get());
_pitch_ctrl.set_k_i(_param_fw_pr_i.get());
_pitch_ctrl.set_k_ff(_param_fw_pr_ff.get());
_pitch_ctrl.set_integrator_max(_param_fw_pr_imax.get());
/* roll control parameters */
_roll_ctrl.set_time_constant(_param_fw_r_tc.get());
_roll_ctrl.set_k_p(_param_fw_rr_p.get());
_roll_ctrl.set_k_i(_param_fw_rr_i.get());
_roll_ctrl.set_k_ff(_param_fw_rr_ff.get());
_roll_ctrl.set_integrator_max(_param_fw_rr_imax.get());
/* yaw control parameters */
_yaw_ctrl.set_k_p(_param_fw_yr_p.get());
_yaw_ctrl.set_k_i(_param_fw_yr_i.get());
_yaw_ctrl.set_k_ff(_param_fw_yr_ff.get());
_yaw_ctrl.set_integrator_max(_param_fw_yr_imax.get());
/* wheel control parameters */
_wheel_ctrl.set_k_p(_param_fw_wr_p.get());
_wheel_ctrl.set_k_i(_param_fw_wr_i.get());
_wheel_ctrl.set_k_ff(_param_fw_wr_ff.get());
_wheel_ctrl.set_integrator_max(_param_fw_wr_imax.get());
_wheel_ctrl.set_max_rate(radians(_param_fw_w_rmax.get()));
return PX4_OK;
}更新控制模式，这些标志位会在下面的程序中用到
void
FixedwingAttitudeControl::vehicle_control_mode_poll()
{
_vcontrol_mode_sub.update(&_vcontrol_mode);
if (_vehicle_status.is_vtol) {
const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& !_vehicle_status.in_transition_mode;
const bool is_tailsitter_transition = _vehicle_status.in_transition_mode && _is_tailsitter;
if (is_hovering || is_tailsitter_transition) {
_vcontrol_mode.flag_control_attitude_enabled = false;
_vcontrol_mode.flag_control_manual_enabled = false;
}
}
}固定翼模式下根据遥控更新期望姿态
void
FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
{
const bool is_tailsitter_transition = _is_tailsitter && _vehicle_status.in_transition_mode;
const bool is_fixed_wing = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
if (_vcontrol_mode.flag_control_manual_enabled && (!is_tailsitter_transition || is_fixed_wing)) {自稳模式摇杆对应期望姿态角
// Always copy the new manual setpoint, even if it wasn't updated, to fill the _actuators with valid values
if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) {
if (!_vcontrol_mode.flag_control_climb_rate_enabled) {
if (_vcontrol_mode.flag_control_attitude_enabled) {
// STABILIZED mode generate the attitude setpoint from manual user inputs
_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
_att_sp.pitch_body = -_manual_control_setpoint.x * radians(_param_fw_man_p_max.get())
+ radians(_param_fw_psp_off.get());
_att_sp.pitch_body = constrain(_att_sp.pitch_body,
-radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));航向角只控制角速度，而不控制角度。所以期望的角度赋值为当前的偏航角，所以在自稳模式下解锁，移动机头方向，飞机不会自稳出舵，但是拨偏航摇杆方向舵是会出舵的，因为不管是自稳模式还是特技模式，偏航摇杆给的是都是期望角速度。
_att_sp.yaw_body = yaw_body; // yaw is not controlled, so set setpoint to current yaw期望油门直接根据摇杆输出
_att_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);发布
Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
q.copyTo(_att_sp.q_d);
_att_sp.timestamp = hrt_absolute_time();
_attitude_sp_pub.publish(_att_sp);
}在特技模式这种角速率控制模式下，摇杆对应的是期望的角速率
else if (_vcontrol_mode.flag_control_rates_enabled &&
!_vcontrol_mode.flag_control_attitude_enabled) {
// RATE mode we need to generate the rate setpoint from manual user inputs
_rates_sp.timestamp = hrt_absolute_time();
_rates_sp.roll = _manual_control_setpoint.y * radians(_param_fw_acro_x_max.get());
_rates_sp.pitch = -_manual_control_setpoint.x * radians(_param_fw_acro_y_max.get());
_rates_sp.yaw = _manual_control_setpoint.r * radians(_param_fw_acro_z_max.get());
_rates_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
_rate_sp_pub.publish(_rates_sp);
}在手动模式下，摇杆的值直接输出到混控
else {
/* manual/direct control */
_actuators.control[actuator_controls_s::INDEX_ROLL] =
_manual_control_setpoint.y * _param_fw_man_r_sc.get() + _param_trim_roll.get();
_actuators.control[actuator_controls_s::INDEX_PITCH] =
-_manual_control_setpoint.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get();
_actuators.control[actuator_controls_s::INDEX_YAW] =
_manual_control_setpoint.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get();
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(_manual_control_setpoint.z, 0.0f,
1.0f);
}
}
}
}
}更新期望姿态
void
FixedwingAttitudeControl::vehicle_attitude_setpoint_poll()
{
if (_att_sp_sub.update(&_att_sp)) {
_rates_sp.thrust_body[0] = _att_sp.thrust_body[0];
_rates_sp.thrust_body[1] = _att_sp.thrust_body[1];
_rates_sp.thrust_body[2] = _att_sp.thrust_body[2];
}
}更新期望的角速率
void
FixedwingAttitudeControl::vehicle_rates_setpoint_poll()
{
if (_rates_sp_sub.update(&_rates_sp)) {
if (_is_tailsitter) {
float tmp = _rates_sp.roll;
_rates_sp.roll = -_rates_sp.yaw;
_rates_sp.yaw = tmp;
}
}
}更新落地检测
void
FixedwingAttitudeControl::vehicle_land_detected_poll()
{
if (_vehicle_land_detected_sub.updated()) {
vehicle_land_detected_s vehicle_land_detected {};
if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
_landed = vehicle_land_detected.landed;
}
}
}根据空速计算控制器的缩放系数
float FixedwingAttitudeControl::get_airspeed_and_update_scaling()
{判断空速是否可用
_airspeed_validated_sub.update();
const bool airspeed_valid = PX4_ISFINITE(_airspeed_validated_sub.get().calibrated_airspeed_m_s)
&& (hrt_elapsed_time(&_airspeed_validated_sub.get().timestamp) < 1_s);如果空速不可用，就采用固定的巡航空速参数FW_AIRSPD_TRIM
// if no airspeed measurement is available out best guess is to use the trim airspeed
float airspeed = _param_fw_airspd_trim.get();如果空速可用，订阅空速数据
if ((_param_fw_arsp_mode.get() == 0) && airspeed_valid) {
/* prevent numerical drama by requiring 0.5 m/s minimal speed */
airspeed = math::max(0.5f, _airspeed_validated_sub.get().calibrated_airspeed_m_s);
}如果空速数据不可用，并且载具为垂起并在多旋翼模式，将空速设置为失速空速参数FW_AIRSPD_STALL
else {
// VTOL: if we have no airspeed available and we are in hover mode then assume the lowest airspeed possible
// this assumption is good as long as the vehicle is not hovering in a headwind which is much larger
// than the stall airspeed
if (_vehicle_status.is_vtol && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& !_vehicle_status.in_transition_mode) {
airspeed = _param_fw_airspd_stall.get();
}
}限幅在失速空速和最大空速之间
/*
* For scaling our actuators using anything less than the stall
* speed doesn't make any sense - its the strongest reasonable deflection we
* want to do in flight and it's the baseline a human pilot would choose.
*
* Forcing the scaling to this value allows reasonable handheld tests.
*/
const float airspeed_constrained = constrain(constrain(airspeed, _param_fw_airspd_stall.get(),
_param_fw_airspd_max.get()), 0.1f, 1000.0f);根据空速计算缩放值，这个缩放值在上面的姿态控制中有用到。因为空速越大，同样角度的舵面产生的力矩越大。
_airspeed_scaling = (_param_fw_arsp_scale_en.get()) ? (_param_fw_airspd_trim.get() / airspeed_constrained) : 1.0f;
return airspeed;
}姿态控制的主循环
void FixedwingAttitudeControl::Run()
{
if (should_exit()) {
_att_sub.unregisterCallback();
exit_and_cleanup();
return;
}状态计数器
perf_begin(_loop_perf);姿态更新的话就开始执行控制器
// only run controller if attitude changed
vehicle_attitude_s att;
if (_att_sub.update(&att)) {更新参数
// only update parameters if they changed
bool params_updated = _parameter_update_sub.updated();
// check for parameter updates
if (params_updated) {
// clear update
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
// update parameters from storage
updateParams();
parameters_update();
}当前姿态更新的时间减去上一次运行的时间得到距离上一次的时间间隔
const float dt = math::constrain((att.timestamp - _last_run) * 1e-6f, 0.002f, 0.04f);
_last_run = att.timestamp;更新姿态和角速率
/* get current rotation matrix and euler angles from control state quaternions */
matrix::Dcmf R = matrix::Quatf(att.q);
vehicle_angular_velocity_s angular_velocity{};
_vehicle_rates_sub.copy(&angular_velocity);
float rollspeed = angular_velocity.xyz[0];
float pitchspeed = angular_velocity.xyz[1];
float yawspeed = angular_velocity.xyz[2];尾座式垂起的飞机是垂直向上放置的，控制的时候需要作一下转换
if (_is_tailsitter) {
/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
*
* Since the VTOL airframe is initialized as a multicopter we need to
* modify the estimated attitude for the fixed wing operation.
* Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around
* the pitch axis compared to the neutral position of the vehicle in multicopter mode
* we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix.
* Additionally, in order to get the correct sign of the pitch, we need to multiply
* the new x axis of the rotation matrix with -1
*
* original: modified:
*
* Rxx Ryx Rzx -Rzx Ryx Rxx
* Rxy Ryy Rzy -Rzy Ryy Rxy
* Rxz Ryz Rzz -Rzz Ryz Rxz
* */
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 = rollspeed;
rollspeed = -yawspeed;
yawspeed = helper;
}旋转矩阵转欧拉角
const matrix::Eulerf euler_angles(R);获取期望姿态
vehicle_attitude_setpoint_poll();载具状态更新
// vehicle status update must be before the vehicle_control_mode_poll(), otherwise rate sp are not published during whole transition
_vehicle_status_sub.update(&_vehicle_status);控制模式更新
vehicle_control_mode_poll();更新手动遥控下的期望姿态（定点/定高等模式的期望姿态不在这里更新，而在L1和TECS控制器中更新）
vehicle_manual_poll(euler_angles.psi());落地检测
vehicle_land_detected_poll();// the position controller will not emit attitude setpoints in some modes
// we need to make sure that this flag is reset
_att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled;判断是否控制滑行轮
bool wheel_control = false;
// TODO: manual wheel_control on ground?
if (_param_fw_w_en.get() && _att_sp.fw_control_yaw) {
wheel_control = true;
}在未使能角速率控制，多旋翼状态（非垂起过渡阶段和尾座式垂起）或者两次运行时间间隔过大的情况下不积分
// lock integrator if no rate control enabled, or in RW mode (but not transitioning VTOL or tailsitter), or for long intervals (> 20 ms)
bool lock_integrator = !_vcontrol_mode.flag_control_rates_enabled
|| (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
!_vehicle_status.in_transition_mode && !_is_tailsitter)
|| (dt > 0.02f);/* if we are in rotary wing mode, do nothing */
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.is_vtol) {
perf_end(_loop_perf);
return;
}襟翼控制
control_flaps(dt);更新空速和缩放系数
/* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_rates_enabled) {
const float airspeed = get_airspeed_and_update_scaling();需要时重置积分
/* reset integrals where needed */
if (_att_sp.roll_reset_integral) {
_roll_ctrl.reset_integrator();
}
if (_att_sp.pitch_reset_integral) {
_pitch_ctrl.reset_integrator();
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}在地面时重置积分
/* Reset integrators if the aircraft is on ground
* or a multicopter (but not transitioning VTOL or tailsitter)
*/
if (_landed
|| (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& !_vehicle_status.in_transition_mode && !_is_tailsitter)) {
_roll_ctrl.reset_integrator();
_pitch_ctrl.reset_integrator();
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}赋值输入
/* Prepare data for attitude controllers */
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 = rollspeed;
control_input.body_y_rate = pitchspeed;
control_input.body_z_rate = 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 = _param_fw_airspd_stall.get();
control_input.airspeed_max = _param_fw_airspd_max.get();
control_input.airspeed = airspeed;
control_input.scaler = _airspeed_scaling;
control_input.lock_integrator = lock_integrator;如果需要控制车轮保持滑行时的航向，需要计算地速，根据地速计算出控制车轮转向的缩放系数，因为地速越快，轮子需要的转向幅度越小。
if (wheel_control) {
_local_pos_sub.update(&_local_pos);
/* Use stall airspeed to calculate ground speed scaling region.
* Don't scale below gspd_scaling_trim
*/
float groundspeed = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy);
float gspd_scaling_trim = (_param_fw_airspd_stall.get());
control_input.groundspeed = groundspeed;
if (groundspeed > gspd_scaling_trim) {
control_input.groundspeed_scaler = gspd_scaling_trim / groundspeed;
} else {
control_input.groundspeed_scaler = 1.0f;
}
}根据控制模式设置限幅参数
/* reset body angular rate limits on mode change */
if ((_vcontrol_mode.flag_control_attitude_enabled != _flag_control_attitude_enabled_last) || params_updated) {
if (_vcontrol_mode.flag_control_attitude_enabled
|| _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
_roll_ctrl.set_max_rate(radians(_param_fw_r_rmax.get()));
_pitch_ctrl.set_max_rate_pos(radians(_param_fw_p_rmax_pos.get()));
_pitch_ctrl.set_max_rate_neg(radians(_param_fw_p_rmax_neg.get()));
_yaw_ctrl.set_max_rate(radians(_param_fw_y_rmax.get()));
} else {
_roll_ctrl.set_max_rate(radians(_param_fw_acro_x_max.get()));
_pitch_ctrl.set_max_rate_pos(radians(_param_fw_acro_y_max.get()));
_pitch_ctrl.set_max_rate_neg(radians(_param_fw_acro_y_max.get()));
_yaw_ctrl.set_max_rate(radians(_param_fw_acro_z_max.get()));
}
}_flag_control_attitude_enabled_last = _vcontrol_mode.flag_control_attitude_enabled;计算控制飞机沿巡航速度飞行所需的舵量配平值
/* bi-linear interpolation over airspeed for actuator trim scheduling */
float trim_roll = _param_trim_roll.get();
float trim_pitch = _param_trim_pitch.get();
float trim_yaw = _param_trim_yaw.get();当空速小于巡航速度时，计算负的配平值，已使飞机能够提升速度
if (airspeed < _param_fw_airspd_trim.get()) {
trim_roll += gradual(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_r_vmin.get(),
0.0f);
trim_pitch += gradual(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_p_vmin.get(),
0.0f);
trim_yaw += gradual(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_y_vmin.get(),
0.0f);当空速大于巡航速度时，计算正的配平值，已使飞机能够降低速度
} else {
trim_roll += gradual(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
_param_fw_dtrim_r_vmax.get());
trim_pitch += gradual(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
_param_fw_dtrim_p_vmax.get());
trim_yaw += gradual(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
_param_fw_dtrim_y_vmax.get());
}用到襟翼的情况下，将襟翼的控制量乘系数加到副翼的控制上
/* add trim increment if flaps are deployed */
trim_roll += _flaps_applied * _param_fw_dtrim_r_flps.get();
trim_pitch += _flaps_applied * _param_fw_dtrim_p_flps.get();roll/pitch姿态控制
/* Run attitude controllers */
if (_vcontrol_mode.flag_control_attitude_enabled) {
if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) {
_roll_ctrl.control_attitude(dt, control_input);
_pitch_ctrl.control_attitude(dt, control_input);控制滑行轮，滑行轮的控制与舵面控制类似，采用前馈+PI控制，采用基于地速的缩放系数，这里不介绍。
if (wheel_control) {
_wheel_ctrl.control_attitude(dt, control_input);
_yaw_ctrl.reset_integrator();
}航向角的控制采用协调转弯，基于roll/pitch，所以最后控制
else {
// runs last, because is depending on output of roll and pitch attitude
_yaw_ctrl.control_attitude(dt, control_input);
_wheel_ctrl.reset_integrator();
}更新期望角速率
/* Update input data for rate controllers */
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();如果需要自动调参，调参所需要的期望角速率控制指令通过前馈的形式加到角速率控制器上
const hrt_abstime now = hrt_absolute_time();
autotune_attitude_control_status_s pid_autotune;
matrix::Vector3f bodyrate_ff;
if (_autotune_attitude_control_status_sub.copy(&pid_autotune)) {
if ((pid_autotune.state == autotune_attitude_control_status_s::STATE_ROLL
|| pid_autotune.state == autotune_attitude_control_status_s::STATE_PITCH
|| pid_autotune.state == autotune_attitude_control_status_s::STATE_YAW
|| pid_autotune.state == autotune_attitude_control_status_s::STATE_TEST)
&& ((now - pid_autotune.timestamp) < 1_s)) {
bodyrate_ff = matrix::Vector3f(pid_autotune.rate_sp);
}
}内环角速率控制，输出roll_u加上上面的配平值，给混控
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
float roll_u = _roll_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(0));
_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;判断值是否可用，不可用的话重置积分
if (!PX4_ISFINITE(roll_u)) {
_roll_ctrl.reset_integrator();
}pith角速率控制，同上
float pitch_u = _pitch_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(1));
_actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch;
if (!PX4_ISFINITE(pitch_u)) {
_pitch_ctrl.reset_integrator();
}偏航角速率控制，分为地面的滑行轮控制和控制的方向舵控制
float yaw_u = 0.0f;
if (wheel_control) {
yaw_u = _wheel_ctrl.control_bodyrate(dt, control_input);
}
else {
yaw_u = _yaw_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(2));
}_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;手动遥控时偏航通道加上偏航摇杆的控制
/* add in manual rudder control in manual modes */
if (_vcontrol_mode.flag_control_manual_enabled) {
_actuators.control[actuator_controls_s::INDEX_YAW] += _manual_control_setpoint.r;
}判断值是否可用，不可用的话重置积分
if (!PX4_ISFINITE(yaw_u)) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}推力控制
/* throttle passed through if it is finite and if no engine failure was detected */
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(_att_sp.thrust_body[0])
&& !_vehicle_status.engine_failure) ? _att_sp.thrust_body[0] : 0.0f;如果使能了电池缩放，将推力输出乘电池的缩放系数
/* scale effort by battery status */
if (_param_fw_bat_scale_en.get() &&
_actuators.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) {
if (_battery_status_sub.updated()) {
battery_status_s battery_status{};
if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) {
_battery_scale = battery_status.scale;
}
}
_actuators.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_scale;
}
}发布期望角速率用于日志分析
/*
* Lazily publish the rate setpoint (for analysis, the actuators are published below)
* only once available
*/
_rates_sp.roll = _roll_ctrl.get_desired_bodyrate();
_rates_sp.pitch = _pitch_ctrl.get_desired_bodyrate();
_rates_sp.yaw = _yaw_ctrl.get_desired_bodyrate();
_rates_sp.timestamp = hrt_absolute_time();
_rate_sp_pub.publish(_rates_sp);
}只控制角速率的情况下，执行下面的程序
更新机体坐标系下的期望角速率
else {
vehicle_rates_setpoint_poll();
_roll_ctrl.set_bodyrate_setpoint(_rates_sp.roll);
_yaw_ctrl.set_bodyrate_setpoint(_rates_sp.yaw);
_pitch_ctrl.set_bodyrate_setpoint(_rates_sp.pitch);执行机体坐标系下的角速率控制器，并输出到混控
float roll_u = _roll_ctrl.control_bodyrate(dt, control_input);
_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
float pitch_u = _pitch_ctrl.control_bodyrate(dt, control_input);
_actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch;
float yaw_u = _yaw_ctrl.control_bodyrate(dt, control_input);
_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ?
_rates_sp.thrust_body[0] : 0.0f;
}发布状态
rate_ctrl_status_s rate_ctrl_status{};
rate_ctrl_status.timestamp = hrt_absolute_time();
rate_ctrl_status.rollspeed_integ = _roll_ctrl.get_integrator();
rate_ctrl_status.pitchspeed_integ = _pitch_ctrl.get_integrator();
if (wheel_control) {
rate_ctrl_status.additional_integ1 = _wheel_ctrl.get_integrator();
} else {
rate_ctrl_status.yawspeed_integ = _yaw_ctrl.get_integrator();
}
_rate_ctrl_status_pub.publish(rate_ctrl_status);
}将横滚控制输出的前馈添加到偏航控制输出，这可以用来抵消飞机滚转时的偏航减弱效应
// Add feed-forward from roll control output to yaw control output
// This can be used to counteract the adverse yaw effect when rolling the plane
_actuators.control[actuator_controls_s::INDEX_YAW] += _param_fw_rll_to_yaw_ff.get()
* constrain(_actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f);襟翼控制
_actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied;5通道映射到遥控的辅助通道1，通道6为空气制动器控制，7通道映射到遥控的辅助通道3
_actuators.control[5] = _manual_control_setpoint.aux1;
_actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied;
// FIXME: this should use _vcontrol_mode.landing_gear_pos in the future
_actuators.control[7] = _manual_control_setpoint.aux3;发布控制输出和状态
/* lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = att.timestamp;
/* Only publish if any of the proper modes are enabled */
if (_vcontrol_mode.flag_control_rates_enabled ||
_vcontrol_mode.flag_control_attitude_enabled ||
_vcontrol_mode.flag_control_manual_enabled) {
_actuators_0_pub.publish(_actuators);
if (!_vehicle_status.is_vtol) {
publishTorqueSetpoint(angular_velocity.timestamp_sample);
publishThrustSetpoint(angular_velocity.timestamp_sample);
}
}
updateActuatorControlsStatus(dt);
}
perf_end(_loop_perf);
}发布消息
void FixedwingAttitudeControl::publishTorqueSetpoint(const hrt_abstime ×tamp_sample)
{
vehicle_torque_setpoint_s v_torque_sp = {};
v_torque_sp.timestamp = hrt_absolute_time();
v_torque_sp.timestamp_sample = timestamp_sample;
v_torque_sp.xyz[0] = _actuators.control[actuator_controls_s::INDEX_ROLL];
v_torque_sp.xyz[1] = _actuators.control[actuator_controls_s::INDEX_PITCH];
v_torque_sp.xyz[2] = _actuators.control[actuator_controls_s::INDEX_YAW];
_vehicle_torque_setpoint_pub.publish(v_torque_sp);
}发布消息
void FixedwingAttitudeControl::publishThrustSetpoint(const hrt_abstime ×tamp_sample)
{
vehicle_thrust_setpoint_s v_thrust_sp = {};
v_thrust_sp.timestamp = hrt_absolute_time();
v_thrust_sp.timestamp_sample = timestamp_sample;
v_thrust_sp.xyz[0] = _actuators.control[actuator_controls_s::INDEX_THROTTLE];
v_thrust_sp.xyz[1] = 0.0f;
v_thrust_sp.xyz[2] = 0.0f;
_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
}襟翼控制
void FixedwingAttitudeControl::control_flaps(const float dt)
{
/* default flaps to center */
float flap_control = 0.0f;直接通过遥控控制
/* map flaps by default to manual if valid */
if (PX4_ISFINITE(_manual_control_setpoint.flaps) && _vcontrol_mode.flag_control_manual_enabled
&& fabsf(_param_fw_flaps_scl.get()) > 0.01f) {
flap_control = _manual_control_setpoint.flaps * _param_fw_flaps_scl.get();
}自动控制
else if (_vcontrol_mode.flag_control_auto_enabled
&& fabsf(_param_fw_flaps_scl.get()) > 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 * _param_fw_flaps_scl.get() * _param_fw_flaps_lnd_scl.get();
break;起飞时的襟翼控制量，直接根据参数控制
case vehicle_attitude_setpoint_s::FLAPS_TAKEOFF:
flap_control = 1.0f * _param_fw_flaps_scl.get() * _param_fw_flaps_to_scl.get();
break;
}
}随时间连续增加实际控制值，在1秒内完成襟翼全行程控制
// move the actual control value continuous with time, full flap travel in 1sec
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 */
if (PX4_ISFINITE(_manual_control_setpoint.aux2) && _vcontrol_mode.flag_control_manual_enabled
&& fabsf(_param_fw_flaperon_scl.get()) > 0.01f) {
flaperon_control = 0.5f * (_manual_control_setpoint.aux2 + 1.0f) * _param_fw_flaperon_scl.get();
} else if (_vcontrol_mode.flag_control_auto_enabled
&& fabsf(_param_fw_flaperon_scl.get()) > 0.01f) {
if (_att_sp.apply_flaps == vehicle_attitude_setpoint_s::FLAPS_LAND) {
flaperon_control = _param_fw_flaperon_scl.get();
} else {
flaperon_control = 0.0f;
}
}控制量在1秒内达到
// move the actual control value continuous with time, full flap travel in 1sec
if (fabsf(_flaperons_applied - flaperon_control) > 0.01f) {
_flaperons_applied += (_flaperons_applied - flaperon_control) < 0 ? dt : -dt;
} else {
_flaperons_applied = flaperon_control;
}
}
