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 = param_find("VT_FW_QC_P");
_params_handles.fw_qc_max_roll = param_find("VT_FW_QC_R");
_params_handles.front_trans_time_openloop = param_find("VT_F_TR_OL_TM");
_params_handles.front_trans_time_min = param_find("VT_TRANS_MIN_TM");
_params_handles.front_trans_duration = param_find("VT_F_TRANS_DUR");
_params_handles.back_trans_duration = param_find("VT_B_TRANS_DUR");
_params_handles.transition_airspeed = param_find("VT_ARSP_TRANS");
_params_handles.front_trans_throttle = param_find("VT_F_TRANS_THR");
_params_handles.back_trans_throttle = param_find("VT_B_TRANS_THR");
_params_handles.airspeed_blend = param_find("VT_ARSP_BLEND");
_params_handles.airspeed_mode = param_find("FW_ARSP_MODE");
_params_handles.front_trans_timeout = param_find("VT_TRANS_TIMEOUT");
_params_handles.mpc_xy_cruise = param_find("MPC_XY_CRUISE");
_params_handles.fw_motors_off = param_find("VT_FW_MOT_OFFID");
_params_handles.diff_thrust = param_find("VT_FW_DIFTHR_EN");
_params_handles.diff_thrust_scale = param_find("VT_FW_DIFTHR_SC");
_params_handles.dec_to_pitch_ff = param_find("VT_B_DEC_FF");
_params_handles.dec_to_pitch_i = param_find("VT_B_DEC_I");
_params_handles.back_trans_dec_sp = param_find("VT_B_DEC_MSS");
_params_handles.down_pitch_max = param_find("VT_DWN_PITCH_MAX");
_params_handles.forward_thrust_scale = param_find("VT_FWD_THRUST_SC");
_params_handles.vt_mc_on_fmu = param_find("VT_MC_ON_FMU");
_params_handles.vt_forward_thrust_enable_mode = param_find("VT_FWD_THRUST_EN");
_params_handles.mpc_land_alt1 = param_find("MPC_LAND_ALT1");
_params_handles.mpc_land_alt2 = param_find("MPC_LAND_ALT2");
_params_handles.down_pitch_max = param_find("VT_DWN_PITCH_MAX");
_params_handles.forward_thrust_scale = param_find("VT_FWD_THRUST_SC");
/* fetch initial parameter values */
parameters_update();
if (static_cast<vtol_type>(_params.vtol_type) == vtol_type::TAILSITTER) {
_vtol_type = new Tailsitter(this);
} else if (static_cast<vtol_type>(_params.vtol_type) == vtol_type::TILTROTOR) {
_vtol_type = new Tiltrotor(this);
} else if (static_cast<vtol_type>(_params.vtol_type) == vtol_type::STANDARD) {
_vtol_type = new Standard(this);
} else {
exit_and_cleanup();
}
}
本文主要针对Tiltrotor构型进行程序追踪。
Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
VtolType(attc)
{
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
_vtol_schedule.transition_start = 0;
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
_flag_was_in_trans_mode = false;
_params_handles_tiltrotor.tilt_mc = param_find("VT_TILT_MC");
_params_handles_tiltrotor.tilt_transition = param_find("VT_TILT_TRANS");
_params_handles_tiltrotor.tilt_transition_1 = param_find("VT_TILT_TRANS_1");
_params_handles_tiltrotor.tilt_fw = param_find("VT_TILT_FW");
_params_handles_tiltrotor.tilt_spinup = param_find("VT_TILT_SPINUP");
_params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR");
_params_handles_tiltrotor.front_trans_dur_p2_1 = param_find("VT_TRANS_P2_DUR_1");
}
进入到Tiltrotor构造函数中，发现对参数完成更新后，该部分就完成初始化参数查找。然后返回到VtolAttitudeControl::task_spawn(int argc, char *argv[])函数中，继续进行初始化。
if (instance->init()) {
return PX4_OK;
}
bool
VtolAttitudeControl::init()
{
if (!_actuator_inputs_mc.registerCallback()) {
PX4_ERR("MC actuator controls callback registration failed!");
return false;
}
if (!_actuator_inputs_fw.registerCallback()) {
PX4_ERR("FW actuator controls callback registration failed!");
return false;
}
return true;
}
至此，Tiltrotor的初始化就完成了，下面对公有化模块与进程运行中调用的Run（）函数进行继续追踪。
Tiltrotor构型程序运行
主要接口函数从VtolAttitudeControl::Run()函数开始，然后对各个参数进行再次更新。
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
_last_run_timestamp = now;
if (!_initialized) {
parameters_update(); // initialize parameter cache
if (_vtol_type->init()) {//进入到执行机构初始化函数中，分配输出信号
_initialized = true;
} else {
exit_and_cleanup();
return;
}
}
perf_begin(_loop_perf);
//判断是否有执行机构输出actuator controls from mc_att_control
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) {
// check for parameter updates
if (_parameter_update_sub.updated()) {
// clear update
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
// update parameters from storage
parameters_update();
}
_v_control_mode_sub.update(&_v_control_mode);
_manual_control_switches_sub.update(&_manual_control_switches);
_v_att_sub.update(&_v_att);
_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_cmd_poll();//implemented MAVLink command for VTOL transitions, pulled switch up for each type
// check if mc and fw setpoint were updated
bool mc_att_sp_updated = _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp);
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();
// reset transition command if not auto control
if (_v_control_mode.flag_control_manual_enabled) {
if (_vtol_type->get_mode() == mode::ROTARY_WING) {
_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
} else if (_vtol_type->get_mode() == mode::FIXED_WING) {
_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
} else if (_vtol_type->get_mode() == mode::TRANSITION_TO_MC) {
/* We want to make sure that a mode change (manual>auto) during the back transition
* doesn't result in an unsafe state. This prevents the instant fall back to
* fixed-wing on the switch from manual to auto */
_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
}
}
// check in which mode we are in and call mode specific functions
switch (_vtol_type->get_mode()) {
case mode::TRANSITION_TO_FW:
case mode::TRANSITION_TO_MC:
// 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() == mode::TRANSITION_TO_FW);
_fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);
if (!_vtol_type->was_in_trans_mode() || mc_att_sp_updated || fw_att_sp_updated) {
_vtol_type->update_transition_state();
_v_att_sp_pub.publish(_v_att_sp);
}
break;
case mode::ROTARY_WING:
// vehicle is in rotary wing mode
_vtol_vehicle_status.vtol_in_rw_mode = true;
_vtol_vehicle_status.vtol_in_trans_mode = false;
_vtol_vehicle_status.in_transition_to_fw = false;
_vtol_type->update_mc_state();
_v_att_sp_pub.publish(_v_att_sp);
break;
case mode::FIXED_WING:
// vehicle is in fw mode
_vtol_vehicle_status.vtol_in_rw_mode = false;
_vtol_vehicle_status.vtol_in_trans_mode = false;
_vtol_vehicle_status.in_transition_to_fw = false;
if (fw_att_sp_updated) {
_vtol_type->update_fw_state();
_v_att_sp_pub.publish(_v_att_sp);
}
break;
}
_vtol_type->fill_actuator_outputs();
_actuators_0_pub.publish(_actuators_out_0);
_actuators_1_pub.publish(_actuators_out_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);
}
通过对_initialized参数的判断，开始对构型进行初始化，完成一些执行机构的初始化（通道分配），后续再根据不同构型进行程序循环。
if (!_initialized) {
parameters_update(); // initialize parameter cache
if (_vtol_type->init()) {
_initialized = true;
} else {
exit_and_cleanup();
return;
}
}
后面重要的调用就是update_vtol_state模式识别与判断，操纵信号输出与最后的信号发布。
// update the vtol state machine which decides which mode we are in
_vtol_type->update_vtol_state();
// reset transition command if not auto control
if (_v_control_mode.flag_control_manual_enabled) {
if (_vtol_type->get_mode() == mode::ROTARY_WING) {
_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
} else if (_vtol_type->get_mode() == mode::FIXED_WING) {
_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
} else if (_vtol_type->get_mode() == mode::TRANSITION_TO_MC) {
/* We want to make sure that a mode change (manual>auto) during the back transition
* doesn't result in an unsafe state. This prevents the instant fall back to
* fixed-wing on the switch from manual to auto */
_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
}
}
更新不同机型的飞行模式：_vtol_type->update_vtol_state();
本文针对是Tiltrotor构型，直接找到该函数。
void Tiltrotor::update_vtol_state()
{
/* simple logic using a two way switch to perform transitions.
* after flipping the switch the vehicle will start tilting rotors, picking up
* forward speed. After the vehicle has picked up enough speed the rotors are tilted
* forward completely. For the backtransition the motors simply rotate back.
*/
if (_vtol_vehicle_status->vtol_transition_failsafe) {
// Failsafe event, switch to MC mode immediately
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
//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()) {
// plane is in multicopter mode
switch (_vtol_schedule.flight_mode) {
case vtol_mode::MC_MODE:
break;
case vtol_mode::FW_MODE:
_vtol_schedule.flight_mode = vtol_mode::TRANSITION_BACK;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case vtol_mode::TRANSITION_FRONT_P1:
// failsafe into multicopter mode
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
break;
case vtol_mode::TRANSITION_FRONT_P2:
// failsafe into multicopter mode
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
break;
case vtol_mode::TRANSITION_BACK:
const float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
const float ground_speed = sqrtf(_local_pos->vx * _local_pos->vx + _local_pos->vy * _local_pos->vy);
const bool ground_speed_below_cruise = _local_pos->v_xy_valid && (ground_speed <= _params->mpc_xy_cruise);
if (_tilt_control <= _params_tiltrotor.tilt_mc && (time_since_trans_start > _params->back_trans_duration
|| ground_speed_below_cruise)) {
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
}
break;
}
} else {
switch (_vtol_schedule.flight_mode) {
case vtol_mode::MC_MODE:
// initialise a front transition
_vtol_schedule.flight_mode = vtol_mode::TRANSITION_FRONT_P1;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case vtol_mode::FW_MODE:
break;
case vtol_mode::TRANSITION_FRONT_P1: {
float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
&& !_params->airspeed_disabled;
//空速触发倾转判断,查看是否能检测到空速
bool transition_to_p2 = false;
//设定一个p2状态评估参数
if (time_since_trans_start > _params->front_trans_time_min) {
if (airspeed_triggers_transition) {
transition_to_p2 = _airspeed_validated->calibrated_airspeed_m_s >= _params->transition_airspeed;
} else {
transition_to_p2 = _tilt_control >= _params_tiltrotor.tilt_transition &&
time_since_trans_start > _params->front_trans_time_openloop;;
}
}
transition_to_p2 |= can_transition_on_ground();
if (transition_to_p2) {
_vtol_schedule.flight_mode = vtol_mode::TRANSITION_FRONT_P2;
_vtol_schedule.transition_start = hrt_absolute_time();
}
break;
}
case vtol_mode::TRANSITION_FRONT_P2:
// if the rotors have been tilted completely we switch to fw mode
if (_tilt_control >= _params_tiltrotor.tilt_fw) {
_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
_tilt_control = _params_tiltrotor.tilt_fw;
}
break;
case vtol_mode::TRANSITION_BACK:
// failsafe into fixed wing mode
_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
break;
}
}
// map tiltrotor 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_FRONT_P1:
case vtol_mode::TRANSITION_FRONT_P2:
_vtol_mode = mode::TRANSITION_TO_FW;
break;
case vtol_mode::TRANSITION_BACK:
_vtol_mode = mode::TRANSITION_TO_MC;
break;
}
}
针对在直升机、固定翼和倾转过渡状态进行判断，在倾转过渡模式中时，依照有无空速计两种不同的不同方式进行判断。
①Tiltrotor::update_vtol_state() 模式转换
使用双向开关执行转换的简单逻辑，在翻转开关后，飞机将开始倾斜旋翼，获得前进速度。
在飞机提升足够的速度后，旋翼完全向前倾斜。 对于反转换，电机只需旋转回来。
固定翼模式请求为false（ is_fixed_wing_requested()==false）：
MC_MODE:保持原模式
FW_MODE: 模式切换成TRANSITION_BACK，转换计时开始
TRANSITION_FRONT_P1 || TRANSITION_FRONT_P2: 失效保护，进入MC_MODE
TRANSITION_BACK：若_tilt_control <= _params_tiltrotor.tilt_mc（应该理解为倾转电机已经在旋翼模式下），则进入MC_MODE
固定翼模式请求为ture（ is_fixed_wing_requested()==ture）：
MC_MODE：切换到TRANSITION_FRONT_P1，转换计时开始
FW_MODE: 保持原模式
TRANSITION_FRONT_P1 ：①有空速计，检查空速是否满足转换空速阈值
②没有空速计，则按时间转换
按位计算，切换到TRANSITION_FRONT_P2，并开始计时
TRANSITION_FRONT_P2: 旋翼倾转完成，切换到FW_MODE
TRANSITION_BACK：失效保护，进入FW_MODE*
Tiltrotor::update_transition_state(）
在完成模式切换与识别之后，在Run（）函数中继续追踪，后续是针对当前飞行器在不同模式，完成对倾转舵机的控制与操纵系数选择。
// check in which mode we are in and call mode specific functions
switch (_vtol_type->get_mode()) {
case mode::TRANSITION_TO_FW:
case mode::TRANSITION_TO_MC:
// 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() == mode::TRANSITION_TO_FW);
_fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);
if (!_vtol_type->was_in_trans_mode() || mc_att_sp_updated || fw_att_sp_updated) {
_vtol_type->update_transition_state();
// mavlink_log_info("The aircraft in TRANSITION_TO_FW =%d",int(_vtol_vehicle_status.in_transition_to_fw));
_v_att_sp_pub.publish(_v_att_sp);
}
//mavlink_log_info("The aircraft in TRANSITION_TO_FW_but_anything=%d",int(_vtol_vehicle_status.in_transition_to_fw));
break;
case mode::ROTARY_WING:
// vehicle is in rotary wing mode
_vtol_vehicle_status.vtol_in_rw_mode = true;
_vtol_vehicle_status.vtol_in_trans_mode = false;
_vtol_vehicle_status.in_transition_to_fw = false;
_vtol_type->update_mc_state();
_v_att_sp_pub.publish(_v_att_sp);
break;
case mode::FIXED_WING:
// vehicle is in fw mode
_vtol_vehicle_status.vtol_in_rw_mode = false;
_vtol_vehicle_status.vtol_in_trans_mode = false;
_vtol_vehicle_status.in_transition_to_fw = false;
if (fw_att_sp_updated) {
_vtol_type->update_fw_state();
_v_att_sp_pub.publish(_v_att_sp);
}
break;
}
_vtol_type->fill_actuator_outputs();
_actuators_0_pub.publish(_actuators_out_0);
_actuators_1_pub.publish(_actuators_out_1);
// Advertise/Publish vtol vehicle status
_vtol_vehicle_status.timestamp = hrt_absolute_time();
_vtol_vehicle_status_pub.publish(_vtol_vehicle_status);
}
通过三个函数进行更新当前倾转舵机控制与操纵系数的选取。
update_transition_state();
update_fw_state();
update_fw_state();
后续通过fill_actuator_outputs()函数进行电机舵机操纵计算，最后通过
_actuators_0_pub.publish(_actuators_out_0);
_actuators_1_pub.publish(_actuators_out_1);
进行最终信号输出。
1229
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
