PX4从放弃到精通（二十八）：垂起过渡控制-CFANZ编程社区

文章目录

前言
一、主程序
二、update_transition_state（）
三、update_transition_state（）

前言

固件版本：1.14.0
可加名片交流学习

一、主程序

代码位置：

PX4从放弃到精通（二十八）：垂起过渡控制_权重

构造函数，用初始化列表进行初始化工作队列和循环计数器，同时更新参数，_vtol_type是一个基类指针，指向抽象类VtolType，Tailsitter、Tiltrotor、Standard是它的三个子类，根据参数_param_vt_type.get()（通过静态转换转成vtol_type类型）为不同的子类分配内存。从而指向不同的的子类对象，实现多态。最后广播期望的推力和力矩。

VtolAttitudeControl::VtolAttitudeControl() :
	ModuleParams(nullptr),
	WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
	_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control: cycle"))
{
	// start vtol in rotary wing mode
	_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;

	parameters_update();

	if (static_cast<vtol_type>(_param_vt_type.get()) == vtol_type::TAILSITTER) {
		_vtol_type = new Tailsitter(this);

	} else if (static_cast<vtol_type>(_param_vt_type.get()) == vtol_type::TILTROTOR) {
		_vtol_type = new Tiltrotor(this);

	} else if (static_cast<vtol_type>(_param_vt_type.get()) == vtol_type::STANDARD) {
		_vtol_type = new Standard(this);

	} else {
		exit_and_cleanup();
	}

	_vtol_vehicle_status_pub.advertise();
	_vehicle_thrust_setpoint0_pub.advertise();
	_vehicle_torque_setpoint0_pub.advertise();
	_vehicle_thrust_setpoint1_pub.advertise();
	_vehicle_torque_setpoint1_pub.advertise();
}

析构函数

VtolAttitudeControl::~VtolAttitudeControl()
{
	perf_free(_loop_perf);
}

多旋翼或者固定翼的控制输入更新时，执行程序。

bool
VtolAttitudeControl::init()
{
	if (!_actuator_inputs_mc.registerCallback()) {
		PX4_ERR("callback registration failed");
		return false;
	}

	if (!_actuator_inputs_fw.registerCallback()) {
		PX4_ERR("callback registration failed");
		return false;
	}

	return true;
}

void VtolAttitudeControl::vehicle_status_poll()
{
_vehicle_status_sub.copy(&_vehicle_status);

// abort front transition when RTL is triggered
if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
    && _nav_state_prev != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL && _vtol_type->get_mode() == mode::TRANSITION_TO_FW) {
	_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
}

_nav_state_prev = _vehicle_status.nav_state;

}
更新状态和命令

void VtolAttitudeControl::action_request_poll()
{
	while (_action_request_sub.updated()) {
		action_request_s action_request;

		if (_action_request_sub.copy(&action_request)) {
			switch (action_request.action) {
			case action_request_s::ACTION_VTOL_TRANSITION_TO_MULTICOPTER:
				_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
				_immediate_transition = false;
				break;

			case action_request_s::ACTION_VTOL_TRANSITION_TO_FIXEDWING:
				_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
				_immediate_transition = false;
				break;
			}
		}
	}
}

void VtolAttitudeControl::vehicle_cmd_poll()
{
	vehicle_command_s vehicle_command;

	while (_vehicle_cmd_sub.update(&vehicle_command)) {
		if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) {

			uint8_t result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;

			const int transition_command_param1 = int(vehicle_command.param1 + 0.5f);

			// deny transition from MC to FW in Takeoff, Land, RTL and Orbit
			if (transition_command_param1 == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW &&
			    (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF
			     || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND
			     || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
			     ||  _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT)) {

				result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;

			} else {
				_transition_command = transition_command_param1;
				_immediate_transition = (PX4_ISFINITE(vehicle_command.param2)) ? int(vehicle_command.param2 + 0.5f) : false;
			}

			if (vehicle_command.from_external) {
				vehicle_command_ack_s command_ack{};
				command_ack.timestamp = hrt_absolute_time();
				command_ack.command = vehicle_command.command;
				command_ack.result = result;
				command_ack.target_system = vehicle_command.source_system;
				command_ack.target_component = vehicle_command.source_component;

				uORB::Publication<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
				command_ack_pub.publish(command_ack);
			}
		}
	}
}

