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

More aggresive FF for averaging <3

This commit is contained in:
ctzsnooze 2020-02-08 13:01:16 +11:00
parent be016dc794
commit 86c309454f
1 changed files with 18 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -44,6 +44,7 @@ static float prevAcceleration[XYZ_AXIS_COUNT];
static float prevRawSetpoint[XYZ_AXIS_COUNT];
static float prevDeltaImpl[XYZ_AXIS_COUNT];
static bool bigStep[XYZ_AXIS_COUNT];
static uint8_t averagingCount;
// Configuration
static float ffMaxRateLimit[XYZ_AXIS_COUNT];
@ -51,11 +52,11 @@ static float ffMaxRate[XYZ_AXIS_COUNT];
void interpolatedSpInit(const pidProfile_t *pidProfile) {
const float ffMaxRateScale = pidProfile->ff_max_rate_limit * 0.01f;
uint8_t j = pidProfile->ff_interpolate_sp;
averagingCount = pidProfile->ff_interpolate_sp;
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
ffMaxRate[i] = applyCurve(i, 1.0f);
ffMaxRateLimit[i] = ffMaxRate[i] * ffMaxRateScale;
laggedMovingAverageInit(&setpointDeltaAvg[i].filter, j, (float *)&setpointDeltaAvg[i].buf[0]);
laggedMovingAverageInit(&setpointDeltaAvg[i].filter, averagingCount, (float *)&setpointDeltaAvg[i].buf[0]);
}
}
@ -113,22 +114,27 @@ FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterp
} else if (holdCount[axis] == 2) {
// interpolation was not applied
} else if (holdCount[axis] == 3) {
// after persistent flat period or recent big step up, no boost
// reduces jitter from boost when flying smoothly
// after persistent flat period, no boost
// reduces jitter from boost when flying smooth lines
// but only when no ff_averaging is active, eg hard core race setups
// WARNING: this means no boost if ADC is active on FrSky radios
setpointAccelerationModified = 0.0f;
if (averagingCount > 1) {
setpointAccelerationModified /= averagingCount;
}
}
holdCount[axis] = 0;
}
}
// smooth deadband type suppression of FF when sticks are centred.
const float rawSetpointCentred = fabsf(rawSetpoint) / 2.0f;
if (rawSetpointCentred < 1.0f) {
// force zero FF when sticks are centred for smoothness
setpointSpeedModified *= rawSetpointCentred;
setpointAccelerationModified *= rawSetpointCentred;
holdCount[axis] = 4;
// smooth deadband type suppression of FF jitter when sticks are at or returning to centre
// only when ff_averaging is 3 or more, for HD or cinematic flying
if (averagingCount > 2) {
const float rawSetpointCentred = fabsf(rawSetpoint) / averagingCount;
if (rawSetpointCentred < 1.0f) {
setpointSpeedModified *= rawSetpointCentred;
setpointAccelerationModified *= rawSetpointCentred;
holdCount[axis] = 4;
}
}
setpointDeltaImpl[axis] = setpointSpeedModified * pidGetDT();