Added notch filter from betaflight (#592)

src/main/common/filter.c

@@ -20,59 +20,18 @@
 #include <string.h>
 #include <math.h>
 
-#include "common/axis.h"
 #include "common/filter.h"
 #include "common/maths.h"
 
-#include "drivers/gyro_sync.h"
-
-#define BIQUAD_Q    (1.0f / 1.41421356f)     /* quality factor - butterworth (1 / sqrt(2)) */
-#define M_PI_FLOAT  3.14159265358979323846f
-
-void biquadFilterInit(biquadFilter_t *filter, uint32_t filterCutFreq, uint32_t refreshRate)
-{
-    // setup variables
-    const float sampleRate = 1.0f / ((float)refreshRate * 0.000001f);
-    const float omega = 2 * M_PIf * ((float)filterCutFreq) / sampleRate;
-    const float sn = sin_approx(omega);
-    const float cs = cos_approx(omega);
-    const float alpha = sn / (2 * BIQUAD_Q);
-
-    float a0, a1, a2, b0, b1, b2;
-
-    b0 = (1 - cs) / 2;
-    b1 = 1 - cs;
-    b2 = (1 - cs) / 2;
-    a0 = 1 + alpha;
-    a1 = -2 * cs;
-    a2 = 1 - alpha;
-
-    // precompute the coefficients
-    filter->b0 = b0 / a0;
-    filter->b1 = b1 / a0;
-    filter->b2 = b2 / a0;
-    filter->a1 = a1 / a0;
-    filter->a2 = a2 / a0;
-
-    // zero initial samples
-    filter->d1 = filter->d2 = 1;
-}
-
-/* Computes a biquad_t filter on a sample */
-float biquadFilterApply(biquadFilter_t *filter, float input)
-{
-    const float result = filter->b0 * input + filter->d1;
-    filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2;
-    filter->d2 = filter->b2 * input - filter->a2 * result;
-    return result;
-}
+#define BIQUAD_BANDWIDTH 1.9f     /* bandwidth in octaves */
+#define BIQUAD_Q 1.0f / sqrtf(2.0f)     /* quality factor - butterworth*/
 
 // PT1 Low Pass filter
 
 // f_cut = cutoff frequency
 void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT)
 {
-    filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut );
+    filter->RC = 1.0f / ( 2.0f * M_PIf * f_cut );
     filter->dT = dT;
 }
 
@@ -82,14 +41,15 @@ float pt1FilterApply(pt1Filter_t *filter, float input)
     return filter->state;
 }
 
-float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dT)
+float pt1FilterApply4(pt1Filter_t *filter, float input, uint16_t f_cut, float dT)
 {
     // Pre calculate and store RC
     if (!filter->RC) {
-        filter->RC = 1.0f / ( 2.0f * (float)M_PI * f_cut );
+        filter->RC = 1.0f / ( 2.0f * M_PIf * f_cut );
+        filter->dT = dT;
     }
 
-    filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
+    filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state);
     return filter->state;
 }
 
@@ -117,7 +77,67 @@ float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_l
     return filter->state;
 }
 
-// FIR filter
+float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff)
+{
+    const float octaves = log2f((float)centerFreq  / (float)cutoff) * 2;
+    return sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1);
+}
+
+// sets up a biquad Filter
+void biquadFilterInitLPF(biquadFilter_t *filter, uint16_t filterFreq, uint32_t refreshRate)
+{
+    biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF);
+}
+
+void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
+{
+    // setup variables
+    const float sampleRate = 1.0f / ((float)refreshRate * 0.000001f);
+    const float omega = 2.0f * M_PIf * ((float)filterFreq) / sampleRate;
+    const float sn = sin_approx(omega);
+    const float cs = cos_approx(omega);
+    const float alpha = sn / (2 * Q);
+
+    float b0, b1, b2;
+    switch (filterType) {
+    case FILTER_LPF:
+        b0 = (1 - cs) / 2;
+        b1 = 1 - cs;
+        b2 = (1 - cs) / 2;
+        break;
+    case FILTER_NOTCH:
+        b0 =  1;
+        b1 = -2 * cs;
+        b2 =  1;
+        break;
+    }
+    const float a0 =  1 + alpha;
+    const float a1 = -2 * cs;
+    const float a2 =  1 - alpha;
+
+    // precompute the coefficients
+    filter->b0 = b0 / a0;
+    filter->b1 = b1 / a0;
+    filter->b2 = b2 / a0;
+    filter->a1 = a1 / a0;
+    filter->a2 = a2 / a0;
+
+    // zero initial samples
+    filter->d1 = filter->d2 = 0;
+}
+
+// Computes a biquad_t filter on a sample
+float biquadFilterApply(biquadFilter_t *filter, float input)
+{
+    const float result = filter->b0 * input + filter->d1;
+    filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2;
+    filter->d2 = filter->b2 * input - filter->a2 * result;
+    return result;
+}
+
+/*
+ * FIR filter
+ */
 void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength)
 {
     filter->buf = buf;
@@ -127,6 +147,10 @@ void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const fl
     memset(filter->buf, 0, sizeof(float) * filter->bufLength);
 }
 
