diff --git a/src/main/common/auto_notch.c b/src/main/common/auto_notch.c index 1fcd169259..7fea4ef205 100644 --- a/src/main/common/auto_notch.c +++ b/src/main/common/auto_notch.c @@ -43,18 +43,17 @@ void initAutoNotch(autoNotch_t *autoNotch, float initial_frequency, int q, int n autoNotch->preVariance = 0.0; pt1FilterInit(&autoNotch->preVarianceFilter, pt1FilterGain(10.0, looptimeUs * 1e-6f)); - biquadFilterInit(&autoNotch->preVarianceBandpass, initial_frequency, looptimeUs, q, FILTER_BPF, 1.0f); biquadFilterInit(&autoNotch->notchFilter, initial_frequency, looptimeUs, q, FILTER_NOTCH, 1.0f); } FAST_CODE float applyAutoNotch(autoNotch_t *autoNotch, float input) { - float preNotchNoise = biquadFilterApplyDF1(&autoNotch->preVarianceBandpass, input); + float notchFilteredNoise = biquadFilterApplyDF1(&autoNotch->notchFilter, input); + + float preNotchNoise = input - notchFilteredNoise; // variance is approximately the noise squared and averaged autoNotch->preVariance = pt1FilterApply(&autoNotch->preVarianceFilter, preNotchNoise * preNotchNoise); - float notchFilteredNoise = biquadFilterApplyDF1(&autoNotch->notchFilter, input); - return autoNotch->weight * notchFilteredNoise + autoNotch->invWeight * input; } @@ -63,7 +62,7 @@ FAST_CODE void updateWeight(autoNotch_t *autoNotch, float frequency, float weigh arm_sqrt_f32(autoNotch->preVariance, &deviation); // 1 / 360 // higher freq have less delay when filtering anyhow and make more dterm noise - float frequencyAccounter = 1.0f + frequency * 0.002777777777f; + float frequencyAccounter = 1.0f + frequency * 0.002777777777f; // the 0.0027 is 1/360 float weight = deviation * frequencyAccounter * autoNotch->noiseLimitInv; autoNotch->weight = MIN(weight * weightMultiplier, 1.0); @@ -71,7 +70,6 @@ FAST_CODE void updateWeight(autoNotch_t *autoNotch, float frequency, float weigh } FAST_CODE void updateAutoNotch(autoNotch_t *autoNotch, float frequency, float q, float weightMultiplier, float looptimeUs) { - biquadFilterInit(&autoNotch->preVarianceBandpass, frequency, looptimeUs, q, FILTER_BPF, 1.0f); biquadFilterUpdate(&autoNotch->notchFilter, frequency, looptimeUs, q, FILTER_NOTCH, 1.0f); updateWeight(autoNotch, frequency, weightMultiplier); diff --git a/src/main/common/auto_notch.h b/src/main/common/auto_notch.h index cb2df3018f..936698554d 100644 --- a/src/main/common/auto_notch.h +++ b/src/main/common/auto_notch.h @@ -25,7 +25,6 @@ typedef struct autoNotch_s { float preVariance; pt1Filter_t preVarianceFilter; // used as an exponential average, setup k to act like exponential average - biquadFilter_t preVarianceBandpass; float noiseLimitInv; // default of 50 allows 70 amplitude noise to be totally notched float weight; diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c index 309f0fb614..32f1f9db3c 100644 --- a/src/main/flight/rpm_filter.c +++ b/src/main/flight/rpm_filter.c @@ -210,11 +210,6 @@ FAST_CODE_NOINLINE void rpmFilterUpdate(void) clone->notchFilter.a1 = template->notchFilter.a1; clone->notchFilter.a2 = template->notchFilter.a2; - clone->preVarianceBandpass.b0 = template->preVarianceBandpass.b0; - //clone->preVarianceBandpass.b1 = template->preVarianceBandpass.b1; // always 0 - clone->preVarianceBandpass.b2 = template->preVarianceBandpass.b2; - clone->preVarianceBandpass.b0 = template->preVarianceBandpass.a1; - clone->preVarianceBandpass.b2 = template->preVarianceBandpass.a2; updateWeight(clone, frequency, weight); }