PX4-固定翼的姿态控制

下面分析代码的版本是v1.8.2

1 参数介绍

固定翼中有很多参数，理解这些参数的含义非常重要

FW_AIRSPD_TRIM 巡航状态下的空速 15m/s
FW_AIRSPD_MIN 最小空速  10m/s
FW_AIRSPD_MAX 最大空速  20m/s
gspd_scaling_trim 
FW_R_TC 外环控制器的比例参数，这只是一个初始值
FW_RR_P 内环控制器的比例参数
FW_RR_I 内环控制器的积分参数
FW_RR_IMAX 限制内环积分的最大值，可以抵抗风？
FW_R_RMAX 设定角速度的最大值
FW_RLL_TO_YAW_FF 横滚对偏航的前馈增益 这个参数乘于横滚后直接加载偏航的输出上
FW_RR_FF 从速率设定值直接前馈到控制表面输出。使用此方法可以在不引入噪声放大的情况下，获得更紧凑的控制器响应。
FW_RSP_OFF 期望角度的偏置值，可以用来抵消飞机的不平衡
FW_MAN_R_SC 手动模式下的缩放值，这个值乘于摇杆输出后直接作用在执行器上
FW_MAN_R_MAX 自稳模式下，期望角度的最大值，直接乘于摇杆的输入就可以得到期望角度了
FW_ACRO_X_MAX 特技模式下，期望角速度的最大值，直接乘于摇杆的输入就得到期望的角速度
FW_DTRIM_R_VMIN 最小空速时，用来计算Roll增量的参数
FW_DTRIM_R_VMAX 最大空速时，用于计算Roll增量的参数
FW_DTRIM_R_FLPS 襟翼完全展开时，用于计算Rol增量的参数

FW_ARSP_MODE 空速计模式的选择，0为启用空速计，1为禁用
FW_BAT_SCALE_EN 是否根据电池情况来缩放油门的输入量
FW_RATT_TH Rattitude模式下，手动输入的最大值

 2 源码分析

run函数 结合源代码阅读最佳，源码太多，不直接粘贴了，文件路径为Firmware/src/modules/fw_att_control/

run
前面部分和其他函数都一样，订阅大量的主题，更新参数。最后一个fds订阅的是姿态，只要有新姿态
就开始进行控制。
如果有新的消息推送，记录下控制的时间差
如果是垂直起飞的固定翼，比如TAILSITTER: 垂直起降的固定翼，需要转换横滚和偏航
将四元数转换成欧拉角，更新消息，调用vehicle_manual_poll函数
如果不允许控制偏航，或者是除于自动飞行过的模式，禁止Yaw的控制
初始化积分锁
control_flaps 对副翼控制并控制襟翼
下面进入角速度的控制(如果在手动模式或者自稳模式下)：
	判断空速计是否能用，并得到空速
	计算空速的缩放值，用于姿态的控制
	gspd_scaling_trim设定为0.6倍的空速，用在起飞轮的控制上
	判断积分是否需要重置？需要的话清0
	准备好控制需要用到的数据，角度、角速度、期望角度(从vehicle_manual_poll计算得到)
	如果是从别的控制模式切换过来的，重置下最大的角速度和(姿态控制模式和自动模式分开设置)	
	trim_roll等等是干嘛用的？
	是否允许角度控制，允许的话：
		通过串级PID来控制角度，最后的计算量直接作为输出，其中油门的期望值来自于摇杆
		如果开启了电池补偿，则对油门输出进行修改
		最后把期望的角速度发布出去
	否则就是纯角速度控制
		进行内环的角度控制
	发布一些控制信息
加上横滚对于偏航的前向增益，可以用来抵消横滚是对于偏航的不利影响。
就是对偏航的输出加上一个横滚输出带来的增益这么简单？
随后整理控制器的输出发布出去

 vehicle_manual_poll 函数
判断是否为手动控制模式，接受到新的manual_control_setpoint，就进行控制
只有当符合控制条件，rattitude模式下，手动输入不超过阈值；不允许爬升控制，板外控制
手动控制模式下支持姿态控制则，摇杆输入的就是姿态期望值，支持角速度控制的话，摇杆
输入量就是角速度期望值。否则的话摇杆的输入量就直接作为输出量

