From 9ba0d5b706fa25f85fc3c7e5a698f3d4f4d6c36f Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 7 Feb 2020 15:27:47 +0100 Subject: [PATCH] tiltrotor tilt support: fix thrust compensation constraining Signed-off-by: Silvan Fuhrer --- src/modules/vtol_att_control/tiltrotor.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 183f2090efc0..59a5b50aa4f2 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -395,11 +395,10 @@ void Tiltrotor::fill_actuator_outputs() float Tiltrotor::thrust_compensation_for_tilt() { - // only compensate for tilt angle up to 0.5 * max tilt float compensated_tilt = math::constrain(_tilt_control, 0.0f, 0.5f); - // 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); + // increase vertical thrust by 1/cos(tilt), limmit to [-1,0] + return math::constrain(_v_att_sp->thrust_body[2] / cosf(compensated_tilt * M_PI_2_F), -1.0f, 0.0f); }