紧急切换到多旋翼

void
VtolAttitudeControl::quadchute(QuadchuteReason reason)
{
	if (!_vtol_vehicle_status.vtol_transition_failsafe) {
		switch (reason) {
		case QuadchuteReason::TransitionTimeout:
			mavlink_log_critical(&_mavlink_log_pub, "Quadchute: transition timeout\t");
			events::send(events::ID("vtol_att_ctrl_quadchute_tout"), events::Log::Critical,
				     "Quadchute triggered, due to transition timeout");
			break;

		case QuadchuteReason::ExternalCommand:
			mavlink_log_critical(&_mavlink_log_pub, "Quadchute: external command\t");
			events::send(events::ID("vtol_att_ctrl_quadchute_ext_cmd"), events::Log::Critical,
				     "Quadchute triggered, due to external command");
			break;

		case QuadchuteReason::MinimumAltBreached:
			mavlink_log_critical(&_mavlink_log_pub, "Quadchute: minimum altitude breached\t");
			events::send(events::ID("vtol_att_ctrl_quadchute_min_alt"), events::Log::Critical,
				     "Quadchute triggered, due to minimum altitude breach");
			break;

		case QuadchuteReason::LossOfAlt:
			mavlink_log_critical(&_mavlink_log_pub, "Quadchute: loss of altitude\t");
			events::send(events::ID("vtol_att_ctrl_quadchute_alt_loss"), events::Log::Critical,
				     "Quadchute triggered, due to loss of altitude");
			break;

		case QuadchuteReason::LargeAltError:
			mavlink_log_critical(&_mavlink_log_pub, "Quadchute: large altitude error\t");
			events::send(events::ID("vtol_att_ctrl_quadchute_alt_err"), events::Log::Critical,
				     "Quadchute triggered, due to large altitude error");
			break;

		case QuadchuteReason::MaximumPitchExceeded:
			mavlink_log_critical(&_mavlink_log_pub, "Quadchute: maximum pitch exceeded\t");
			events::send(events::ID("vtol_att_ctrl_quadchute_max_pitch"), events::Log::Critical,
				     "Quadchute triggered, due to maximum pitch angle exceeded");
			break;

		case QuadchuteReason::MaximumRollExceeded:
			mavlink_log_critical(&_mavlink_log_pub, "Quadchute: maximum roll exceeded\t");
			events::send(events::ID("vtol_att_ctrl_quadchute_max_roll"), events::Log::Critical,
				     "Quadchute triggered, due to maximum roll angle exceeded");
			break;
		}

		_vtol_vehicle_status.vtol_transition_failsafe = true;
	}
}

参数更新

void
VtolAttitudeControl::parameters_update()
{
	// check for parameter updates
	if (_parameter_update_sub.updated()) {
		// clear update
		parameter_update_s param_update;
		_parameter_update_sub.copy(¶m_update);

		// update parameters from storage
		updateParams();

		if (_vtol_type != nullptr) {
			_vtol_type->parameters_update();
		}
	}
}

主循环