+/*
+ * FIR filter initialisation
+ * If FIR filter is just used for averaging, coeffs can be set to NULL
+ */
 void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs)
 {
     firFilterInit2(filter, buf, bufLength, coeffs, bufLength);
@@ -138,7 +162,7 @@ void firFilterUpdate(firFilter_t *filter, float input)
     filter->buf[0] = input;
 }
 
-float firFilterApply(firFilter_t *filter)
+float firFilterApply(const firFilter_t *filter)
 {
     float ret = 0.0f;
     for (int ii = 0; ii < filter->coeffsLength; ++ii) {

src/main/common/filter.h

@@ -33,6 +33,17 @@ typedef struct biquadFilter_s {
     float d1, d2;
 } biquadFilter_t;
 
+typedef enum {
+    FILTER_PT1 = 0,
+    FILTER_BIQUAD,
+    FILTER_FIR,
+} filterType_e;
+
+typedef enum {
+    FILTER_LPF,
+    FILTER_NOTCH
+} biquadFilterType_e;
+
 typedef struct firFilter_s {
     float *buf;
     const float *coeffs;
@@ -41,17 +52,19 @@ typedef struct firFilter_s {
 } firFilter_t;
 
 float pt1FilterApply(pt1Filter_t *filter, float input);
-float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dt);
+float pt1FilterApply4(pt1Filter_t *filter, float input, uint16_t f_cut, float dt);
 void pt1FilterReset(pt1Filter_t *filter, float input);
 
 void rateLimitFilterInit(rateLimitFilter_t *filter);
 float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_limit, float dT);
 
-void biquadFilterInit(biquadFilter_t *filter, uint32_t filterCutFreq, uint32_t refreshRate);
+void biquadFilterInitLPF(biquadFilter_t *filter, uint16_t filterFreq, uint32_t refreshRate);
+void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
 float biquadFilterApply(biquadFilter_t *filter, float sample);
+float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff);
 
 void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs);
 void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength);
 void firFilterUpdate(firFilter_t *filter, float input);
-float firFilterApply(firFilter_t *filter);
+float firFilterApply(const firFilter_t *filter);
 

src/main/fc/mw.c

@@ -499,7 +499,7 @@ void filterRc(bool isRXDataNew)
 
     // Calculate average cycle time (1Hz LPF on cycle time)
     if (!filterInitialised) {
-        biquadFilterInit(&filteredCycleTimeState, 1, gyro.targetLooptime);
+        biquadFilterInitLPF(&filteredCycleTimeState, 1, gyro.targetLooptime);
         filterInitialised = true;
     }
 

src/main/flight/mixer.c

@@ -921,7 +921,7 @@ void filterServos(void)
         // Initialize servo lowpass filter (servos are calculated at looptime rate)
         if (!servoFilterIsSet) {
             for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
-                biquadFilterInit(&servoFitlerState[servoIdx], mixerConfig->servo_lowpass_freq, gyro.targetLooptime);
+                biquadFilterInitLPF(&servoFitlerState[servoIdx], mixerConfig->servo_lowpass_freq, gyro.targetLooptime);
             }
 
             servoFilterIsSet = true;

src/main/sensors/acceleration.c

@@ -58,7 +58,7 @@ void accInit(uint32_t targetLooptime)
     accTargetLooptime = targetLooptime;
     if (accLpfCutHz) {
         for (int axis = 0; axis < 3; axis++) {
-            biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime);
+            biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accTargetLooptime);
         }
     }
 }
@@ -223,7 +223,7 @@ void setAccelerationFilter(uint8_t initialAccLpfCutHz)
     accLpfCutHz = initialAccLpfCutHz;
     if (accTargetLooptime) {
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime);
+            biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accTargetLooptime);
         }
     }
 }

src/main/sensors/gyro.c

@@ -59,7 +59,7 @@ void gyroInit(void)
 {
     if (gyroSoftLpfHz && gyro.targetLooptime) {  // Initialisation needs to happen once samplingrate is known
         for (int axis = 0; axis < 3; axis++) {
-            biquadFilterInit(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
+            biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
         }
     }
 }