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 (飞机+四旋翼)

垂直起降飞行器与其它种类的飞行器共享代码库，所不同的仅仅是增加了额外的控制逻辑，特别是转换阶段。

下面看源码：

vtol_att_control_main

1、订阅并更新参数

与所有控制器一样，由start函数开启进程，执行任务，指向task_main()

orb订阅各种参数：

        /* do subscriptions */
	_v_att_sp_sub          = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	_mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint));
	_fw_virtual_att_sp_sub = orb_subscribe(ORB_ID(fw_virtual_attitude_setpoint));
	_mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint));
	_fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint));
	_v_att_sub             = orb_subscribe(ORB_ID(vehicle_attitude));
	_v_control_mode_sub    = orb_subscribe(ORB_ID(vehicle_control_mode));
	_params_sub            = orb_subscribe(ORB_ID(parameter_update));
	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	_local_pos_sub         = orb_subscribe(ORB_ID(vehicle_local_position));
	_local_pos_sp_sub         = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
	_pos_sp_triplet_sub    = orb_subscribe(ORB_ID(position_setpoint_triplet));
	_airspeed_sub          = orb_subscribe(ORB_ID(airspeed));
	_vehicle_cmd_sub	   = orb_subscribe(ORB_ID(vehicle_command));
	_tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));
	_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));

	_actuator_inputs_mc    = orb_subscribe(ORB_ID(actuator_controls_virtual_mc));
	_actuator_inputs_fw    = orb_subscribe(ORB_ID(actuator_controls_virtual_fw));

	parameters_update();  // initialize parameter cache

parameters_update()更新通用参数和特定机型的参数。

2、_vtol_type->set_idle_mc()确保飞机以mc模式贴地起飞。

3、进入while循环首先检查参数更新

vehicle_control_mode_poll() //检查飞机控制模式的轮询

手动模式下重置转换命令；

更新不同机型的飞行模式①：_vtol_type->update_vtol_state();

检查飞机处于哪种模式并调用特定函数②:

    ROTARY_WING：主要函数为_vtol_type->update_mc_state(); 计算期望姿态

   mc_virtual_att_sp_poll();

			// vehicle is in rotary wing mode
			_vtol_vehicle_status.vtol_in_rw_mode = true;
			_vtol_vehicle_status.vtol_in_trans_mode = false;

			// got data from mc attitude controller
			_vtol_type->update_mc_state();
			fill_mc_att_rates_sp();

    FIXED_WING：主要函数为_vtol_type->update_fw_state(); 计算期望姿态

   fw_virtual_att_sp_poll();

			// vehicle is in fw mode
			_vtol_vehicle_status.vtol_in_rw_mode = false;
			_vtol_vehicle_status.vtol_in_trans_mode = false;

			_vtol_type->update_fw_state();
			fill_fw_att_rates_sp();

    TRANSITION：主要函数为_vtol_type->update_transition_state();计算转换时的姿态期望值

   mc_virtual_att_sp_poll();
			fw_virtual_att_sp_poll();

			// vehicle is doing a transition
			_vtol_vehicle_status.vtol_in_trans_mode = true;
			_vtol_vehicle_status.vtol_in_rw_mode = true; //making mc attitude controller work during transition
			_vtol_vehicle_status.in_transition_to_fw = (_vtol_type->get_mode() == TRANSITION_TO_FW);

			_vtol_type->update_transition_state();
			fill_mc_att_rates_sp();

4、填充actuator输出并发布姿态期望值