From b5988ed38fc9d50ea722b2dcf33470a2f4bd4446 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 19 Apr 2024 19:58:05 +0200 Subject: [PATCH] VTOL Tailsitter: add back transition throttle blending Signed-off-by: Silvan Fuhrer --- src/modules/vtol_att_control/tailsitter.cpp | 10 ++++++++++ src/modules/vtol_att_control/tailsitter.h | 4 ++++ 2 files changed, 14 insertions(+) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 272ad4cb44..ea326b059a 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -225,6 +225,11 @@ void Tailsitter::update_transition_state() _v_att_sp->thrust_body[2] = _mc_virtual_att_sp->thrust_body[2]; + if (_vtol_mode == vtol_mode::TRANSITION_BACK) { + const float progress = math::constrain(_time_since_trans_start / B_TRANS_THRUST_BLENDING_DURATION, 0.f, 1.f); + blendThrottleBeginningBackTransition(progress); + } + _v_att_sp->timestamp = hrt_absolute_time(); const Eulerf euler_sp(_q_trans_sp); @@ -351,3 +356,8 @@ void Tailsitter::blendThrottleAfterFrontTransition(float scale) // note: MC throttle is negative (as in negative z), while FW throttle is positive (positive x) _v_att_sp->thrust_body[0] = scale * _v_att_sp->thrust_body[0] + (1.f - scale) * (-_last_thr_in_mc); } + +void Tailsitter::blendThrottleBeginningBackTransition(float scale) +{ + _v_att_sp->thrust_body[2] = scale * _v_att_sp->thrust_body[2] + (1.f - scale) * (-_last_thr_in_fw_mode); +} diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index 39265a2e97..135acd6c92 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -54,6 +54,9 @@ static constexpr float PITCH_THRESHOLD_AUTO_TRANSITION_TO_FW = -1.05f; // -60° // [rad] Pitch threshold required for completing transition to hover in automatic transitions static constexpr float PITCH_THRESHOLD_AUTO_TRANSITION_TO_MC = -0.26f; // -15° +// [s] Thrust blending duration from fixed-wing to back transition throttle +static constexpr float B_TRANS_THRUST_BLENDING_DURATION = 0.5f; + class Tailsitter : public VtolType { @@ -67,6 +70,7 @@ public: void fill_actuator_outputs() override; void waiting_on_tecs() override; void blendThrottleAfterFrontTransition(float scale) override; + void blendThrottleBeginningBackTransition(float scale); private: enum class vtol_mode {