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

cleanup dterm lowpass order

Browse source
This commit is contained in:
Steffen Windoffer 2018-10-20 15:12:40 +02:00
parent 9ab8370379
commit f45ab5bb0a
1 changed files with 24 additions and 23 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -268,14 +268,34 @@ void pidInitFilters(const pidProfile_t *pidProfile)
dtermNotchApplyFn = nullFilterApply; dtermNotchApplyFn = nullFilterApply;
} }
//1st Dterm Lowpass Filter
if (pidProfile->dterm_lowpass_hz == 0 || pidProfile->dterm_lowpass_hz > pidFrequencyNyquist) {
dtermLowpassApplyFn = nullFilterApply;
} else {
switch (pidProfile->dterm_filter_type) {
case FILTER_PT1:
dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt1FilterInit(&dtermLowpass[axis].pt1Filter, pt1FilterGain(pidProfile->dterm_lowpass_hz, dT));
}
break;
case FILTER_BIQUAD:
dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, pidProfile->dterm_lowpass_hz, targetPidLooptime);
}
break;
default:
dtermLowpassApplyFn = nullFilterApply;
break;
}
}
//2nd Dterm Lowpass Filter //2nd Dterm Lowpass Filter
if (pidProfile->dterm_lowpass2_hz == 0 || pidProfile->dterm_lowpass2_hz > pidFrequencyNyquist) { if (pidProfile->dterm_lowpass2_hz == 0 || pidProfile->dterm_lowpass2_hz > pidFrequencyNyquist) {
dtermLowpass2ApplyFn = nullFilterApply; dtermLowpass2ApplyFn = nullFilterApply;
} else { } else {
switch (pidProfile->dterm_filter2_type) { switch (pidProfile->dterm_filter2_type) {
default:
dtermLowpass2ApplyFn = nullFilterApply;
break;
case FILTER_PT1: case FILTER_PT1:
dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply; dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@ -288,27 +308,8 @@ void pidInitFilters(const pidProfile_t *pidProfile)
biquadFilterInitLPF(&dtermLowpass2[axis].biquadFilter, pidProfile->dterm_lowpass2_hz, targetPidLooptime); biquadFilterInitLPF(&dtermLowpass2[axis].biquadFilter, pidProfile->dterm_lowpass2_hz, targetPidLooptime);
} }
break; break;
}
}
if (pidProfile->dterm_lowpass_hz == 0 || pidProfile->dterm_lowpass_hz > pidFrequencyNyquist) {
dtermLowpassApplyFn = nullFilterApply;
} else {
switch (pidProfile->dterm_filter_type) {
default: default:
dtermLowpassApplyFn = nullFilterApply; dtermLowpass2ApplyFn = nullFilterApply;
break;
case FILTER_PT1:
dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt1FilterInit(&dtermLowpass[axis].pt1Filter, pt1FilterGain(pidProfile->dterm_lowpass_hz, dT));
}
break;
case FILTER_BIQUAD:
dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, pidProfile->dterm_lowpass_hz, targetPidLooptime);
}
break; break;
} }
} }