From fd18ba649678a8aa5f1b871b2f6b3d770c1d87d3 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Tue, 19 May 2020 14:24:03 +0300 Subject: [PATCH] tiltrotor: allow to finish transition if groundspeed is below cruise speed Signed-off-by: RomanBapst --- src/modules/vtol_att_control/tiltrotor.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 63a8192f86..21564e9b6e 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -123,9 +123,12 @@ void Tiltrotor::update_vtol_state() break; case vtol_mode::TRANSITION_BACK: - float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; + 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) { + 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; }