PX4中vtol_att_control 源码解析

px4中vtol姿态控制源码分析

/src/modules/vtol_att_control/文件夹中包含vtol_att_control_main、vtol_type、standard/tailsitter/tiltrotor等文件。下面是主要控制逻辑：

事实上，PX4飞控系统支持所有的垂直起降机型配置：

• 尾座式tailsitter (X/+型布局的双/四旋翼)
• 倾转式tiltrotor (Firefly Y6)
• 复合式standard (飞机+四旋翼)

本文主要针对tiltrotor构型飞行器进行代码分析

Tiltrotor代码流程分析

该模块与其他模块类似，都是从vtol_att_control_main函数入口进入

int vtol_att_control_main(int argc, char *argv[])
{
   
	return VtolAttitudeControl::main(argc, argv);
}

然后由公有模块module中根据不同的关键字，调用不同的函数

static int main(int argc, char *argv[])
	{
   
		if (argc <= 1 ||
		    strcmp(argv[1], "-h")    == 0 ||
		    strcmp(argv[1], "help")  == 0 ||
		    strcmp(argv[1], "info")  == 0 ||
		    strcmp(argv[1], "usage") == 0) {
   
			return T::print_usage();
		}

		if (strcmp(argv[1], "start") == 0) {
   
			// Pass the 'start' argument too, because later on px4_getopt() will ignore the first argument.
			return start_command_base(argc - 1, argv + 1);
		}

		if (strcmp(argv[1], "status") == 0) {
   
			return status_command();
		}

		if (strcmp(argv[1], "stop") == 0) {
   
			return stop_command();
		}

		lock_module(); // Lock here, as the method could access _object.
		int ret = T::custom_command(argc - 1, argv + 1);
		unlock_module();

		return ret;
	}

通过start_command_base函数调用，进入到主程序中task_spawn函数

	static int start_command_base(int argc, char *argv[])
	{
   
		int ret = 0;
		lock_module();

		if (is_running()) {
   
			ret = -1;
			PX4_ERR("Task already running");

		} else {
   
			ret = T::task_spawn(argc, argv);

			if (ret < 0) {
   
				PX4_ERR("Task start failed (%i)", ret);
			}
		}

		unlock_module();
		return ret;
	}

返回到VtolAttitudeControl::task_spawn(int argc, char *argv[])函数中，首先构造VtolAttitudeControl()，然后对创建进程，通过进程系统，保证该函数持续运行。

int
VtolAttitudeControl::task_spawn(int argc, char *argv[])
{
   
	VtolAttitudeControl *instance = new VtolAttitudeControl();//创建VtolAttitudeControl进程

	if (instance) {
   
		_object.store(instance);
		_task_id = task_id_is_work_queue;

		if (instance->init()) {
   //对该进程进行初始化
			return PX4_OK;
		}

	} else {
   
		PX4_ERR("alloc failed");
	}

	delete instance;
	_object.store(nullptr);
	_task_id = -1;

	return PX4_ERROR;
}

VtolAttitudeControl构造函数与初始化

继续转向new VtolAttitudeControl函数，进入构造函数，查找相关参数，并进行复制。通过对构型参数的判断，转向不同的构型飞行器控制程序，进程其他参数更新。

VtolAttitudeControl::VtolAttitudeControl() :
	WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
	_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control: cycle"))
{
   
	_vtol_vehicle_status.vtol_in_rw_mode = true;	/* start vtol in rotary wing mode*/

	_params.idle_pwm_mc = PWM_DEFAULT_MIN;
	_params.vtol_motor_id = 0;

	_params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC");
	_params_handles.vtol_motor_id = param_find("VT_MOT_ID");
	_params_handles.vtol_fw_permanent_stab = param_find("VT_FW_PERM_STAB");
	_params_handles.vtol_type = param_find("VT_TYPE");
	_params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK");
	_params_handles.fw_min_alt = param_find("VT_FW_MIN_ALT");
	_params_handles.fw_alt_err = param_find("VT_FW_ALT_ERR");
	_params_handles.fw_qc_max_pitch =