Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-14 03:50:02 +03:00
Code Issues Wiki Activity

add PT2 and PT3 lowpass filter options

Browse source
This commit is contained in:
ctzsnooze 2021-05-18 09:59:27 +10:00
parent 636d563abe
commit 3b62b2e5d4
8 changed files with 123 additions and 29 deletions
Download patch file Download diff file Expand all files Collapse all files

18
src/main/flight/pid.c
View file

@ -1231,15 +1231,27 @@ void dynLpfDTermUpdate(float throttle)
} else {
cutoffFreq = fmax(dynThrottle(throttle) * pidRuntime.dynLpfMax, pidRuntime.dynLpfMin);
}
if (pidRuntime.dynLpfFilter == DYN_LPF_PT1) {
switch (pidRuntime.dynLpfFilter) {
case DYN_LPF_PT1:
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt1FilterUpdateCutoff(&pidRuntime.dtermLowpass[axis].pt1Filter, pt1FilterGain(cutoffFreq, pidRuntime.dT));
}
} else if (pidRuntime.dynLpfFilter == DYN_LPF_BIQUAD) {
break;
case DYN_LPF_BIQUAD:
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterUpdateLPF(&pidRuntime.dtermLowpass[axis].biquadFilter, cutoffFreq, targetPidLooptime);
}
break;
case DYN_LPF_PT2:
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt2FilterUpdateCutoff(&pidRuntime.dtermLowpass[axis].pt2Filter, pt2FilterGain(cutoffFreq, pidRuntime.dT));
}
break;
case DYN_LPF_PT3:
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt3FilterUpdateCutoff(&pidRuntime.dtermLowpass[axis].pt3Filter, pt3FilterGain(cutoffFreq, pidRuntime.dT));
}
break;
}
}
}