rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Improved the efficiency of 'applyFfLimit()'.

This commit is contained in:
mikeller 2019-09-22 16:17:51 +12:00
parent 5112b66a0f
commit 2e9f4c76c3
1 changed files with 12 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/main/flight/interpolated_setpoint.c
View File

@ -98,24 +98,27 @@ FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterp
}
FAST_CODE_NOINLINE float applyFfLimit(int axis, float value, float Kp, float currentPidSetpoint) {
if (axis == FD_ROLL) {
switch (axis) {
case FD_ROLL:
DEBUG_SET(DEBUG_FF_LIMIT, 0, value);
}
if (axis == FD_PITCH) {
break;
case FD_PITCH:
DEBUG_SET(DEBUG_FF_LIMIT, 1, value);
break;
}
if (ffMaxRateLimit[axis]) {
if (fabsf(currentPidSetpoint) <= ffMaxRateLimit[axis]) {
value = constrainf(value, (-ffMaxRateLimit[axis] - currentPidSetpoint) * Kp, (ffMaxRateLimit[axis] - currentPidSetpoint) * Kp);
} else {
value = 0;
}
if (fabsf(currentPidSetpoint) <= ffMaxRateLimit[axis]) {
value = constrainf(value, (-ffMaxRateLimit[axis] - currentPidSetpoint) * Kp, (ffMaxRateLimit[axis] - currentPidSetpoint) * Kp);
} else {
value = 0;
}
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FF_LIMIT, 2, value);
}
return value;
}
@ -123,6 +126,4 @@ bool shouldApplyFfLimits(int axis)
{
return ffMaxRateLimit[axis] != 0.0f && axis < FD_YAW;
}
#endif