fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 06:43:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

VTOL: move to cpp params API

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-05-18 10:01:04 +02:00
parent ff16131874
commit 7292ce483c
11 changed files with 242 additions and 490 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/tiltrotor.cpp
+41 -67
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2015-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2015-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
@@ -63,38 +63,12 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
_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_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");
}
void
Tiltrotor::parameters_update()
{
float v;
/* vtol tilt mechanism position in mc mode */
param_get(_params_handles_tiltrotor.tilt_mc, &v);
_params_tiltrotor.tilt_mc = v;
/* vtol tilt mechanism position in transition mode */
param_get(_params_handles_tiltrotor.tilt_transition, &v);
_params_tiltrotor.tilt_transition = v;
/* vtol tilt mechanism position in fw mode */
param_get(_params_handles_tiltrotor.tilt_fw, &v);
_params_tiltrotor.tilt_fw = v;
/* vtol tilt mechanism position during motor spinup */
param_get(_params_handles_tiltrotor.tilt_spinup, &v);
_params_tiltrotor.tilt_spinup = v;
/* vtol front transition phase 2 duration */
param_get(_params_handles_tiltrotor.front_trans_dur_p2, &v);
_params_tiltrotor.front_trans_dur_p2 = v;
VtolType::updateParams();
}
void Tiltrotor::update_vtol_state()
@@ -137,7 +111,7 @@ void Tiltrotor::update_vtol_state()
break;
case vtol_mode::TRANSITION_BACK:
const bool exit_backtransition_tilt_condition = _tilt_control <= (_params_tiltrotor.tilt_mc + 0.01f);
const bool exit_backtransition_tilt_condition = _tilt_control <= (_param_vt_tilt_mc.get() + 0.01f);
// speed exit condition: use ground if valid, otherwise airspeed
bool exit_backtransition_speed_condition = false;
@@ -145,14 +119,14 @@ void Tiltrotor::update_vtol_state()
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) < _params->mpc_xy_cruise;
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 < _params->mpc_xy_cruise;
exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _param_mpc_xy_cruise.get() ;
}
const float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
const bool exit_backtransition_time_condition = time_since_trans_start > _params->back_trans_duration;
const bool exit_backtransition_time_condition = time_since_trans_start > _param_vt_b_trans_dur.get() ;
if (exit_backtransition_tilt_condition && (exit_backtransition_speed_condition || exit_backtransition_time_condition)) {
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
@@ -178,16 +152,16 @@ void Tiltrotor::update_vtol_state()
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;
&& !_param_fw_arsp_mode.get() ;
bool transition_to_p2 = false;
if (time_since_trans_start > getMinimumFrontTransitionTime()) {
if (airspeed_triggers_transition) {
transition_to_p2 = _airspeed_validated->calibrated_airspeed_m_s >= _params->transition_airspeed;
transition_to_p2 = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get() ;
} else {
transition_to_p2 = _tilt_control >= _params_tiltrotor.tilt_transition &&
transition_to_p2 = _tilt_control >= _param_vt_tilt_trans.get() &&
time_since_trans_start > getOpenLoopFrontTransitionTime();
}
}
@@ -200,8 +174,8 @@ void Tiltrotor::update_vtol_state()
}
// check front transition timeout
if (_params->front_trans_timeout > FLT_EPSILON) {
if (time_since_trans_start > _params->front_trans_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);
}
@@ -213,9 +187,9 @@ void Tiltrotor::update_vtol_state()
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) {
if (_tilt_control >= _param_vt_tilt_fw.get()) {
_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
_tilt_control = _params_tiltrotor.tilt_fw;
_tilt_control = _param_vt_tilt_fw.get();
}
break;
@@ -263,7 +237,7 @@ void Tiltrotor::update_mc_state()
// reset this timestamp while disarmed
if (!_v_control_mode->flag_armed) {
_last_timestamp_disarmed = hrt_absolute_time();
_tilt_motors_for_startup = _params_tiltrotor.tilt_spinup > 0.01f; // spinup phase only required if spinup tilt > 0
_tilt_motors_for_startup = _param_vt_tilt_spinup.get() > 0.01f; // spinup phase only required if spinup tilt > 0
} else if (_tilt_motors_for_startup) {
// leave motors tilted forward after arming to allow them to spin up easier
@@ -274,12 +248,12 @@ void Tiltrotor::update_mc_state()
if (_tilt_motors_for_startup) {
if (hrt_absolute_time() - _last_timestamp_disarmed < spin_up_duration_p1) {
_tilt_control = _params_tiltrotor.tilt_spinup;
_tilt_control = _param_vt_tilt_spinup.get();
} else {
// duration phase 2: begin to adapt tilt to multicopter tilt
float delta_tilt = (_params_tiltrotor.tilt_mc - _params_tiltrotor.tilt_spinup);
_tilt_control = _params_tiltrotor.tilt_spinup + delta_tilt / spin_up_duration_p2 * (hrt_absolute_time() -
float delta_tilt = (_param_vt_tilt_mc.get() - _param_vt_tilt_spinup.get());
_tilt_control = _param_vt_tilt_spinup.get() + delta_tilt / spin_up_duration_p2 * (hrt_absolute_time() -
(_last_timestamp_disarmed + spin_up_duration_p1));
}
@@ -287,11 +261,11 @@ void Tiltrotor::update_mc_state()
} else {
// normal operation
_tilt_control = VtolType::pusher_assist() + _params_tiltrotor.tilt_mc;
_tilt_control = VtolType::pusher_assist() + _param_vt_tilt_mc.get();
_mc_yaw_weight = 1.0f;
// do thrust compensation only for legacy (static) allocation
if (_params->ctrl_alloc != 1) {
if (!_param_sys_ctrl_alloc.get()) {
_v_att_sp->thrust_body[2] = Tiltrotor::thrust_compensation_for_tilt();
}
}
@@ -307,7 +281,7 @@ void Tiltrotor::update_fw_state()
_v_att_sp->thrust_body[2] = -_v_att_sp->thrust_body[0];
// make sure motors are tilted forward
_tilt_control = _params_tiltrotor.tilt_fw;
_tilt_control = _param_vt_tilt_fw.get();
}
void Tiltrotor::update_transition_state()
@@ -338,10 +312,10 @@ void Tiltrotor::update_transition_state()
set_all_motor_state(motor_state::ENABLED);
// tilt rotors forward up to certain angle
if (_tilt_control <= _params_tiltrotor.tilt_transition) {
const float ramped_up_tilt = _params_tiltrotor.tilt_mc +
fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) *
time_since_trans_start / _params->front_trans_duration;
if (_tilt_control <= _param_vt_tilt_trans.get()) {
const float ramped_up_tilt = _param_vt_tilt_mc.get() +
fabsf(_param_vt_tilt_trans.get() - _param_vt_tilt_mc.get()) *
time_since_trans_start / _param_vt_f_trans_dur.get() ;
// only allow increasing tilt (tilt in hover can already be non-zero)
_tilt_control = math::max(_tilt_control, ramped_up_tilt);
@@ -351,16 +325,16 @@ void Tiltrotor::update_transition_state()
_mc_roll_weight = 1.0f;
_mc_yaw_weight = 1.0f;
if (!_params->airspeed_disabled && PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) &&
_airspeed_validated->calibrated_airspeed_m_s >= _params->airspeed_blend) {
const float weight = 1.0f - (_airspeed_validated->calibrated_airspeed_m_s - _params->airspeed_blend) /
(_params->transition_airspeed - _params->airspeed_blend);
if (!_param_fw_arsp_mode.get() && PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) &&
_airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_blend.get()) {
const float weight = 1.0f - (_airspeed_validated->calibrated_airspeed_m_s - _param_vt_arsp_blend.get()) /
(_param_vt_arsp_trans.get() - _param_vt_arsp_blend.get());
_mc_roll_weight = weight;
_mc_yaw_weight = weight;
}
// without airspeed do timed weight changes
if ((_params->airspeed_disabled || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) &&
if ((_param_fw_arsp_mode.get() || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) &&
time_since_trans_start > getMinimumFrontTransitionTime()) {
_mc_roll_weight = 1.0f - (time_since_trans_start - getMinimumFrontTransitionTime()) /
(getOpenLoopFrontTransitionTime() - getMinimumFrontTransitionTime());
@@ -376,15 +350,15 @@ void Tiltrotor::update_transition_state()
} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P2) {
// the plane is ready to go into fixed wing mode, tilt the rotors forward completely
_tilt_control = math::constrain(_params_tiltrotor.tilt_transition +
fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * time_since_trans_start /
_params_tiltrotor.front_trans_dur_p2, _params_tiltrotor.tilt_transition, _params_tiltrotor.tilt_fw);
_tilt_control = math::constrain(_param_vt_tilt_trans.get() +
fabsf(_param_vt_tilt_fw.get() - _param_vt_tilt_trans.get()) * time_since_trans_start /
_param_vt_trans_p2_dur.get(), _param_vt_tilt_trans.get(), _param_vt_tilt_fw.get());
_mc_roll_weight = 0.0f;
_mc_yaw_weight = 0.0f;
// ramp down motors not used in fixed-wing flight (setting MAX_PWM down scales the given output into the new range)
int ramp_down_value = (1.0f - time_since_trans_start / _params_tiltrotor.front_trans_dur_p2) *
int ramp_down_value = (1.0f - time_since_trans_start / _param_vt_trans_p2_dur.get()) *
(PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) + PWM_DEFAULT_MIN;
set_alternate_motor_state(motor_state::VALUE, ramp_down_value);
@@ -412,7 +386,7 @@ void Tiltrotor::update_transition_state()
float progress = (time_since_trans_start - BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) / BACKTRANS_MOTORS_UPTILT_DUR_S;
progress = math::constrain(progress, 0.0f, 1.0f);
_tilt_control = moveLinear(_params_tiltrotor.tilt_fw, _params_tiltrotor.tilt_mc, progress);
_tilt_control = moveLinear(_param_vt_tilt_fw.get(), _param_vt_tilt_mc.get(), progress);
}
_mc_yaw_weight = 1.0f;
@@ -521,9 +495,9 @@ void Tiltrotor::fill_actuator_outputs()
_thrust_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_THROTTLE];
/* allow differential thrust if enabled */
if (_params->diff_thrust == 1) {
mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale;
_torque_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale;
if (_param_vt_fw_difthr_en.get()) {
mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ;
_torque_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ;
}
} else {
@@ -544,10 +518,10 @@ void Tiltrotor::fill_actuator_outputs()
// Fixed wing output
fw_out[actuator_controls_s::INDEX_COLLECTIVE_TILT] = _tilt_control;
if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
fw_out[actuator_controls_s::INDEX_ROLL] = 0;
fw_out[actuator_controls_s::INDEX_PITCH] = 0;
fw_out[actuator_controls_s::INDEX_YAW] = 0;
if (_param_vt_elev_mc_lock.get() && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
fw_out[actuator_controls_s::INDEX_ROLL] = 0;
fw_out[actuator_controls_s::INDEX_PITCH] = 0;
fw_out[actuator_controls_s::INDEX_YAW] = 0;
} else {
fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL];