mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-30 10:23:36 +08:00
VTOL: move to cpp params API
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2021 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -55,71 +55,21 @@ using namespace matrix;
|
||||
using namespace time_literals;
|
||||
|
||||
VtolAttitudeControl::VtolAttitudeControl() :
|
||||
ModuleParams(nullptr),
|
||||
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.sys_ctrl_alloc = param_find("SYS_CTRL_ALLOC");
|
||||
_params.ctrl_alloc = 0;
|
||||
param_get(_params_handles.sys_ctrl_alloc, &_params.ctrl_alloc);
|
||||
|
||||
if (_params.ctrl_alloc != 1) {
|
||||
// these are not used with dynamic control allocation
|
||||
_params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC");
|
||||
_params_handles.vtol_motor_id = param_find("VT_MOT_ID");
|
||||
_params_handles.vt_mc_on_fmu = param_find("VT_MC_ON_FMU");
|
||||
_params_handles.fw_motors_off = param_find("VT_FW_MOT_OFFID");
|
||||
}
|
||||
|
||||
_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.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.pitch_min_rad = param_find("VT_PTCH_MIN");
|
||||
_params_handles.forward_thrust_scale = param_find("VT_FWD_THRUST_SC");
|
||||
|
||||
_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.land_pitch_min_rad = param_find("VT_LND_PTCH_MIN");
|
||||
|
||||
_params_handles.vt_spoiler_mc_ld = param_find("VT_SPOILER_MC_LD");
|
||||
/* fetch initial parameter values */
|
||||
parameters_update();
|
||||
|
||||
if (static_cast<vtol_type>(_params.vtol_type) == vtol_type::TAILSITTER) {
|
||||
if (static_cast<vtol_type>(_param_vt_type.get()) == vtol_type::TAILSITTER) {
|
||||
_vtol_type = new Tailsitter(this);
|
||||
|
||||
} else if (static_cast<vtol_type>(_params.vtol_type) == vtol_type::TILTROTOR) {
|
||||
} else if (static_cast<vtol_type>(_param_vt_type.get()) == vtol_type::TILTROTOR) {
|
||||
_vtol_type = new Tiltrotor(this);
|
||||
|
||||
} else if (static_cast<vtol_type>(_params.vtol_type) == vtol_type::STANDARD) {
|
||||
} else if (static_cast<vtol_type>(_param_vt_type.get()) == vtol_type::STANDARD) {
|
||||
_vtol_type = new Standard(this);
|
||||
|
||||
} else {
|
||||
@@ -269,118 +219,24 @@ VtolAttitudeControl::quadchute(QuadchuteReason reason)
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
void
|
||||
VtolAttitudeControl::parameters_update()
|
||||
{
|
||||
float v;
|
||||
int32_t l;
|
||||
// check for parameter updates
|
||||
if (_parameter_update_sub.updated()) {
|
||||
// clear update
|
||||
parameter_update_s param_update;
|
||||
_parameter_update_sub.copy(¶m_update);
|
||||
|
||||
if (_params.ctrl_alloc != 1) {
|
||||
/* idle pwm for mc mode */
|
||||
param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc);
|
||||
param_get(_params_handles.vtol_motor_id, &_params.vtol_motor_id);
|
||||
param_get(_params_handles.vt_mc_on_fmu, &l);
|
||||
_params.vt_mc_on_fmu = l;
|
||||
// update parameters from storage
|
||||
updateParams();
|
||||
|
||||
/* vtol motor count */
|
||||
param_get(_params_handles.fw_motors_off, &_params.fw_motors_off);
|
||||
if (_vtol_type != nullptr) {
|
||||
_vtol_type->parameters_update();
|
||||
}
|
||||
|
||||
_vtol_vehicle_status.fw_permanent_stab = _param_vt_fw_perm_stab.get();
|
||||
}
|
||||
|
||||
/* vtol fw permanent stabilization */
|
||||
param_get(_params_handles.vtol_fw_permanent_stab, &l);
|
||||
_vtol_vehicle_status.fw_permanent_stab = (l == 1);
|
||||
|
||||
param_get(_params_handles.vtol_type, &l);
|
||||
_params.vtol_type = l;
|
||||
|
||||
/* vtol lock elevons in multicopter */
|
||||
param_get(_params_handles.elevons_mc_lock, &l);
|
||||
_params.elevons_mc_lock = (l == 1);
|
||||
|
||||
/* minimum relative altitude for FW mode (QuadChute) */
|
||||
param_get(_params_handles.fw_min_alt, &v);
|
||||
_params.fw_min_alt = v;
|
||||
|
||||
/* maximum negative altitude error for FW mode (Adaptive QuadChute) */
|
||||
param_get(_params_handles.fw_alt_err, &v);
|
||||
_params.fw_alt_err = v;
|
||||
|
||||
/* maximum pitch angle (QuadChute) */
|
||||
param_get(_params_handles.fw_qc_max_pitch, &l);
|
||||
_params.fw_qc_max_pitch = l;
|
||||
|
||||
/* maximum roll angle (QuadChute) */
|
||||
param_get(_params_handles.fw_qc_max_roll, &l);
|
||||
_params.fw_qc_max_roll = l;
|
||||
|
||||
param_get(_params_handles.front_trans_time_openloop, &_params.front_trans_time_openloop);
|
||||
|
||||
param_get(_params_handles.front_trans_time_min, &_params.front_trans_time_min);
|
||||
|
||||
/*
|
||||
* Open loop transition time needs to be larger than minimum transition time,
|
||||
* anything else makes no sense and can potentially lead to numerical problems.
|
||||
*/
|
||||
if (_params.front_trans_time_openloop < _params.front_trans_time_min * 1.1f) {
|
||||
_params.front_trans_time_openloop = _params.front_trans_time_min * 1.1f;
|
||||
param_set_no_notification(_params_handles.front_trans_time_openloop, &_params.front_trans_time_openloop);
|
||||
mavlink_log_critical(&_mavlink_log_pub, "OL transition time set larger than min transition time\t");
|
||||
/* EVENT
|
||||
* @description <param>VT_F_TR_OL_TM</param> set to {1:.1}.
|
||||
*/
|
||||
events::send<float>(events::ID("vtol_att_ctrl_ol_trans_too_large"), events::Log::Warning,
|
||||
"Open loop transition time set larger than minimum transition time", _params.front_trans_time_openloop);
|
||||
}
|
||||
|
||||
param_get(_params_handles.front_trans_duration, &_params.front_trans_duration);
|
||||
param_get(_params_handles.back_trans_duration, &_params.back_trans_duration);
|
||||
param_get(_params_handles.transition_airspeed, &_params.transition_airspeed);
|
||||
param_get(_params_handles.front_trans_throttle, &_params.front_trans_throttle);
|
||||
param_get(_params_handles.back_trans_throttle, &_params.back_trans_throttle);
|
||||
param_get(_params_handles.airspeed_blend, &_params.airspeed_blend);
|
||||
param_get(_params_handles.airspeed_mode, &l);
|
||||
_params.airspeed_disabled = l != 0;
|
||||
param_get(_params_handles.front_trans_timeout, &_params.front_trans_timeout);
|
||||
param_get(_params_handles.mpc_xy_cruise, &_params.mpc_xy_cruise);
|
||||
param_get(_params_handles.diff_thrust, &_params.diff_thrust);
|
||||
|
||||
param_get(_params_handles.diff_thrust_scale, &v);
|
||||
_params.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f);
|
||||
|
||||
/* maximum down pitch allowed */
|
||||
param_get(_params_handles.pitch_min_rad, &v);
|
||||
_params.pitch_min_rad = math::radians(v);
|
||||
|
||||
/* maximum down pitch allowed during landing*/
|
||||
param_get(_params_handles.land_pitch_min_rad, &v);
|
||||
_params.land_pitch_min_rad = math::radians(v);
|
||||
|
||||
/* scale for fixed wing thrust used for forward acceleration in multirotor mode */
|
||||
param_get(_params_handles.forward_thrust_scale, &_params.forward_thrust_scale);
|
||||
|
||||
// make sure parameters are feasible, require at least 1 m/s difference between transition and blend airspeed
|
||||
_params.airspeed_blend = math::min(_params.airspeed_blend, _params.transition_airspeed - 1.0f);
|
||||
|
||||
param_get(_params_handles.back_trans_dec_sp, &v);
|
||||
// increase the target deceleration setpoint provided to the controller by 20%
|
||||
// to make overshooting the transition waypoint less likely in the presence of tracking errors
|
||||
_params.back_trans_dec_sp = 1.2f * v;
|
||||
|
||||
param_get(_params_handles.dec_to_pitch_ff, &_params.dec_to_pitch_ff);
|
||||
param_get(_params_handles.dec_to_pitch_i, &_params.dec_to_pitch_i);
|
||||
|
||||
param_get(_params_handles.vt_forward_thrust_enable_mode, &_params.vt_forward_thrust_enable_mode);
|
||||
param_get(_params_handles.mpc_land_alt1, &_params.mpc_land_alt1);
|
||||
param_get(_params_handles.mpc_land_alt2, &_params.mpc_land_alt2);
|
||||
|
||||
param_get(_params_handles.vt_spoiler_mc_ld, &_params.vt_spoiler_mc_ld);
|
||||
|
||||
// update the parameters of the instances of base VtolType
|
||||
if (_vtol_type != nullptr) {
|
||||
_vtol_type->parameters_update();
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void
|
||||
@@ -408,7 +264,6 @@ VtolAttitudeControl::Run()
|
||||
_last_run_timestamp = now;
|
||||
|
||||
if (!_initialized) {
|
||||
parameters_update(); // initialize parameter cache
|
||||
|
||||
if (_vtol_type->init()) {
|
||||
_initialized = true;
|
||||
@@ -445,15 +300,7 @@ VtolAttitudeControl::Run()
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
parameters_update();
|
||||
|
||||
_v_control_mode_sub.update(&_v_control_mode);
|
||||
_v_att_sub.update(&_v_att);
|
||||
@@ -473,8 +320,8 @@ VtolAttitudeControl::Run()
|
||||
}
|
||||
|
||||
// check if mc and fw sp 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);
|
||||
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();
|
||||
|
||||
Reference in New Issue
Block a user