void
VtolAttitudeControl::Run()
{
	if (should_exit()) {
		_actuator_inputs_fw.unregisterCallback();
		_actuator_inputs_mc.unregisterCallback();
		exit_and_cleanup();
		return;
	}

限制执行频率

const hrt_abstime now = hrt_absolute_time();

#if !defined(ENABLE_LOCKSTEP_SCHEDULER)

	// prevent excessive scheduling (> 500 Hz)
	if (now - _last_run_timestamp < 2_ms) {
		return;
	}

#endif // !ENABLE_LOCKSTEP_SCHEDULER

const float dt = math::min((now - _last_run_timestamp) / 1e6f, kMaxVTOLAttitudeControlTimeStep);
_last_run_timestamp = now;

if (!_initialized) {

	if (_vtol_type->init()) {
		_initialized = true;

	} else {
		exit_and_cleanup();
		return;
	}
}

_vtol_type->setDt(dt);

更新固定翼和多旋翼期望控制输入

perf_begin(_loop_perf);

const bool updated_fw_in = _actuator_inputs_fw.update(&_actuators_fw_in);
const bool updated_mc_in = _actuator_inputs_mc.update(&_actuators_mc_in);

// run on actuator publications corresponding to VTOL mode
bool should_run = false;

相应的话题更新就运行后面的程序

switch (_vtol_type->get_mode()) {
case mode::TRANSITION_TO_FW:
case mode::TRANSITION_TO_MC:
	should_run = updated_fw_in || updated_mc_in;
	break;

case mode::ROTARY_WING:
	should_run = updated_mc_in;
	break;

case mode::FIXED_WING:
	should_run = updated_fw_in;
	break;
}

下面就是垂起的控制逻辑，先订阅一些必须的数据。

if (should_run) {
	parameters_update();

	_vehicle_control_mode_sub.update(&_vehicle_control_mode);
	_vehicle_attitude_sub.update(&_vehicle_attitude);
	_local_pos_sub.update(&_local_pos);
	_local_pos_sp_sub.update(&_local_pos_sp);
	_pos_sp_triplet_sub.update(&_pos_sp_triplet);
	_airspeed_validated_sub.update(&_airspeed_validated);
	_tecs_status_sub.update(&_tecs_status);
	_land_detected_sub.update(&_land_detected);
	vehicle_status_poll();
	action_request_poll();
	vehicle_cmd_poll();

获取空气密度，这个在计算前切换时间的时候会用到。

vehicle_air_data_s air_data;

if (_vehicle_air_data_sub.update(&air_data)) {
	_air_density = air_data.rho;
}

判断期望控制输入是否更新

// check if mc and fw sp were updated
const bool mc_att_sp_updated = _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp);
const bool fw_att_sp_updated = _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);

状态机更新，详见第二节

// update the vtol state machine which decides which mode we are in
_vtol_type->update_vtol_state();

计算过渡状态，也就是计算过渡时多旋翼的控制权重

// check in which mode we are in and call mode specific functions
switch (_vtol_type->get_mode()) {

前切换

case mode::TRANSITION_TO_FW:
	// vehicle is doing a transition to FW
	_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW;

控制输入更新后，通过update_transition_state()计算控制权重和期望的俯仰角，期望的俯仰角发布出去给姿态控制模块。update_transition_state()的介绍见第三节。

if (mc_att_sp_updated || fw_att_sp_updated) {
	_vtol_type->update_transition_state();
	_vehicle_attitude_sp_pub.publish(_vehicle_attitude_sp);
}

break;

后切换，同上

case mode::TRANSITION_TO_MC:
		// vehicle is doing a transition to MC
		_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_MC;

		if (mc_att_sp_updated || fw_att_sp_updated) {
			_vtol_type->update_transition_state();
			_vehicle_attitude_sp_pub.publish(_vehicle_attitude_sp);
		}

		break;

多旋翼状态，控制权重为1

case mode::ROTARY_WING:
	// vehicle is in rotary wing mode
	_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;

	if (mc_att_sp_updated) {
		_vtol_type->update_mc_state();
		_vehicle_attitude_sp_pub.publish(_vehicle_attitude_sp);
	}

	break;

固定翼状态，多旋翼控制权重为0；

case mode::FIXED_WING:
		// vehicle is in fw mode
		_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;

		if (fw_att_sp_updated) {
			_vtol_type->update_fw_state();
			_vehicle_attitude_sp_pub.publish(_vehicle_attitude_sp);
		}

		break;
	}

