add frequency accounter

Author: Quick-Flash

src/main/cli/settings.c

@@ -1701,6 +1701,7 @@ const clivalue_t valueTable[] = {
 #ifdef USE_RPM_FILTER
     { PARAM_NAME_RPM_FILTER_HARMONICS,     VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_harmonics) },
     { PARAM_NAME_RPM_FILTER_Q,             VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 250, 3000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_q) },
+    { "rpm_noise_limit",                   VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 1, 255 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, noise_limit) },
     { PARAM_NAME_RPM_FILTER_MIN_HZ,        VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 30, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_min_hz) },
     { PARAM_NAME_RPM_FILTER_FADE_RANGE_HZ, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_fade_range_hz) },
     { PARAM_NAME_RPM_FILTER_LPF_HZ,        VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 500 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_lpf_hz) },

src/main/common/auto_notch.c

@@ -37,7 +37,7 @@
 
 
 void initAutoNotch(autoNotch_t *autoNotch, float initial_frequency, int q, int noiseLimit, float looptimeUs) {
-    autoNotch->noiseLimit = noiseLimit;
+    autoNotch->noiseLimitInv = 1.0 / noiseLimit;
     autoNotch->weight = 1.0;
     autoNotch->invWeight = 0.0;
 
@@ -58,10 +58,13 @@ FAST_CODE float applyAutoNotch(autoNotch_t *autoNotch, float input) {
     return autoNotch->weight * notchFilteredNoise + autoNotch->invWeight * input;
 }
 
-FAST_CODE void updateWeight(autoNotch_t *autoNotch, float weightMultiplier) {
+FAST_CODE void updateWeight(autoNotch_t *autoNotch, float frequency, float weightMultiplier) {
     float deviation;
     arm_sqrt_f32(autoNotch->preVariance, &deviation);
-    float weight = deviation / autoNotch->noiseLimit;
+    // 1 / 360
+    // higher freq have less delay when filtering anyhow and make more dterm noise
+    float frequencyAccounter = 1.0f + frequency * 0.002777777777f;
+    float weight = deviation * frequencyAccounter * autoNotch->noiseLimitInv;
 
     autoNotch->weight = MIN(weight * weightMultiplier, 1.0);
     autoNotch->invWeight = 1.0 - autoNotch->weight;
@@ -71,5 +74,5 @@ FAST_CODE void updateAutoNotch(autoNotch_t *autoNotch, float frequency, float q,
     biquadFilterInit(&autoNotch->preVarianceBandpass, frequency, looptimeUs, q, FILTER_BPF, 1.0f);
     biquadFilterUpdate(&autoNotch->notchFilter, frequency, looptimeUs, q, FILTER_NOTCH, 1.0f);
 
-    updateWeight(autoNotch, weightMultiplier);
+    updateWeight(autoNotch, frequency, weightMultiplier);
 }

src/main/common/auto_notch.h

@@ -27,7 +27,7 @@ typedef struct autoNotch_s {
     pt1Filter_t preVarianceFilter; // used as an exponential average, setup k to act like exponential average
     biquadFilter_t preVarianceBandpass;
 
-    float noiseLimit; // default of 50 allows 70 amplitude noise to be totally notched
+    float noiseLimitInv; // default of 50 allows 70 amplitude noise to be totally notched
     float weight;
     float invWeight;
 
@@ -36,5 +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 updateWeight(autoNotch_t *autoNotch, float frequency, float weightMultiplier);
 void updateAutoNotch(autoNotch_t *autoNotch, float frequency, float q, float weightMultiplier, float looptimeUs);

src/main/flight/rpm_filter.c

@@ -215,7 +215,7 @@ FAST_CODE_NOINLINE void rpmFilterUpdate(void)
             clone->preVarianceBandpass.b2 = template->preVarianceBandpass.b2;
             clone->preVarianceBandpass.b0 = template->preVarianceBandpass.a1;
             clone->preVarianceBandpass.b2 = template->preVarianceBandpass.a2;
-            updateWeight(clone, weight);
+            updateWeight(clone, frequency, weight);
             }
 
         if (++currentHarmonic == currentFilter->harmonics) {