PX4 固定翼姿态控制代码Run函数

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的控制思路都是一样的

5.yaw通道的控制有所不同