Merge pull request #5702 from iNavFlight/dzikuvx-fix-biquad-notch

Proper fix for wide static notches not working
iNavFlight · May 9, 2020 · 272de56 · 272de56
2 parents 33319a1 + b3e0ea3
commit 272de56
Show file tree

Hide file tree

Showing 4 changed files with 7 additions and 10 deletions.
diff --git a/src/main/common/filter.c b/src/main/common/filter.c
@@ -118,10 +118,9 @@ float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_l
     return filter->state;
 }
 
-float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff)
+float filterGetNotchQ(float centerFrequencyHz, float cutoffFrequencyHz)
 {
-    const float octaves = log2f((float)centerFreq  / (float)cutoff) * 2;
-    return sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1);
+    return centerFrequencyHz * cutoffFrequencyHz / (centerFrequencyHz * centerFrequencyHz - cutoffFrequencyHz * cutoffFrequencyHz);
 }
 
 void biquadFilterInitNotch(biquadFilter_t *filter, uint32_t samplingIntervalUs, uint16_t filterFreq, uint16_t cutoffHz)

diff --git a/src/main/common/filter.h b/src/main/common/filter.h
@@ -79,7 +79,7 @@ void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samp
 float biquadFilterApply(biquadFilter_t *filter, float sample);
 float biquadFilterReset(biquadFilter_t *filter, float value);
 float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
-float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff);
+float filterGetNotchQ(float centerFrequencyHz, float cutoffFrequencyHz);
 void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
 
 void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs);

diff --git a/src/main/flight/dynamic_gyro_notch.c b/src/main/flight/dynamic_gyro_notch.c
@@ -40,15 +40,14 @@ void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state) {
     state->looptime = getLooptime();
 
     if (state->enabled) {
-        const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here
-
         /*
          * Step 1 - init all filters even if they will not be used further down the road
          */
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            biquadFilterInit(&state->filters[axis][0], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, notchQ, FILTER_NOTCH);
-            biquadFilterInit(&state->filters[axis][1], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, notchQ, FILTER_NOTCH);
-            biquadFilterInit(&state->filters[axis][2], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, notchQ, FILTER_NOTCH);
+            //Any initial notch Q is valid sice it will be updated immediately after
+            biquadFilterInit(&state->filters[axis][0], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
+            biquadFilterInit(&state->filters[axis][1], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
+            biquadFilterInit(&state->filters[axis][2], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
         }
 
         state->filtersApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1;

diff --git a/src/main/flight/dynamic_gyro_notch.h b/src/main/flight/dynamic_gyro_notch.h
@@ -29,7 +29,6 @@
 #include "common/filter.h"
 
 #define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350
-#define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 300
 
 typedef struct dynamicGyroNotchState_s {
     uint16_t frequency[XYZ_AXIS_COUNT];