Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 05:15:25 +03:00
Code Issues Wiki Activity

tidy up code

Browse source
This commit is contained in:
ctzsnooze 2019-09-10 20:05:50 +10:00
parent e1a8a80584
commit 582e42506a
1 changed files with 9 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

21
src/main/flight/interpolated_setpoint.c
View file

@ -62,26 +62,23 @@ FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterp
const float setpointAcceleration = (setpointSpeed - prevSetpointSpeed[axis]) * pidGetDT();
setpointDeltaImpl[axis] = setpointSpeed * pidGetDT();
const float ffBoostFactor = pidGetFfBoostFactor();
float clip = 1.0f;
float boostAmount = 0.0f;
if ((axis == FD_ROLL)||(axis == FD_PITCH)) {
if (ffBoostFactor != 0.0f) {
if (pidGetSpikeLimitInverse()) {
clip = 1 / (1 + (setpointAcceleration * setpointAcceleration * pidGetSpikeLimitInverse()));
clip *= clip;
}
// prevent kick-back spike at max deflection
if (fabsf(rawSetpoint) < 0.95f * ffMaxRate[axis]) {
boostAmount = ffBoostFactor * setpointAcceleration;
}
if (axis != FD_YAW && ffBoostFactor != 0.0f) {
if (pidGetSpikeLimitInverse()) {
clip = 1 / (1 + (setpointAcceleration * setpointAcceleration * pidGetSpikeLimitInverse()));
clip *= clip;
}
// prevent kick-back spike at max deflection
if (fabsf(rawSetpoint) < 0.95f * ffMaxRate[axis]) {
boostAmount = ffBoostFactor * setpointAcceleration;
}
}
prevSetpointSpeed[axis] = setpointSpeed;
prevSetpointAcceleration[axis] = setpointAcceleration;
prevRawSetpoint[axis] = rawSetpoint;
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, setpointDeltaImpl[axis] * 1000);
DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, boostAmount * 1000);