From 599a66c8a5f33fd9e139ab950e716d0e36a3e809 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 11 May 2023 17:50:46 +0200 Subject: [PATCH] Tailsitter: make pitch angle thresholds for transition in Stabilized depending on max manual pitch Transitions in Stabilized mode are done manually, the pilot controls the pitch angle and if it's above the threshold the transition is declared finished (plus airspeed check for front transition). Thus we can't have fixed thresholds but need to link them to the actual max pitch angle in Stabilized mode. Signed-off-by: Silvan Fuhrer --- src/modules/vtol_att_control/tailsitter.cpp | 23 +++++++++++++++------ src/modules/vtol_att_control/tailsitter.h | 11 ++++++++-- 2 files changed, 26 insertions(+), 8 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 7c7c4d4202..ff812f926e 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2015-2023 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 @@ -42,9 +42,6 @@ #include "tailsitter.h" #include "vtol_att_control_main.h" -#define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2 -#define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC - using namespace matrix; Tailsitter::Tailsitter(VtolAttitudeControl *attc) : @@ -91,8 +88,15 @@ void Tailsitter::update_vtol_state() case vtol_mode::TRANSITION_BACK: const float pitch = Eulerf(Quatf(_v_att->q)).theta(); + float pitch_threshold_mc = PITCH_THRESHOLD_AUTO_TRANSITION_TO_MC; + + // if doing transition in Stabilized mode set threshold to max angle plus 5° margin + if (!_v_control_mode->flag_control_altitude_enabled) { + pitch_threshold_mc = math::radians(-_param_mpc_tilt_max.get() - 5.f); + } + // check if we have reached pitch angle to switch to MC mode - if (pitch >= PITCH_TRANSITION_BACK || _time_since_trans_start > _param_vt_b_trans_dur.get()) { + if (pitch >= pitch_threshold_mc || _time_since_trans_start > _param_vt_b_trans_dur.get()) { _vtol_mode = vtol_mode::MC_MODE; } @@ -325,7 +329,14 @@ bool Tailsitter::isFrontTransitionCompletedBase() bool transition_to_fw = false; const float pitch = Eulerf(Quatf(_v_att->q)).theta(); - if (pitch <= PITCH_TRANSITION_FRONT_P1) { + float pitch_threshold_fw = PITCH_THRESHOLD_AUTO_TRANSITION_TO_FW; + + // if doing transition in Stabilized mode set threshold to max angle minus 5° margin + if (!_v_control_mode->flag_control_altitude_enabled) { + pitch_threshold_fw = math::radians(-_param_mpc_tilt_max.get() + 5.f); + } + + if (pitch <= pitch_threshold_fw) { if (airspeed_triggers_transition) { transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get() ; diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index 965186a1c0..3770caec2c 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2015-2023 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 @@ -48,6 +48,12 @@ #include #include +// [rad] Pitch threshold required for completing transition to fixed-wing in automatic transitions +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° + class Tailsitter : public VtolType { @@ -82,7 +88,8 @@ private: bool isFrontTransitionCompletedBase() override; DEFINE_PARAMETERS_CUSTOM_PARENT(VtolType, - (ParamFloat) _param_fw_psp_off + (ParamFloat) _param_fw_psp_off, + (ParamFloat) _param_mpc_tilt_max )