Run函数
1. 何时运行
// 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();
运行的逻辑是当姿态att消息改变之后 就运行
姿态时刻在改变,姿态数据的采集、发布到uorb上
fw_att_control是一个单独的线程,监控着姿态数据并运行
具体运行的频率这还真不知道
但这个频率应该是相对稳定的 我只需要一个相对稳定的控制频率来做一件事
就是记录0.5s左右的一段时间的俯仰角的值
我会把这段内容保存为一个fifo的队列
不断刷新
2. 数据来源
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];
const matrix::Eulerf euler_angles(R);
dt是这次控制与last_run的时间差,2-40ms
从四元数获得一个旋转矩阵R
拉取角速度数据
从旋转矩阵R获得欧拉角
姿态控制最终还是控制欧拉角
vehicle_attitude_setpoint_poll();
拉取姿态setpoint的结果 这个结果是通过uorb得到的,那我的疑问是这个结果是哪里发布的呢?
首先这里的姿态setpoint是级联pid以外的部分
固定翼控制根据的是L1和tecs 前者决定一个横滚期望roll sp
后者决定油门和picth sp
rollsp和pitchsp就是固定翼姿态控制的目标
所以说思考一个问题,全自动即mission模式 和 手飞的stablized模式的区别
就在于这里sp的来源 mission模式的期望姿态是程序计算的(根据航迹)
手飞的stablized的sp是遥控器来的
后续的姿态控制pid就是自稳的效果
在同文件夹下的hpp中可以找到
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub;
那就可以知道是在哪里 说明时候发布的姿态期望sp
FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
在这个位置可以找到att_sp发布的过程
有一行注释写的很明白
// STABILIZED mode generate the attitude setpoint from manual user inputs
自稳模式的attsp就是从遥控器输入的
(这里我先不找mission模式的attsp在哪里发布了,我猜测是在l1模块和tecs模块发布的)
3. 判断自稳还是纯手动
/* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_rates_enabled) {
// 自稳模式
// 这里执行自稳模式下的代码,例如调整姿态控制或速率控制
} else {
// 纯手动模式
// 这里执行纯手动模式下的代码,例如直接使用飞行员的输入控制控制面
}
这里进行了分支 到这一步自稳和mission是一样的,纯手动是另一种
我们要改的肯定也是自稳和mission这一个分支 改好了无论是自稳还是程控都是可以一起使用的
看第一个分支,将所有的数据,控制需要的数据打包成ECL_ControlData
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;
wheel_control和在mode转换时reset body rate limits不需要看
4.姿态控制核心
/* 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);
if (wheel_control) {
_wheel_ctrl.control_attitude(dt, control_input);
_yaw_ctrl.reset_integrator();
//PX4_INFO("diyigefenzhi");
//起飞阶段走这个分支
} else {
// runs last, because is depending on output of roll and pitch attitude
//这里说yaw控制器是最后运行的 因为yaw取决于roll和pitch控制器的输出
_yaw_ctrl.control_attitude(dt, control_input);
_wheel_ctrl.reset_integrator();
//PX4_INFO("222222222222222222222");
//起飞结束飞行中走这个分支
}
需要仿真判断一下我们的飞机起飞会过这个wheel_control吗?我看到的是fw_w_en表示wheelcontrol不开启 那么无论是起飞还是起飞结束飞机都不会走前面这个分支
在gazebo仿真中sitl会走第一个分支wheel control类似滑跑起飞
但是在我们的飞机中确定是不走上面这条的
看下面这条
所以运行核心姿态控制的流程是
roll、pitch
yaw
这里的做法说明yawcontroller的思路和别的是不一样的yaw的setpoint取决于roll和pitch控制器的输出
但是问题是混控器只接受 roll和pitch两个通道
yaw的输出用在哪里呢?
这个问题还没有解决
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;
control_euler_rate的具体代码
float ECL_PitchController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff)
{
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint + bodyrate_ff;
set_bodyrate_setpoint(_bodyrate_setpoint);
return control_bodyrate(dt, ctl_data);
}
很有必要看看这里做了几件事情
首先 control_attitude函数用期望欧拉角计算出一个pitch_error再计算rate_setpoint
rate_setpoint也就是一个角速度的期望(比方说俯仰这个通道上的角速度)
固定翼飞机控制姿态的思路不是直接控制俯仰角 而是控制俯仰角速度(准确地说是机身y轴上的角速度,但其实这俩基本差不多)
这样设计是合理的,比如说我上一层控制给出的期望pitch是30,虽然我现在只有20但是我以一个很大的角速度pitchspeed在调整姿态,那需要做的应该是把舵面往下打抑制上翘的趋势,而不是继续往上打,那样会导致超调
所以这里的做法是姿态控制的内环,根据rate_setpoint期望角速度和当前角速度做一个pid,来给出执行器通道的控制值
那来看control_euler_rate就很清楚了
第一步是把欧拉角速度都转到机体系下来算body angular rates(同时后面陀螺仪测出的状态也是这个东西)
第二步set
第三步 调用control_bodyrate 进去看一下就是计算角速度的误差 然后做pid给出output 还做了一些限制
pitch和roll的控制思路都是一样的