From 0ad2bf15bcb42e2e1626a7cc5ffa6d6914871079 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 30 Jul 2021 22:38:19 +0930 Subject: [PATCH] AC_AttitudeControl: Support thrust to weight of 10:1 --- libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index da42ba74d9..d28206cd8f 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -295,8 +295,9 @@ float AC_AttitudeControl_Multi::get_throttle_boosted(float throttle_in) // inverted_factor reduces from 1 to 0 for tilt angles between 60 and 90 degrees float cos_tilt = _ahrs.cos_pitch() * _ahrs.cos_roll(); - float inverted_factor = constrain_float(2.0f * cos_tilt, 0.0f, 1.0f); - float boost_factor = 1.0f / constrain_float(cos_tilt, 0.5f, 1.0f); + float inverted_factor = constrain_float(10.0f * cos_tilt, 0.0f, 1.0f); + float cos_tilt_target = cosf(_thrust_angle); + float boost_factor = 1.0f / constrain_float(cos_tilt_target, 0.1f, 1.0f); float throttle_out = throttle_in * inverted_factor * boost_factor; _angle_boost = constrain_float(throttle_out - throttle_in, -1.0f, 1.0f);