将控制权重应用到输出

_vtol_type->fill_actuator_outputs();

发布控制输出

_actuator_controls_0_pub.publish(_actuators_out_0);
		_actuator_controls_1_pub.publish(_actuators_out_1);

		_vehicle_torque_setpoint0_pub.publish(_torque_setpoint_0);
		_vehicle_torque_setpoint1_pub.publish(_torque_setpoint_1);
		_vehicle_thrust_setpoint0_pub.publish(_thrust_setpoint_0);
		_vehicle_thrust_setpoint1_pub.publish(_thrust_setpoint_1);

		// Advertise/Publish vtol vehicle status
		_vtol_vehicle_status.timestamp = hrt_absolute_time();
		_vtol_vehicle_status_pub.publish(_vtol_vehicle_status);
	}

	perf_end(_loop_perf);
}

二、update_transition_state（）

以标准垂起为例

void Standard::update_vtol_state()
{
/* After flipping the switch the vehicle will start the pusher (or tractor) motor, picking up
* forward speed. After the vehicle has picked up enough speed the rotors shutdown.
* For the back transition the pusher motor is immediately stopped and rotors reactivated.
*/
多旋翼控制权重

float mc_weight = _mc_roll_weight;

切换时间

float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;

切换失败的话就直接切到多旋翼模式

if (_vtol_vehicle_status->vtol_transition_failsafe) {
	// Failsafe event, engage mc motors immediately
	_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
	_pusher_throttle = 0.0f;
	_reverse_output = 0.0f;

	//reset failsafe when FW is no longer requested
	if (!_attc->is_fixed_wing_requested()) {
		_vtol_vehicle_status->vtol_transition_failsafe = false;
	}

}

切换到多旋翼（后切换）

else if (!_attc->is_fixed_wing_requested()) {

已经在多旋翼模式，多旋翼控制权重为1，反推输出0；

// the transition to fw mode switch is off
if (_vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
	// in mc mode
	_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
	mc_weight = 1.0f;
	_reverse_output = 0.0f;

}

如果当前是固定翼模式，开始执行后切换，记录下后切换的开始时间，设置反推输出

else if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) {
			// Regular backtransition
			_vtol_schedule.flight_mode = vtol_mode::TRANSITION_TO_MC;
			_vtol_schedule.transition_start = hrt_absolute_time();
			_reverse_output = _param_vt_b_rev_out.get();

		}

如果正在执行前切换，则直接切到多旋翼，并且正推和反推都置0；

else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {
			// failsafe back to mc mode
			_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
			mc_weight = 1.0f;
			_pusher_throttle = 0.0f;
			_reverse_output = 0.0f;

		}

执行后切换

else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) {

在执行过渡时，飞机需要先在固定翼和多旋翼控制同时进行的情况下减速到设置的多旋翼巡航速度，然后关闭固定翼控制。有空速的情况下用空速判断，否则用地速判断。

// speed exit condition: use ground if valid, otherwise airspeed
bool exit_backtransition_speed_condition = false;

if (_local_pos->v_xy_valid) {
	const Dcmf R_to_body(Quatf(_v_att->q).inversed());
	const Vector3f vel = R_to_body * Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz);
	exit_backtransition_speed_condition = vel(0) < _param_mpc_xy_cruise.get();

} else if (PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
	exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _param_mpc_xy_cruise.get();
}

判断是否切换超时

const bool exit_backtransition_time_condition = time_since_trans_start > _param_vt_b_trans_dur.get();

如果在地面/达到多旋翼巡航速度/切换超时，直接切到多旋翼模式

if (can_transition_on_ground() || exit_backtransition_speed_condition || exit_backtransition_time_condition) {
			_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
		}
	}

}

