diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index cd2283f501e0..87703356c99a 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -64,6 +64,7 @@ Standard::Standard(VtolAttitudeControl *attc) : _params_handles_standard.airspeed_blend = param_find("VT_ARSP_BLEND"); _params_handles_standard.airspeed_trans = param_find("VT_ARSP_TRANS"); _params_handles_standard.front_trans_timeout = param_find("VT_TRANS_TIMEOUT"); + _params_handles_standard.front_trans_time_min = param_find("VT_TRANS_MIN_TM"); } Standard::~Standard() @@ -100,6 +101,10 @@ Standard::parameters_update() /* timeout for transition to fw mode */ param_get(_params_handles_standard.front_trans_timeout, &_params_standard.front_trans_timeout); + /* minimum time for transition to fw mode */ + param_get(_params_handles_standard.front_trans_time_min, &_params_standard.front_trans_time_min); + + return OK; } @@ -153,6 +158,7 @@ void Standard::update_vtol_state() // start transition to fw mode _vtol_schedule.flight_mode = TRANSITION_TO_FW; _vtol_schedule.transition_start = hrt_absolute_time(); + warnx("Front transition started"); } else if (_vtol_schedule.flight_mode == FW_MODE) { // in fw mode @@ -164,12 +170,17 @@ void Standard::update_vtol_state() } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { // continue the transition to fw mode while monitoring airspeed for a final switch to fw mode - if (_airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_trans || !_armed->armed) { + if ((_airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_trans && + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) + > (_params_standard.front_trans_time_min * 1000000.0f)) || + !_armed->armed) { + warnx("Front transition complete"); _vtol_schedule.flight_mode = FW_MODE; // we can turn off the multirotor motors now _flag_enable_mc_motors = false; // don't set pusher throttle here as it's being ramped up elsewhere _trans_finished_ts = hrt_absolute_time(); + warnx("Front transition complete"); } } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { @@ -211,8 +222,11 @@ void Standard::update_transition_state() (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_dur * 1000000.0f); } - // do blending of mc and fw controls if a blending airspeed has been provided - if (_airspeed_trans_blend_margin > 0.0f && _airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_blend) { + // do blending of mc and fw controls if a blending airspeed has been provided and the minimum transition time has passed + if (_airspeed_trans_blend_margin > 0.0f && + _airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_blend && + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_time_min * 1000000.0f) + ) { float weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params_standard.airspeed_blend) / _airspeed_trans_blend_margin; _mc_roll_weight = weight; @@ -230,7 +244,7 @@ void Standard::update_transition_state() // check front transition timeout if (_params_standard.front_trans_timeout > FLT_EPSILON) { - if ( (float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_timeout * 1000000.0f)) { + if ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_timeout * 1000000.0f)) { // transition timeout occured, abort transition _attc->abort_front_transition(); } diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index 38676d44c053..de3ca96e38dd 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -72,6 +72,7 @@ class Standard : public VtolType float airspeed_blend; float airspeed_trans; float front_trans_timeout; + float front_trans_time_min; } _params_standard; struct { @@ -81,6 +82,7 @@ class Standard : public VtolType param_t airspeed_blend; param_t airspeed_trans; param_t front_trans_timeout; + param_t front_trans_time_min; } _params_handles_standard; enum vtol_mode { diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index df9de15b312a..66cfae160fc6 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -253,3 +253,14 @@ PARAM_DEFINE_INT32(VT_WV_LTR_EN, 0); * @group VTOL Attitude Control */ PARAM_DEFINE_FLOAT(VT_TRANS_TIMEOUT, 15.0f); + +/** + * Front transition minimum time + * + * Minimum time in seconds for front transition. + * + * @min 0.0 + * @max 10.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TRANS_MIN_TM, 2.0f);