rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-22 14:19:42 +01:00
Code Issues Releases Activity

OP-1309 correctly limit the maximum attitude-loop-rate in rattitude mode

Browse Source
This commit is contained in:
Corvus Corax 2014-05-01 16:37:22 +02:00
parent 09a5578f5a
commit 7275d9a807
1 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
flight/modules/Stabilization/outerloop.c
View File

@ -158,13 +158,17 @@ static void stabilizationOuterloopTask()
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 rateDesiredAxisRate = stickinput[t] * cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t];
rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
// limit corrective rate to maximum rates to not give it overly large impact over manual rate when joined together
rateDesiredAxis[t] = boundf(pid_apply(&stabSettings.outerPids[t], local_error[t], dT),
-cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t],
cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t]
);
// Compute the weighted average rate desired
// Using max() rather than sqrt() for cpu speed;
// - this makes the stick region into a square;
// - this is a feature!
// - hold a roll angle and add just pitch without the stick sensitivity changing
float magnitude = stickinput[t];
float magnitude = fabsf(stickinput[t]);
if (t < 2) {
magnitude = fmaxf(fabsf(stickinput[0]), fabsf(stickinput[1]));
}