From c5e0e58e6ae4c488a31fd2d1ee6840588972e88c Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 22 Aug 2014 22:38:53 +0200 Subject: [PATCH] OP-1454 - fix weak leveling bug --- flight/modules/Stabilization/outerloop.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/flight/modules/Stabilization/outerloop.c b/flight/modules/Stabilization/outerloop.c index b5ffb4d35..7d865b556 100644 --- a/flight/modules/Stabilization/outerloop.c +++ b/flight/modules/Stabilization/outerloop.c @@ -228,7 +228,11 @@ static void stabilizationOuterloopTask() // That would be changed to Attitude mode max angle affecting Kp // Also does not take dT into account { - float rate_input = cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t] * stabilizationDesiredAxis[t] / cast_struct_to_array(stabSettings.stabBank, stabSettings.stabBank.RollMax)[t]; + float stickinput[3]; + stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f); + stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f); + stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f); + float rate_input = stickinput[t] * cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t]; float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp; weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate);