autonotch on rpm filter

Author: Quick-Flash

src/main/common/auto_notch.c

@@ -21,9 +21,12 @@
 #include <string.h>
 #include "arm_math.h"
 
+#include "platform.h"
+
 #include "auto_notch.h"
 #include "build/debug.h"
 #include "maths.h"
+
 /*
  * The idea is simple, run a passband at the notch frequency to isolate noise
  * at the notch frequency. Then look at the averaged squared rate of change over
@@ -34,21 +37,18 @@
 
 
 void initAutoNotch(autoNotch_t *autoNotch, float initial_frequency, int q, int noiseLimit, float looptimeUs) {
-    // creates an exponential moving average that will cover the freq we are nothing/bandpassing
-    float adjustedQ = q / 100.0f;
-
     autoNotch->noiseLimit = noiseLimit;
     autoNotch->weight = 1.0;
     autoNotch->invWeight = 0.0;
 
     autoNotch->preVariance = 0.0;
     pt1FilterInit(&autoNotch->preVarianceFilter, pt1FilterGain(10.0, looptimeUs * 1e-6f));
-    biquadFilterInit(&autoNotch->preVarianceBandpass, initial_frequency, looptimeUs, adjustedQ, FILTER_BPF, 1.0f);
+    biquadFilterInit(&autoNotch->preVarianceBandpass, initial_frequency, looptimeUs, q, FILTER_BPF, 1.0f);
 
-    biquadFilterInit(&autoNotch->notchFilter, initial_frequency, looptimeUs, adjustedQ, FILTER_NOTCH, 1.0f);
+    biquadFilterInit(&autoNotch->notchFilter, initial_frequency, looptimeUs, q, FILTER_NOTCH, 1.0f);
 }
 
-float applyAutoNotch(autoNotch_t *autoNotch, float input) {
+FAST_CODE float applyAutoNotch(autoNotch_t *autoNotch, float input) {
     float preNotchNoise = biquadFilterApplyDF1(&autoNotch->preVarianceBandpass, input);
     // variance is approximately the noise squared and averaged
     autoNotch->preVariance = pt1FilterApply(&autoNotch->preVarianceFilter, preNotchNoise * preNotchNoise);
@@ -58,14 +58,18 @@ float applyAutoNotch(autoNotch_t *autoNotch, float input) {
     return autoNotch->weight * notchFilteredNoise + autoNotch->invWeight * input;
 }
 
-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);
-
+FAST_CODE void updateWeight(autoNotch_t *autoNotch, float weightMultiplier) {
     float deviation;
     arm_sqrt_f32(autoNotch->preVariance, &deviation);
     float weight = deviation / autoNotch->noiseLimit;
 
     autoNotch->weight = MIN(weight * weightMultiplier, 1.0);
     autoNotch->invWeight = 1.0 - autoNotch->weight;
 }
+
+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, weightMultiplier);
+}

src/main/common/auto_notch.h

@@ -36,4 +36,5 @@ typedef struct autoNotch_s {
 
 void initAutoNotch(autoNotch_t *autoNotch, float initial_frequency, int q, int noiseLimit, float looptimeUs);
 float applyAutoNotch(autoNotch_t *autoNotch, float input);
+void updateWeight(autoNotch_t *autoNotch, float weightMultiplier);
 void updateAutoNotch(autoNotch_t *autoNotch, float frequency, float q, float weightMultiplier, float looptimeUs);

src/main/common/filter.c

@@ -214,6 +214,7 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint
     const float a0 = 1.0 / (1.0 + alpha);
 
     // precompute the coefficients
+    // consider moving inside switch statement as you could save cpu
     filter->b0 *= a0;
     filter->b1 *= a0;
     filter->b2 *= a0;

src/main/flight/rpm_filter.c

@@ -31,6 +31,7 @@
 #include "common/filter.h"
 #include "common/maths.h"
 #include "common/time.h"
+#include "common/auto_notch.h"
 
 #include "drivers/dshot.h"
 
@@ -62,7 +63,7 @@ typedef struct rpmNotchFilter_s {
     float    q;
     timeUs_t looptimeUs;
 
-    biquadFilter_t notch[XYZ_AXIS_COUNT][MAX_SUPPORTED_MOTORS][RPM_FILTER_MAXHARMONICS];
+    autoNotch_t notch[XYZ_AXIS_COUNT][MAX_SUPPORTED_MOTORS][RPM_FILTER_MAXHARMONICS];
 
 } rpmNotchFilter_t;
 
@@ -94,6 +95,8 @@ void pgResetFn_rpmFilterConfig(rpmFilterConfig_t *config)
     config->rpm_filter_q = 500;
 
     config->rpm_filter_lpf_hz = 150;
+
+    config->noise_limit = 50;
 }
 
 static void rpmNotchFilterInit(rpmNotchFilter_t *filter, const rpmFilterConfig_t *config, const timeUs_t looptimeUs)
@@ -108,8 +111,8 @@ static void rpmNotchFilterInit(rpmNotchFilter_t *filter, const rpmFilterConfig_t
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         for (int motor = 0; motor < getMotorCount(); motor++) {
             for (int i = 0; i < filter->harmonics; i++) {
-                biquadFilterInit(
-                    &filter->notch[axis][motor][i], filter->minHz * i, filter->looptimeUs, filter->q, FILTER_NOTCH, 0.0f);
+                initAutoNotch(
+                    &filter->notch[axis][motor][i], filter->minHz * i, filter->q, config->noise_limit, filter->looptimeUs);
             }
         }
     }
@@ -153,7 +156,7 @@ static float applyFilter(rpmNotchFilter_t *filter, const int axis, float value)
     }
     for (int motor = 0; motor < getMotorCount(); motor++) {
         for (int i = 0; i < filter->harmonics; i++) {
-            value = biquadFilterApplyDF1Weighted(&filter->notch[axis][motor][i], value);
+            value = applyAutoNotch(&filter->notch[axis][motor][i], value);
         }
     }
     return value;
@@ -183,31 +186,37 @@ FAST_CODE_NOINLINE void rpmFilterUpdate(void)
 
         float frequency = constrainf(
             (currentHarmonic + 1) * motorFrequency[currentMotor], currentFilter->minHz, currentFilter->maxHz);
-        biquadFilter_t *template = &currentFilter->notch[0][currentMotor][currentHarmonic];
+        autoNotch_t *template = &currentFilter->notch[0][currentMotor][currentHarmonic];
         // uncomment below to debug filter stepping. Need to also comment out motor rpm DEBUG_SET above
         /* DEBUG_SET(DEBUG_RPM_FILTER, 0, harmonic); */
         /* DEBUG_SET(DEBUG_RPM_FILTER, 1, motor); */
         /* DEBUG_SET(DEBUG_RPM_FILTER, 2, currentFilter == &gyroFilter); */
         /* DEBUG_SET(DEBUG_RPM_FILTER, 3, frequency) */
