From e3310796ce63d44f1680d2ebe5a8870ac4212194 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 7 Feb 2020 10:24:48 +0100 Subject: [PATCH] tiltrotor: limmit the adapted thrust for tilt to [0,1] Signed-off-by: Silvan Fuhrer --- src/modules/vtol_att_control/tiltrotor.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 3e0b069f2c..aa3c2d2f08 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -401,6 +401,7 @@ float Tiltrotor::thrust_compensation_for_tilt() compensated_tilt = compensated_tilt < 0.0f ? 0.0f : compensated_tilt; compensated_tilt = compensated_tilt > 0.5f ? 0.5f : compensated_tilt; - return _v_att_sp->thrust_body[2] / cosf(compensated_tilt * M_PI_2_F); + // increase vertical thrust by 1/cos(tilt), limmit to [0,1] + return math::constrain(_v_att_sp->thrust_body[2] / cosf(compensated_tilt * M_PI_2_F), 0.0f, 1.0f); }