控制逻辑

襟翼控制

1
2
3

flap_control=0.5*(1+manual.flap) * flap_scale
//自动起飞、降落：
flap_control = 1*flap_scale*land_scale(takeoff_scale)

襟副翼控制

1
2
3

flaperon_control=0.5*(1+manual.aux2) * flaperon_scale
//自动降落：
flaperon_control=1*flaperon_scale

角度控制—得到角速度值

滚转控制

$\dot{\phi}_{s p}=\left(\phi_{s p}-\phi\right) / t_c$

1 2	roll_error = ctl_data.oll_setpoint ctl_data.roll rate_setpoint = roll_error / _tc【_tc(0.1f)】

俯仰控制

$\dot{\theta}_{s p}=\left(\theta_{s p}-\theta\right) / t_c$

1 2	pitch_error = ctl_data.pitch_setpoint ctl_data.pitch rate_setpoint = pitch_error / _tc

偏航控制【协调转弯】

根据滚转角得到偏航角速度：

$\dot{\psi}_{s p}=\frac{g}{U} * \tan (\varphi) * \cos (\theta)$

1 2	rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / ctl_data.airspeed; //其中，constrained_roll为roll的限幅值0~80、100~180，避免roll=90°

偏航角速度为0：
$\dot{\psi}_{s p}=0$

轮子【地面】:

$\dot{\psi}_{s p}=\left( {\psi}_{s p}- {\psi}\right) / {t}_c$

1 2	yaw_error = wrap_pi(ctl_data.yaw_setpoint ctl_data.yaw); rate_setpoint = yaw_error / _tc;

角速度控制—得到舵量

滚转控制

坐标转换：

$p_{s p}=\dot{\phi}_{s p}-\dot{\psi}_{s p} \sin \theta$

1	roll_bodyrate_setpoint = ctl_data.roll_rate_setpoint sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;

PI控制：

$\Delta p=p_{s p}-p \newline integrator = integrator +K_i * \Delta p * d t * IAS_{scaler} \newline \delta a=p_{s p} * K_{f f} * IAS_{scaler} +IAS_{scaler}^2 * K_p * \Delta p+ integrator$

roll_rate_error = roll_bodyrate_setpoint ctl_data.body_x_rate;
    
id = _rate_error * dt * ctl_data.scaler;
_integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max);

_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +_rate_error * _k_p * ctl_data.scaler * ctl_data.scaler+ _integrator;

俯仰控制

坐标转换：

$q_{s p}=\dot{\theta}_{s p} \cos \phi+\dot{\psi}_{s p} \cos \theta \sin \phi$

1	pitch_bodyrate_setpoint = cosf(ctl_data.roll) * pitch_rate_setpoint + cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;

PI控制：

$\Delta q=q_{s p}-q \newline integrator = integrator +K_i * \Delta q * d t * IAS_{scaler} \newline \delta e=q_{s p} * K_{f f} * IAS_{scaler} +IAS_{scaler}^2 * K_p * \Delta q+ integrator$

pitch_rate_error = pitch_bodyrate_setpoint ctl_data.body_y_rate;

id = _rate_error * dt * ctl_data.scaler;
_integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max);

_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +_rate_error * _k_p * ctl_data.scaler * ctl_data.scaler+ _integrator;

轮子控制【判断】

$\Delta \dot \psi=\dot \psi_{s p}-r \newline integrator = integrator +K_i * \Delta \dot \psi * d t * IAS_{scaler} \newline \delta r=\Delta \dot \psi_{s p} * K_{f f} * GS_{scaler} + GS_{scaler}^2 * (K_p * \Delta \dot \psi+ integrator)$

yaw_rate_error = yaw_bodyrate_setpoint ctl_data.body_z_rate;

id = yaw_rate_error * dt * ctl_data.groundspeed_scaler;

_integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max);

_last_output = yaw_rate_setpoint * _k_ff * ctl_data.groundspeed_scaler + ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler * (yaw_rate_error * _k_p + _integrator);

偏航控制【判断非轮子控制】

坐标转换：

$r_{s p}=-\dot{\theta}_{s p} \sin \phi+\dot{\psi}_{s p} \cos \theta \cos \phi$

1	yaw_bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint + cosf(ctl_data.roll) * cosf(ctl_data.pitch) * yaw_rate_setpoint;

空速设置【未使用】：

airspeed=ctl_data.airspeed;
//如果(空速无穷大)
airspeed=0.5*(ctl_data.airspeed_min + ctl_data.airspeed_max);
//否则如果(空速<最小速度)
airspeed=ctl_data.airspeed_min;

PI控制：

$\Delta r=r_{s p}-r \newline integrator = integrator +K_i * \Delta r * d t * IAS_{scaler} \newline \delta r=GS_{scaler}^2 * (r_{s p} * K_{f f} + K_p * \Delta r+ integrator)$

yaw_rate_error = yaw_bodyrate_setpoint ctl_data.body_z_rate;

id = _rate_error * dt;
integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max);

last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + _integrator) * ctl_data.scaler * ctl_data.scaler;

代码流程

FixedwingAttitudeControl::run()

订阅消息
_att_sub姿态
循环

如果参数更新，则复制参数【orb_check】

等待数据att_sub，超时则continue;错误则报错并continue【px4_warn】

perf_begin(_loop_perf)开始

如果姿态改变，进入下面

1.时间、步长更新，如果步长>1，dt=0.01
2.复制att_sub信息
3.计算四元素、DCM

计算姿态转换矩阵DCM等【R包括欧拉角、四元素、旋转矩阵】
如果是尾座式无人机，根据坐标变换DCM

4.更新信息

vehicle_attitude_setpoint_poll();
vehicle_attitude_setpoint_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
global_pos_poll();
vehicle_status_poll();
vehicle_land_detected_poll();

5.状态确认

6.襟翼控制control_flaps(deltaT)

7.如果(control_rates_enabled)

①空速更新
②地速计算
③准备工作【积分清零等】
④姿态控制【判断是否进入】
⑤角速度控制
⑥发布

8.输出调整

perf_begin(_loop_perf)开始