Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 13:25:30 +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(); const float setpointAcceleration = (setpointSpeed - prevSetpointSpeed[axis]) * pidGetDT();
setpointDeltaImpl[axis] = setpointSpeed * pidGetDT(); setpointDeltaImpl[axis] = setpointSpeed * pidGetDT();
const float ffBoostFactor = pidGetFfBoostFactor(); const float ffBoostFactor = pidGetFfBoostFactor();
float clip = 1.0f; float clip = 1.0f;
float boostAmount = 0.0f; float boostAmount = 0.0f;
if ((axis == FD_ROLL)||(axis == FD_PITCH)) { if (axis != FD_YAW && ffBoostFactor != 0.0f) {
if (ffBoostFactor != 0.0f) { if (pidGetSpikeLimitInverse()) {
if (pidGetSpikeLimitInverse()) { clip = 1 / (1 + (setpointAcceleration * setpointAcceleration * pidGetSpikeLimitInverse()));
clip = 1 / (1 + (setpointAcceleration * setpointAcceleration * pidGetSpikeLimitInverse())); clip *= clip;
clip *= clip; }
} // prevent kick-back spike at max deflection
// prevent kick-back spike at max deflection if (fabsf(rawSetpoint) < 0.95f * ffMaxRate[axis]) {
if (fabsf(rawSetpoint) < 0.95f * ffMaxRate[axis]) { boostAmount = ffBoostFactor * setpointAcceleration;
boostAmount = ffBoostFactor * setpointAcceleration;
}
} }
} }
prevSetpointSpeed[axis] = setpointSpeed; prevSetpointSpeed[axis] = setpointSpeed;
prevSetpointAcceleration[axis] = setpointAcceleration; prevSetpointAcceleration[axis] = setpointAcceleration;
prevRawSetpoint[axis] = rawSetpoint; prevRawSetpoint[axis] = rawSetpoint;
if (axis == FD_ROLL) { if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, setpointDeltaImpl[axis] * 1000); DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, setpointDeltaImpl[axis] * 1000);
DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, boostAmount * 1000); DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, boostAmount * 1000);