void
FixedwingAttitudeControl::vehicle_manual_poll()
{
	// only update manual if in a manual mode
	if (_vcontrol_mode.flag_control_manual_enabled) {

		// Always copy the new manual setpoint, even if it wasn't updated, to fill the _actuators with valid values
		if (orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual) == PX4_OK) {

			// Check if we are in rattitude mode and the pilot is above the threshold on pitch
			if (_vcontrol_mode.flag_control_rattitude_enabled) {
				if (fabsf(_manual.y) > _parameters.rattitude_thres || fabsf(_manual.x) > _parameters.rattitude_thres) {
					_vcontrol_mode.flag_control_attitude_enabled = false;
				}
			}

			if (!_vcontrol_mode.flag_control_climb_rate_enabled &&
			    !_vcontrol_mode.flag_control_offboard_enabled) {

				if (_vcontrol_mode.flag_control_attitude_enabled) {
					// STABILIZED mode generate the attitude setpoint from manual user inputs
					_att_sp.timestamp = hrt_absolute_time();
					_att_sp.roll_body = _manual.y * _parameters.man_roll_max + _parameters.rollsp_offset_rad;
					_att_sp.roll_body = math::constrain(_att_sp.roll_body, -_parameters.man_roll_max, _parameters.man_roll_max);
					_att_sp.pitch_body = -_manual.x * _parameters.man_pitch_max + _parameters.pitchsp_offset_rad;
					_att_sp.pitch_body = math::constrain(_att_sp.pitch_body, -_parameters.man_pitch_max, _parameters.man_pitch_max);
					_att_sp.yaw_body = 0.0f;
					_att_sp.thrust = _manual.z;

					Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
					q.copyTo(_att_sp.q_d);
					_att_sp.q_d_valid = true;

					if (_attitude_sp_pub != nullptr) {
						/* publish the attitude rates setpoint */
						orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &_att_sp);

					} else if (_attitude_setpoint_id) {
						/* advertise the attitude rates setpoint */
						_attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &_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.y * _parameters.acro_max_x_rate_rad;
					_rates_sp.pitch = -_manual.x * _parameters.acro_max_y_rate_rad;
					_rates_sp.yaw = _manual.r * _parameters.acro_max_z_rate_rad;
					_rates_sp.thrust = _manual.z;

					if (_rate_sp_pub != nullptr) {
						/* publish the attitude rates setpoint */
						orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp);

					} else if (_rates_sp_id) {
						/* advertise the attitude rates setpoint */
						_rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
					}

				} else {
					/* manual/direct control */
					_actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y * _parameters.man_roll_scale + _parameters.trim_roll;
					_actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x * _parameters.man_pitch_scale +
							_parameters.trim_pitch;
					_actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r * _parameters.man_yaw_scale + _parameters.trim_yaw;
					_actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z;
				}
			}
		}
	}
}

 3 串级PID内外环的控制

control_attitude外环控制，外环就是一个纯比例，比例的增益为控制时间间隔

float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data)
{
	/* Do not calculate control signal with bad inputs */
	if (!(ISFINITE(ctl_data.roll_setpoint) && ISFINITE(ctl_data.roll))) {
		return _rate_setpoint;
	}

	/* Calculate error */
	float roll_error = ctl_data.roll_setpoint - ctl_data.roll;

	/* Apply P controller */
	_rate_setpoint = roll_error / _tc;

	return _rate_setpoint;
}

内环控制，内环是控制角速度，在Roll的内环控制之前最期望的角速度做了一个变换，为什么要这样还不懂

float ECL_RollController::control_euler_rate(const struct ECL_ControlData &ctl_data)
{
	/* Transform setpoint to body angular rates (jacobian) */
	_bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;

	set_bodyrate_setpoint(_bodyrate_setpoint);

	return control_bodyrate(ctl_data);

}

角速度控制，

判断控制量是否有效，无效的话输出上次的控制量
获得控制周期，如果控制周期太长，积分上锁，不允许积分
计算这次控制的积分增量，如果上次输出已经打到最大了，积分增量的符号只能与之相反
内环的计算
前向增益直接乘于期望速度，为什么不是乘于角速度的误差？
内环比例为什么要乘于缩放值的平方？缩放值是空速的缩放值

这里内环的计算公式不太懂，值得再研究

float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
	/* Do not calculate control signal with bad inputs */
	if (!(ISFINITE(ctl_data.pitch) &&
	      ISFINITE(ctl_data.body_x_rate) &&
	      ISFINITE(ctl_data.body_z_rate) &&
	      ISFINITE(ctl_data.yaw_rate_setpoint) &&
	      ISFINITE(ctl_data.airspeed_min) &&
	      ISFINITE(ctl_data.airspeed_max) &&
	      ISFINITE(ctl_data.scaler))) {
		return math::constrain(_last_output, -1.0f, 1.0f);
	}

	/* get the usual dt estimate */
	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
	_last_run = ecl_absolute_time();
	float dt = (float)dt_micros * 1e-6f;

	/* lock integral for long intervals */
	bool lock_integrator = ctl_data.lock_integrator;

	if (dt_micros > 500000) {
		lock_integrator = true;
	}

	/* Calculate body angular rate error */
	_rate_error = _bodyrate_setpoint - ctl_data.body_x_rate; //body angular rate error

	if (!lock_integrator && _k_i > 0.0f) {

		float id = _rate_error * dt * 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);
		}

		/* add and constrain */
		_integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max);
	}

	/* Apply PI rate controller and store non-limited output */
	_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
		       _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
		       + _integrator;  //scaler is proportional to 1/airspeed

	return math::constrain(_last_output, -1.0f, 1.0f);
}