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

Allow dyn lpf to initialize if static lpf config is 0.

Browse source 
Change min > max per review.

Fix missing ifdef.
This commit is contained in:
Kenneth Mitchell 2019-01-13 12:57:58 -05:00
parent 7f58ecc77f
commit f6e456cd06
No known key found for this signature in database
GPG key ID: E27133AAF586AB21
3 changed files with 35 additions and 9 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -315,14 +315,18 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
//1st Dterm Lowpass Filter
if (pidProfile->dterm_lowpass_hz == 0 || pidProfile->dterm_lowpass_hz > pidFrequencyNyquist) {
dtermLowpassApplyFn = nullFilterApply;
} else {
uint16_t dterm_lowpass_hz = pidProfile->dterm_lowpass_hz;
#ifdef USE_DYN_LPF
dterm_lowpass_hz = MAX(pidProfile->dterm_lowpass_hz, pidProfile->dyn_lpf_dterm_min_hz);
#endif
if (dterm_lowpass_hz > 0 && dterm_lowpass_hz < pidFrequencyNyquist) {
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));
pt1FilterInit(&dtermLowpass[axis].pt1Filter, pt1FilterGain(dterm_lowpass_hz, dT));
}
break;
case FILTER_BIQUAD:
@ -332,13 +336,15 @@ void pidInitFilters(const pidProfile_t *pidProfile)
dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
#endif
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, pidProfile->dterm_lowpass_hz, targetPidLooptime);
biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, dterm_lowpass_hz, targetPidLooptime);
}
break;
default:
dtermLowpassApplyFn = nullFilterApply;
break;
}
} else {
dtermLowpassApplyFn = nullFilterApply;
}
//2nd Dterm Lowpass Filter
@ -609,7 +615,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
#endif
#ifdef USE_DYN_LPF
if (pidProfile->dyn_lpf_dterm_min_hz > 0 && pidProfile->dyn_lpf_dterm_max_hz > pidProfile->dyn_lpf_dterm_min_hz) {
if (pidProfile->dyn_lpf_dterm_min_hz > 0) {
switch (pidProfile->dterm_filter_type) {
case FILTER_PT1:
dynLpfFilter = DYN_LPF_PT1;