执行前切换

else {

在多旋翼或者后切换时，直接开始前切换

// the transition to fw mode switch is on
if (_vtol_schedule.flight_mode == vtol_mode::MC_MODE || _vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) {
	// start transition to fw mode
	/* NOTE: The failsafe transition to fixed-wing was removed because it can result in an
	 * unsafe flying state. */
	_vtol_schedule.flight_mode = vtol_mode::TRANSITION_TO_FW;
	_vtol_schedule.transition_start = hrt_absolute_time();

}

已经切换到固定翼状态，多旋翼的控制权重设置为0；

else if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) {
			// in fw mode
			_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
			mc_weight = 0.0f;

		}

执行前切换

else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {

过渡完成的判据有两个，在有空速的情况下，达到切换空速则完成过渡，否则根据时间判断，过渡时间到达最小的切换时间则完成过渡。这个最小时间根据参数设置和空气密度得出。

// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode

const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
		&& !_param_fw_arsp_mode.get();
const bool minimum_trans_time_elapsed = time_since_trans_start > getMinimumFrontTransitionTime();

bool transition_to_fw = false;


			if (minimum_trans_time_elapsed) {
				if (airspeed_triggers_transition) {
					transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get();

				} else {
					transition_to_fw = true;
				}
			}

判断是否过渡完成

transition_to_fw |= can_transition_on_ground();

		if (transition_to_fw) {
			_vtol_schedule.flight_mode = vtol_mode::FW_MODE;

			// don't set pusher throttle here as it's being ramped up elsewhere
			_trans_finished_ts = hrt_absolute_time();
		}
	}
}

更新权重和当前状态

_mc_roll_weight = mc_weight;
	_mc_pitch_weight = mc_weight;
	_mc_yaw_weight = mc_weight;
	_mc_throttle_weight = mc_weight;

	// map specific control phases to simple control modes
	switch (_vtol_schedule.flight_mode) {
	case vtol_mode::MC_MODE:
		_vtol_mode = mode::ROTARY_WING;
		break;

	case vtol_mode::FW_MODE:
		_vtol_mode = mode::FIXED_WING;
		break;

	case vtol_mode::TRANSITION_TO_FW:
		_vtol_mode = mode::TRANSITION_TO_FW;
		break;

	case vtol_mode::TRANSITION_TO_MC:
		_vtol_mode = mode::TRANSITION_TO_MC;
		break;
	}
}

三、update_transition_state（）

以标准垂起为例

