|
|
|
@ -1,6 +1,6 @@
@@ -1,6 +1,6 @@
|
|
|
|
|
/****************************************************************************
|
|
|
|
|
* |
|
|
|
|
* Copyright (c) 2015 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 |
|
|
|
@ -52,6 +52,7 @@ using namespace matrix;
@@ -52,6 +52,7 @@ using namespace matrix;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
VtolType::VtolType(VtolAttitudeControl *att_controller) : |
|
|
|
|
ModuleParams(nullptr), |
|
|
|
|
_attc(att_controller), |
|
|
|
|
_vtol_mode(mode::ROTARY_WING) |
|
|
|
|
{ |
|
|
|
@ -74,7 +75,6 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
@@ -74,7 +75,6 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
|
|
|
|
|
_airspeed_validated = _attc->get_airspeed(); |
|
|
|
|
_tecs_status = _attc->get_tecs_status(); |
|
|
|
|
_land_detected = _attc->get_land_detected(); |
|
|
|
|
_params = _attc->get_params(); |
|
|
|
|
|
|
|
|
|
for (auto &pwm_max : _max_mc_pwm_values.values) { |
|
|
|
|
pwm_max = PWM_DEFAULT_MAX; |
|
|
|
@ -87,8 +87,8 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
@@ -87,8 +87,8 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
|
|
|
|
|
|
|
|
|
|
bool VtolType::init() |
|
|
|
|
{ |
|
|
|
|
if (_params->ctrl_alloc != 1) { |
|
|
|
|
const char *dev = _params->vt_mc_on_fmu ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH; |
|
|
|
|
if (!_param_sys_ctrl_alloc.get()) { |
|
|
|
|
const char *dev = _param_vt_mc_on_fmu.get() ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH; |
|
|
|
|
|
|
|
|
|
int fd = px4_open(dev, 0); |
|
|
|
|
|
|
|
|
@ -124,8 +124,8 @@ bool VtolType::init()
@@ -124,8 +124,8 @@ bool VtolType::init()
|
|
|
|
|
|
|
|
|
|
px4_close(fd); |
|
|
|
|
|
|
|
|
|
_main_motor_channel_bitmap = generate_bitmap_from_channel_numbers(_params->vtol_motor_id); |
|
|
|
|
_alternate_motor_channel_bitmap = generate_bitmap_from_channel_numbers(_params->fw_motors_off); |
|
|
|
|
_main_motor_channel_bitmap = generate_bitmap_from_channel_numbers(_param_vt_mot_id.get()); |
|
|
|
|
_alternate_motor_channel_bitmap = generate_bitmap_from_channel_numbers(_param_vt_fw_mot_offid.get()); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// in order to get the main motors we take all motors and clear the alternate motor bits
|
|
|
|
@ -143,6 +143,16 @@ bool VtolType::init()
@@ -143,6 +143,16 @@ bool VtolType::init()
|
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void VtolType::parameters_update() |
|
|
|
|
{ |
|
|
|
|
updateParams(); |
|
|
|
|
|
|
|
|
|
// make sure that transition speed is above blending speed
|
|
|
|
|
_param_vt_arsp_trans.set(math::max(_param_vt_arsp_trans.get(), _param_vt_arsp_blend.get())); |
|
|
|
|
// make sure that openloop transition time is above minimum time
|
|
|
|
|
_param_vt_f_tr_ol_tm.set(math::max(_param_vt_f_tr_ol_tm.get(), _param_vt_trans_min_tm.get())); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void VtolType::update_mc_state() |
|
|
|
|
{ |
|
|
|
|
if (!_flag_idle_mc) { |
|
|
|
@ -165,7 +175,7 @@ void VtolType::update_mc_state()
@@ -165,7 +175,7 @@ void VtolType::update_mc_state()
|
|
|
|
|
|
|
|
|
|
if (_attc->get_pos_sp_triplet()->current.valid |
|
|
|
|
&& _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { |
|
|
|
|
spoiler_setpoint_hover = _params->vt_spoiler_mc_ld; |
|
|
|
|
spoiler_setpoint_hover = _param_vt_spoiler_mc_ld.get(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
_spoiler_setpoint_with_slewrate.update(math::constrain(spoiler_setpoint_hover, 0.f, 1.f), _dt); |
|
|
|
@ -243,12 +253,16 @@ float VtolType::update_and_get_backtransition_pitch_sp()
@@ -243,12 +253,16 @@ float VtolType::update_and_get_backtransition_pitch_sp()
|
|
|
|
|
const float track = atan2f(_local_pos->vy, _local_pos->vx); |
|
|
|
|
const float accel_body_forward = cosf(track) * _local_pos->ax + sinf(track) * _local_pos->ay; |
|
|
|
|
|
|
|
|
|
// 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
|
|
|
|
|
const float dec_sp = _param_vt_b_dec_mss.get() * 1.2f; |
|
|
|
|
|
|
|
|
|
// get accel error, positive means decelerating too slow, need to pitch up (must reverse dec_max, as it is a positive number)
|
|
|
|
|
const float accel_error_forward = _params->back_trans_dec_sp + accel_body_forward; |
|
|
|
|
const float accel_error_forward = dec_sp + accel_body_forward; |
|
|
|
|
|
|
|
|
|
const float pitch_sp_new = _params->dec_to_pitch_ff * _params->back_trans_dec_sp + _accel_to_pitch_integ; |
|
|
|
|
const float pitch_sp_new = _param_vt_b_dec_ff.get() * dec_sp + _accel_to_pitch_integ; |
|
|
|
|
|
|
|
|
|
float integrator_input = _params->dec_to_pitch_i * accel_error_forward; |
|
|
|
|
float integrator_input = _param_vt_b_dec_i.get() * accel_error_forward; |
|
|
|
|
|
|
|
|
|
if ((pitch_sp_new >= pitch_lim && accel_error_forward > 0.0f) || |
|
|
|
|
(pitch_sp_new <= 0.f && accel_error_forward < 0.0f)) { |
|
|
|
@ -288,15 +302,15 @@ void VtolType::check_quadchute_condition()
@@ -288,15 +302,15 @@ void VtolType::check_quadchute_condition()
|
|
|
|
|
Eulerf euler = Quatf(_v_att->q); |
|
|
|
|
|
|
|
|
|
// fixed-wing minimum altitude
|
|
|
|
|
if (_params->fw_min_alt > FLT_EPSILON) { |
|
|
|
|
if (_param_vt_fw_min_alt.get() > FLT_EPSILON) { |
|
|
|
|
|
|
|
|
|
if (-(_local_pos->z) < _params->fw_min_alt) { |
|
|
|
|
if (-(_local_pos->z) < _param_vt_fw_min_alt.get()) { |
|
|
|
|
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::MinimumAltBreached); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// adaptive quadchute
|
|
|
|
|
if (_params->fw_alt_err > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled) { |
|
|
|
|
if (_param_vt_fw_alt_err.get() > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled) { |
|
|
|
|
|
|
|
|
|
// We use tecs for tracking in FW and local_pos_sp during transitions
|
|
|
|
|
if (_tecs_running) { |
|
|
|
@ -305,7 +319,7 @@ void VtolType::check_quadchute_condition()
@@ -305,7 +319,7 @@ void VtolType::check_quadchute_condition()
|
|
|
|
|
_ra_hrate_sp = (49 * _ra_hrate_sp + _tecs_status->height_rate_setpoint) / 50; |
|
|
|
|
|
|
|
|
|
// are we dropping while requesting significant ascend?
|
|
|
|
|
if (((_tecs_status->altitude_sp - _tecs_status->altitude_filtered) > _params->fw_alt_err) && |
|
|
|
|
if (((_tecs_status->altitude_sp - _tecs_status->altitude_filtered) > _param_vt_fw_alt_err.get()) && |
|
|
|
|
(_ra_hrate < -1.0f) && |
|
|
|
|
(_ra_hrate_sp > 1.0f)) { |
|
|
|
|
|
|
|
|
@ -313,7 +327,7 @@ void VtolType::check_quadchute_condition()
@@ -313,7 +327,7 @@ void VtolType::check_quadchute_condition()
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
const bool height_error = _local_pos->z_valid && ((-_local_pos_sp->z - -_local_pos->z) > _params->fw_alt_err); |
|
|
|
|
const bool height_error = _local_pos->z_valid && ((-_local_pos_sp->z - -_local_pos->z) > _param_vt_fw_alt_err.get()); |
|
|
|
|
const bool height_rate_error = _local_pos->v_z_valid && (_local_pos->vz > 1.0f) && (_local_pos->z_deriv > 1.0f); |
|
|
|
|
|
|
|
|
|
if (height_error && height_rate_error) { |
|
|
|
@ -323,17 +337,17 @@ void VtolType::check_quadchute_condition()
@@ -323,17 +337,17 @@ void VtolType::check_quadchute_condition()
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// fixed-wing maximum pitch angle
|
|
|
|
|
if (_params->fw_qc_max_pitch > 0) { |
|
|
|
|
if (_param_vt_fw_qc_p.get() > 0) { |
|
|
|
|
|
|
|
|
|
if (fabsf(euler.theta()) > fabsf(math::radians(_params->fw_qc_max_pitch))) { |
|
|
|
|
if (fabsf(euler.theta()) > fabsf(math::radians(static_cast<float>(_param_vt_fw_qc_p.get())))) { |
|
|
|
|
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::MaximumPitchExceeded); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// fixed-wing maximum roll angle
|
|
|
|
|
if (_params->fw_qc_max_roll > 0) { |
|
|
|
|
if (_param_vt_fw_qc_r.get() > 0) { |
|
|
|
|
|
|
|
|
|
if (fabsf(euler.phi()) > fabsf(math::radians(_params->fw_qc_max_roll))) { |
|
|
|
|
if (fabsf(euler.phi()) > fabsf(math::radians(static_cast<float>(_param_vt_fw_qc_r.get())))) { |
|
|
|
|
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::MaximumRollExceeded); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
@ -342,15 +356,15 @@ void VtolType::check_quadchute_condition()
@@ -342,15 +356,15 @@ void VtolType::check_quadchute_condition()
|
|
|
|
|
|
|
|
|
|
bool VtolType::set_idle_mc() |
|
|
|
|
{ |
|
|
|
|
if (_params->ctrl_alloc == 1) { |
|
|
|
|
if (_param_sys_ctrl_alloc.get()) { |
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
unsigned pwm_value = _params->idle_pwm_mc; |
|
|
|
|
unsigned pwm_value = _param_vt_idle_pwm_mc.get(); |
|
|
|
|
struct pwm_output_values pwm_values {}; |
|
|
|
|
|
|
|
|
|
for (int i = 0; i < num_outputs_max; i++) { |
|
|
|
|
if (is_channel_set(i, generate_bitmap_from_channel_numbers(_params->vtol_motor_id))) { |
|
|
|
|
if (is_channel_set(i, generate_bitmap_from_channel_numbers(_param_vt_mot_id.get()))) { |
|
|
|
|
pwm_values.values[i] = pwm_value; |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
@ -365,14 +379,14 @@ bool VtolType::set_idle_mc()
@@ -365,14 +379,14 @@ bool VtolType::set_idle_mc()
|
|
|
|
|
|
|
|
|
|
bool VtolType::set_idle_fw() |
|
|
|
|
{ |
|
|
|
|
if (_params->ctrl_alloc == 1) { |
|
|
|
|
if (_param_sys_ctrl_alloc.get()) { |
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
struct pwm_output_values pwm_values {}; |
|
|
|
|
|
|
|
|
|
for (int i = 0; i < num_outputs_max; i++) { |
|
|
|
|
if (is_channel_set(i, generate_bitmap_from_channel_numbers(_params->vtol_motor_id))) { |
|
|
|
|
if (is_channel_set(i, generate_bitmap_from_channel_numbers(_param_vt_mot_id.get()))) { |
|
|
|
|
pwm_values.values[i] = PWM_DEFAULT_MIN; |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
@ -387,7 +401,7 @@ bool VtolType::set_idle_fw()
@@ -387,7 +401,7 @@ bool VtolType::set_idle_fw()
|
|
|
|
|
|
|
|
|
|
bool VtolType::apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_type type) |
|
|
|
|
{ |
|
|
|
|
const char *dev = _params->vt_mc_on_fmu ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH; |
|
|
|
|
const char *dev = _param_vt_mc_on_fmu.get() ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH; |
|
|
|
|
|
|
|
|
|
int fd = px4_open(dev, 0); |
|
|
|
|
|
|
|
|
@ -418,7 +432,7 @@ bool VtolType::apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_
@@ -418,7 +432,7 @@ bool VtolType::apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_
|
|
|
|
|
|
|
|
|
|
void VtolType::set_all_motor_state(const motor_state target_state, const int value) |
|
|
|
|
{ |
|
|
|
|
if (_params->ctrl_alloc == 1) { |
|
|
|
|
if (_param_sys_ctrl_alloc.get()) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -428,7 +442,7 @@ void VtolType::set_all_motor_state(const motor_state target_state, const int val
@@ -428,7 +442,7 @@ void VtolType::set_all_motor_state(const motor_state target_state, const int val
|
|
|
|
|
|
|
|
|
|
void VtolType::set_main_motor_state(const motor_state target_state, const int value) |
|
|
|
|
{ |
|
|
|
|
if (_params->ctrl_alloc == 1) { |
|
|
|
|
if (_param_sys_ctrl_alloc.get()) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -442,7 +456,7 @@ void VtolType::set_main_motor_state(const motor_state target_state, const int va
@@ -442,7 +456,7 @@ void VtolType::set_main_motor_state(const motor_state target_state, const int va
|
|
|
|
|
|
|
|
|
|
void VtolType::set_alternate_motor_state(const motor_state target_state, const int value) |
|
|
|
|
{ |
|
|
|
|
if (_params->ctrl_alloc == 1) { |
|
|
|
|
if (_param_sys_ctrl_alloc.get()) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -479,7 +493,7 @@ bool VtolType::set_motor_state(const motor_state target_state, const int32_t cha
@@ -479,7 +493,7 @@ bool VtolType::set_motor_state(const motor_state target_state, const int32_t cha
|
|
|
|
|
|
|
|
|
|
for (int i = 0; i < num_outputs_max; i++) { |
|
|
|
|
if (is_channel_set(i, channel_bitmap)) { |
|
|
|
|
_current_max_pwm_values.values[i] = _params->idle_pwm_mc; |
|
|
|
|
_current_max_pwm_values.values[i] = _param_vt_idle_pwm_mc.get(); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -537,7 +551,7 @@ float VtolType::pusher_assist()
@@ -537,7 +551,7 @@ float VtolType::pusher_assist()
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// disable pusher assist depending on setting of forward_thrust_enable_mode:
|
|
|
|
|
switch (_params->vt_forward_thrust_enable_mode) { |
|
|
|
|
switch (_param_vt_fwd_thrust_en.get()) { |
|
|
|
|
case DISABLE: // disable in all modes
|
|
|
|
|
return 0.0f; |
|
|
|
|
break; |
|
|
|
@ -552,14 +566,14 @@ float VtolType::pusher_assist()
@@ -552,14 +566,14 @@ float VtolType::pusher_assist()
|
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case ENABLE_ABOVE_MPC_LAND_ALT1: // disable if below MPC_LAND_ALT1
|
|
|
|
|
if (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt1)) { |
|
|
|
|
if (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _param_mpc_land_alt1.get())) { |
|
|
|
|
return 0.0f; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case ENABLE_ABOVE_MPC_LAND_ALT2: // disable if below MPC_LAND_ALT2
|
|
|
|
|
if (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt2)) { |
|
|
|
|
if (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _param_mpc_land_alt2.get())) { |
|
|
|
|
return 0.0f; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -569,7 +583,7 @@ float VtolType::pusher_assist()
@@ -569,7 +583,7 @@ float VtolType::pusher_assist()
|
|
|
|
|
if ((_attc->get_pos_sp_triplet()->current.valid |
|
|
|
|
&& _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND |
|
|
|
|
&& _v_control_mode->flag_control_auto_enabled) || |
|
|
|
|
(!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt1))) { |
|
|
|
|
(!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _param_mpc_land_alt1.get()))) { |
|
|
|
|
return 0.0f; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -579,7 +593,7 @@ float VtolType::pusher_assist()
@@ -579,7 +593,7 @@ float VtolType::pusher_assist()
|
|
|
|
|
if ((_attc->get_pos_sp_triplet()->current.valid |
|
|
|
|
&& _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND |
|
|
|
|
&& _v_control_mode->flag_control_auto_enabled) || |
|
|
|
|
(!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt2))) { |
|
|
|
|
(!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _param_mpc_land_alt2.get()))) { |
|
|
|
|
return 0.0f; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -588,7 +602,7 @@ float VtolType::pusher_assist()
@@ -588,7 +602,7 @@ float VtolType::pusher_assist()
|
|
|
|
|
|
|
|
|
|
// if the thrust scale param is zero or the drone is not in some position or altitude control mode,
|
|
|
|
|
// then the pusher-for-pitch strategy is disabled and we can return
|
|
|
|
|
if (_params->forward_thrust_scale < FLT_EPSILON || !(_v_control_mode->flag_control_position_enabled |
|
|
|
|
if (_param_vt_fwd_thrust_sc.get() < FLT_EPSILON || !(_v_control_mode->flag_control_position_enabled |
|
|
|
|
|| _v_control_mode->flag_control_altitude_enabled)) { |
|
|
|
|
return 0.0f; |
|
|
|
|
} |
|
|
|
@ -619,11 +633,12 @@ float VtolType::pusher_assist()
@@ -619,11 +633,12 @@ float VtolType::pusher_assist()
|
|
|
|
|
// normalized pusher support throttle (standard VTOL) or tilt (tiltrotor), initialize to 0
|
|
|
|
|
float forward_thrust = 0.0f; |
|
|
|
|
|
|
|
|
|
float pitch_setpoint_min = _params->pitch_min_rad; |
|
|
|
|
float pitch_setpoint_min = math::radians(_param_vt_ptch_min.get()); |
|
|
|
|
|
|
|
|
|
if (_attc->get_pos_sp_triplet()->current.valid |
|
|
|
|
&& _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { |
|
|
|
|
pitch_setpoint_min = _params->land_pitch_min_rad; // set min pitch during LAND (usually lower to generate less lift)
|
|
|
|
|
pitch_setpoint_min = math::radians( |
|
|
|
|
_param_vt_lnd_ptch_min.get()); // set min pitch during LAND (usually lower to generate less lift)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// only allow pitching down up to threshold, the rest of the desired
|
|
|
|
@ -633,7 +648,7 @@ float VtolType::pusher_assist()
@@ -633,7 +648,7 @@ float VtolType::pusher_assist()
|
|
|
|
|
// desired roll angle in heading frame stays the same
|
|
|
|
|
const float roll_new = -asinf(body_z_sp(1)); |
|
|
|
|
|
|
|
|
|
forward_thrust = (sinf(pitch_setpoint_min) - sinf(pitch_setpoint)) * _params->forward_thrust_scale; |
|
|
|
|
forward_thrust = (sinf(pitch_setpoint_min) - sinf(pitch_setpoint)) * _param_vt_fwd_thrust_sc.get(); |
|
|
|
|
// limit forward actuation to [0, 0.9]
|
|
|
|
|
forward_thrust = math::constrain(forward_thrust, 0.0f, 0.9f); |
|
|
|
|
|
|
|
|
@ -684,10 +699,10 @@ float VtolType::getFrontTransitionTimeFactor() const
@@ -684,10 +699,10 @@ float VtolType::getFrontTransitionTimeFactor() const
|
|
|
|
|
|
|
|
|
|
float VtolType::getMinimumFrontTransitionTime() const |
|
|
|
|
{ |
|
|
|
|
return getFrontTransitionTimeFactor() * _params->front_trans_time_min; |
|
|
|
|
return getFrontTransitionTimeFactor() * _param_vt_trans_min_tm.get(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
float VtolType::getOpenLoopFrontTransitionTime() const |
|
|
|
|
{ |
|
|
|
|
return getFrontTransitionTimeFactor() * _params->front_trans_time_openloop; |
|
|
|
|
return getFrontTransitionTimeFactor() * _param_vt_f_tr_ol_tm.get(); |
|
|
|
|
} |
|
|
|
|