From a7b3a5713d6eee33ec8a84c90f7dca76c35e779f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 4 May 2022 08:29:48 +1000 Subject: [PATCH] AP_Scripting: fixed non-oscillating yaw limit --- libraries/AP_Scripting/applets/VTOL-quicktune.lua | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Scripting/applets/VTOL-quicktune.lua b/libraries/AP_Scripting/applets/VTOL-quicktune.lua index ed67a7f62f..706f4f2a83 100644 --- a/libraries/AP_Scripting/applets/VTOL-quicktune.lua +++ b/libraries/AP_Scripting/applets/VTOL-quicktune.lua @@ -295,10 +295,7 @@ function gain_limit(pname) return 0.0 end -function reached_limit(pname, gain, srate) - if srate > QUIK_OSC_SMAX:get() then - return true - end +function reached_limit(pname, gain) local limit = gain_limit(pname) if limit > 0.0 and gain >= limit then return true @@ -368,8 +365,13 @@ function update() local srate = get_slew_rate(axis) local pname = get_pname(axis, stage) local P = params[pname] - if reached_limit(pname, P:get(), srate) then + local oscillating = srate > QUIK_OSC_SMAX:get() + local limited = reached_limit(pname, P:get()) + if limited or oscillating then local reduction = (100.0-QUIK_GAIN_MARGIN:get())*0.01 + if not oscillating then + reduction = 1.0 + end local new_gain = P:get() * reduction local limit = gain_limit(pname) if limit > 0.0 and new_gain > limit then