void Standard::update_transition_state()
{
	const hrt_abstime now = hrt_absolute_time();
	float mc_weight = 1.0f;
	const float time_since_trans_start = (float)(now - _vtol_schedule.transition_start) * 1e-6f;

先执行基类的update_transition_state()，主要是计算过渡时间，还有检查是否需要紧急切换到多旋翼。

VtolType::update_transition_state();

// we get attitude setpoint from a multirotor flighttask if climbrate is controlled.
// in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input.
if (_v_control_mode->flag_control_climb_rate_enabled) {
	// we need the incoming (virtual) attitude setpoints (both mc and fw) to be recent, otherwise return (means the previous setpoint stays active)
	if (_mc_virtual_att_sp->timestamp < (now - 1_s) || _fw_virtual_att_sp->timestamp < (now - 1_s)) {
		return;
	}

	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
	_v_att_sp->roll_body = _fw_virtual_att_sp->roll_body;

} else {
	// we need a recent incoming (fw virtual) attitude setpoint, otherwise return (means the previous setpoint stays active)
	if (_fw_virtual_att_sp->timestamp < (now - 1_s)) {
		return;
	}

	memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
	_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
}

前切换，计算切换时固定翼电机的期望推力，如果没有设置推力加速时间参数，则直接输出期望的推力，这个期望的推力是由参数设置的。如果有设置加速时间，则在加速度时间内线性的加速到期望的推力

if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {
	if (_param_vt_psher_slew.get() <= FLT_EPSILON) {
		// just set the final target throttle value
		_pusher_throttle = _param_vt_f_trans_thr.get();

	} else if (_pusher_throttle <= _param_vt_f_trans_thr.get()) {
		// ramp up throttle to the target throttle value
		_pusher_throttle = math::min(_pusher_throttle +
					     _param_vt_psher_slew.get() * _dt, _param_vt_f_trans_thr.get());
	}

切换空速不能低于混合控制的空速（混合控制的空速是指固定翼姿态控制开始起作用的空速，在此之前，固定翼舵面不参与控制，只有尾推/前拉电机提供推力）

_airspeed_trans_blend_margin = _param_vt_arsp_trans.get() - _param_vt_arsp_blend.get();

达到条件开始进行混合控制

// do blending of mc and fw controls if a blending airspeed has been provided and the minimum transition time has passed
		if (_airspeed_trans_blend_margin > 0.0f &&
		    PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) &&
		    _airspeed_validated->calibrated_airspeed_m_s > 0.0f &&
		    _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_blend.get() &&
		    time_since_trans_start > getMinimumFrontTransitionTime()) {

计算多旋翼控制权重，有空速的情况下根据空速计算，随之空速增加线性降低。没有空速的情况下根据最小切换时间计算。

mc_weight = 1.0f - fabsf(_airspeed_validated->calibrated_airspeed_m_s - _param_vt_arsp_blend.get()) /
				    _airspeed_trans_blend_margin;
			// time based blending when no airspeed sensor is set

		} else if (_param_fw_arsp_mode.get() || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
			mc_weight = 1.0f - time_since_trans_start / getMinimumFrontTransitionTime();
			mc_weight = math::constrain(2.0f * mc_weight, 0.0f, 1.0f);

		}

计算切换时的期望俯仰角

// ramp up FW_PSP_OFF
		_v_att_sp->pitch_body = math::radians(_param_fw_psp_off.get()) * (1.0f - mc_weight);

		const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
		q_sp.copyTo(_v_att_sp->q_d);

切换超时则直接切到多旋翼

// check front transition timeout
if (_param_vt_trans_timeout.get() > FLT_EPSILON) {
	if (time_since_trans_start > _param_vt_trans_timeout.get()) {
		// transition timeout occured, abort transition
		_attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout);
	}
}

扰流板和活门不控制

// set spoiler and flaps to 0
_flaps_setpoint_with_slewrate.update(0.f, _dt);
_spoiler_setpoint_with_slewrate.update(0.f, _dt);

后切换

} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) {

通过控制固定翼俯仰来减速

if (_v_control_mode->flag_control_climb_rate_enabled) {
	// control backtransition deceleration using pitch.
	_v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp();
}


		const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
		q_sp.copyTo(_v_att_sp->q_d);

固定翼电机停转

_pusher_throttle = 0.0f;

如果可以的话控制电机反推来减速（需要电调支持）

if (time_since_trans_start >= _param_vt_b_rev_del.get()) {
	// Handle throttle reversal for active breaking
	_pusher_throttle = math::constrain((time_since_trans_start - _param_vt_b_rev_del.get())
					   * _param_vt_psher_slew.get(), 0.0f, _param_vt_b_trans_thr.get());
}

在设置的时间范围内逐渐增加多旋翼控制权重到1

// continually increase mc attitude control as we transition back to mc mode
	if (_param_vt_b_trans_ramp.get() > FLT_EPSILON) {
		mc_weight = time_since_trans_start / _param_vt_b_trans_ramp.get();
	}
}

更新控制权重

mc_weight = math::constrain(mc_weight, 0.0f, 1.0f);

	_mc_roll_weight = mc_weight;
	_mc_pitch_weight = mc_weight;
	_mc_yaw_weight = mc_weight;
	_mc_throttle_weight = mc_weight;
}