-        
+
         // fade out notch when approaching minHz (turn it off)
         float weight = 1.0f;
         if (frequency < currentFilter->minHz + currentFilter->fadeRangeHz) {
             weight = (frequency - currentFilter->minHz) / currentFilter->fadeRangeHz;
         }
 
-        biquadFilterUpdate(
-            template, frequency, currentFilter->looptimeUs, currentFilter->q, FILTER_NOTCH, weight);
+        updateAutoNotch(
+            template, frequency, currentFilter->q, weight, currentFilter->looptimeUs);
 
         for (int axis = 1; axis < XYZ_AXIS_COUNT; axis++) {
-            biquadFilter_t *clone = &currentFilter->notch[axis][currentMotor][currentHarmonic];
-            clone->b0 = template->b0;
-            clone->b1 = template->b1;
-            clone->b2 = template->b2;
-            clone->a1 = template->a1;
-            clone->a2 = template->a2;
-            clone->weight = template->weight;
-        }
+            autoNotch_t *clone = &currentFilter->notch[axis][currentMotor][currentHarmonic];
+            clone->notchFilter.b0 = template->notchFilter.b0;
+            clone->notchFilter.b1 = template->notchFilter.b1;
+            clone->notchFilter.b2 = template->notchFilter.b2;
+            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, weight);
+            }
 
         if (++currentHarmonic == currentFilter->harmonics) {
             currentHarmonic = 0;

src/main/flight/rpm_filter.h

@@ -32,6 +32,8 @@ typedef struct rpmFilterConfig_s
 
     uint16_t rpm_filter_lpf_hz;        // the cutoff of the lpf on reported motor rpm
 
+    uint8_t noise_limit;
+
 } rpmFilterConfig_t;
 
 PG_DECLARE(rpmFilterConfig_t, rpmFilterConfig);