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

Fix filter bug for trying filter over nyquist

This commit is contained in:
borisbstyle 2017-02-20 01:52:15 +01:00
parent 1e695ab7d6
commit 84105629ea
2 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/main/flight/pid.c
View File

@ -96,9 +96,11 @@ void pidInitFilters(const pidProfile_t *pidProfile)
static firFilterDenoise_t denoisingFilter[2];
static pt1Filter_t pt1FilterYaw;
uint32_t pidFrequencyNyquist = (1.0f / dT) / 2; // No rounding needed
BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2
if (pidProfile->dterm_notch_hz == 0) {
if (pidProfile->dterm_notch_hz == 0 || pidProfile->dterm_notch_hz > pidFrequencyNyquist) {
dtermNotchFilterApplyFn = nullFilterApply;
} else {
dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
@ -109,7 +111,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
}
if (pidProfile->dterm_lpf_hz == 0) {
if (pidProfile->dterm_lpf_hz == 0 || pidProfile->dterm_lpf_hz > pidFrequencyNyquist) {
dtermLpfApplyFn = nullFilterApply;
} else {
switch (pidProfile->dterm_filter_type) {
@ -140,7 +142,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
}
if (pidProfile->yaw_lpf_hz == 0) {
if (pidProfile->yaw_lpf_hz == 0 || pidProfile->yaw_lpf_hz > pidFrequencyNyquist) {
ptermYawFilterApplyFn = nullFilterApply;
} else {
ptermYawFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;

8
src/main/sensors/gyro.c
View File

@ -280,7 +280,9 @@ void gyroInitFilters(void)
notchFilter1ApplyFn = nullFilterApply;
notchFilter2ApplyFn = nullFilterApply;
if (gyroConfig->gyro_soft_lpf_hz) { // Initialisation needs to happen once samplingrate is known
uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed
if (gyroConfig->gyro_soft_lpf_hz && gyroConfig->gyro_soft_lpf_hz <= gyroFrequencyNyquist) { // Initialisation needs to happen once samplingrate is known
if (gyroConfig->gyro_soft_lpf_type == FILTER_BIQUAD) {
softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = 0; axis < 3; axis++) {
@ -303,7 +305,7 @@ void gyroInitFilters(void)
}
}
if (gyroConfig->gyro_soft_notch_hz_1) {
if (gyroConfig->gyro_soft_notch_hz_1 && gyroConfig->gyro_soft_notch_hz_1 <= gyroFrequencyNyquist) {
notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_1, gyroConfig->gyro_soft_notch_cutoff_1);
for (int axis = 0; axis < 3; axis++) {
@ -311,7 +313,7 @@ void gyroInitFilters(void)
biquadFilterInit(notchFilter1[axis], gyroConfig->gyro_soft_notch_hz_1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH);
}
}
if (gyroConfig->gyro_soft_notch_hz_2) {
if (gyroConfig->gyro_soft_notch_hz_2 && gyroConfig->gyro_soft_notch_hz_2 <= gyroFrequencyNyquist) {
notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_2, gyroConfig->gyro_soft_notch_cutoff_2);
for (int axis = 0; axis < 3; axis++) {