Auto Notch

make/source.mk

@@ -229,6 +229,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
             common/maths.c \
             common/sdft.c \
             common/typeconversion.c \
+            common/auto_notch.c \
             drivers/accgyro/accgyro_mpu.c \
             drivers/accgyro/accgyro_mpu3050.c \
             drivers/accgyro/accgyro_spi_bmi160.c \

src/main/cli/settings.c

@@ -693,6 +693,7 @@ const clivalue_t valueTable[] = {
 #if defined(USE_DYN_NOTCH_FILTER)
     { PARAM_NAME_DYN_NOTCH_COUNT,   VAR_UINT8   | MASTER_VALUE, .config.minmaxUnsigned = { 0, DYN_NOTCH_COUNT_MAX }, PG_DYN_NOTCH_CONFIG, offsetof(dynNotchConfig_t, dyn_notch_count) },
     { PARAM_NAME_DYN_NOTCH_Q,       VAR_UINT16  | MASTER_VALUE, .config.minmaxUnsigned = { 1, 1000 }, PG_DYN_NOTCH_CONFIG, offsetof(dynNotchConfig_t, dyn_notch_q) },
+    { "dyn_notch_noise_limit",      VAR_UINT8   | MASTER_VALUE, .config.minmaxUnsigned = { 1, 255 }, PG_DYN_NOTCH_CONFIG, offsetof(dynNotchConfig_t, noise_limit) },
     { PARAM_NAME_DYN_NOTCH_MIN_HZ,  VAR_UINT16  | MASTER_VALUE, .config.minmaxUnsigned = { 20, 250 }, PG_DYN_NOTCH_CONFIG, offsetof(dynNotchConfig_t, dyn_notch_min_hz) },
     { PARAM_NAME_DYN_NOTCH_MAX_HZ,  VAR_UINT16  | MASTER_VALUE, .config.minmaxUnsigned = { 200, 1000 }, PG_DYN_NOTCH_CONFIG, offsetof(dynNotchConfig_t, dyn_notch_max_hz) },
 #endif
@@ -1700,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

@@ -0,0 +1,76 @@
+/*
+ * This file is part of Cleanflight and Betaflight and EmuFlight.
+ *
+ * Cleanflight and Betaflight and EmuFlight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight and EmuFlight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#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
+ * a period of time great enough to cover at least 1 full wave of noise at notch
+ * frequency. This way we can measure noise and compare noise of pre/post notch.
+ * this allows us to crossfade based on noise.
+ */
+
+
+void initAutoNotch(autoNotch_t *autoNotch, float initial_frequency, int q, int noiseLimit, float looptimeUs) {
+    autoNotch->noiseLimitInv = 1.0 / noiseLimit;
+    autoNotch->weight = 1.0;
+    autoNotch->invWeight = 0.0;
+
+    autoNotch->preVariance = 0.0;
+    pt1FilterInit(&autoNotch->preVarianceFilter, pt1FilterGain(10.0, looptimeUs * 1e-6f));
+
+    biquadFilterInit(&autoNotch->notchFilter, initial_frequency, looptimeUs, q, FILTER_NOTCH, 1.0f);
+}
+
+FAST_CODE float applyAutoNotch(autoNotch_t *autoNotch, float 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);
+
+    return autoNotch->weight * notchFilteredNoise + autoNotch->invWeight * input;
+}
+
+FAST_CODE void updateWeight(autoNotch_t *autoNotch, float frequency, float weightMultiplier) {
+    float deviation;
+    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; // the 0.0027 is 1/360
+    float weight = deviation * frequencyAccounter * autoNotch->noiseLimitInv;
+
+    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) {
+    biquadFilterUpdate(&autoNotch->notchFilter, frequency, looptimeUs, q, FILTER_NOTCH, 1.0f);
+
+    updateWeight(autoNotch, frequency, weightMultiplier);
+}

src/main/common/auto_notch.h

@@ -0,0 +1,39 @@
+/*
+ * This file is part of Cleanflight and Betaflight and EmuFlight.
+ *
+ * Cleanflight and Betaflight and EmuFlight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight and EmuFlight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "filter.h"
+
+typedef struct autoNotch_s {
+    float preVariance;
+    pt1Filter_t preVarianceFilter; // used as an exponential average, setup k to act like exponential average
+
+    float noiseLimitInv; // default of 50 allows 70 amplitude noise to be totally notched
+    float weight;
+    float invWeight;
+
+    biquadFilter_t notchFilter; // the notch filter we apply to the data
+} autoNotch_t;
+
+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 frequency, float weightMultiplier);
+void updateAutoNotch(autoNotch_t *autoNotch, float frequency, float q, float weightMultiplier, float looptimeUs);

src/main/common/filter.c

@@ -189,36 +189,37 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint
     case FILTER_LPF:
         // 2nd order Butterworth (with Q=1/sqrt(2)) / Butterworth biquad section with Q
         // described in http://www.ti.com/lit/an/slaa447/slaa447.pdf
-        filter->b1 = 1 - cs;
+        filter->b1 = 1.0 - cs;
         filter->b0 = filter->b1 * 0.5f;
         filter->b2 = filter->b0;
-        filter->a1 = -2 * cs;
-        filter->a2 = 1 - alpha;
+        filter->a1 = -2.0 * cs;
+        filter->a2 = 1.0 - alpha;
         break;
     case FILTER_NOTCH:
-        filter->b0 = 1;
-        filter->b1 = -2 * cs;
-        filter->b2 = 1;
+        filter->b0 = 1.0;
+        filter->b1 = -2.0 * cs;
+        filter->b2 = 1.0;
         filter->a1 = filter->b1;
-        filter->a2 = 1 - alpha;
+        filter->a2 = 1.0 - alpha;
         break;
     case FILTER_BPF:
         filter->b0 = alpha;
-        filter->b1 = 0;
+        filter->b1 = 0.0;
         filter->b2 = -alpha;
-        filter->a1 = -2 * cs;
-        filter->a2 = 1 - alpha;
+        filter->a1 = -2.0 * cs;
+        filter->a2 = 1.0 - alpha;
         break;
     }
 
-    const float a0 = 1 + alpha;
+    const float a0 = 1.0 / (1.0 + alpha);
 
     // precompute the coefficients
-    filter->b0 /= a0;
-    filter->b1 /= a0;
-    filter->b2 /= a0;
-    filter->a1 /= a0;
-    filter->a2 /= a0;
+    // consider moving inside switch statement as you could save cpu
+    filter->b0 *= a0;
+    filter->b1 *= a0;
+    filter->b2 *= a0;
+    filter->a1 *= a0;
+    filter->a2 *= a0;
 
     // update weight
     filter->weight = weight;

src/main/fc/controlrate_profile.c

@@ -37,7 +37,7 @@
 
 controlRateConfig_t *currentControlRateProfile;
 
-PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 4);
+PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 0);
 
 void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig)
 {

src/main/flight/dyn_notch_filter.c

@@ -19,11 +19,11 @@
  */
 
 /* original work by Rav
- * 
+ *
  * 2018_07 updated by ctzsnooze to post filter, wider Q, different peak detection
  * coding assistance and advice from DieHertz, Rav, eTracer
  * test pilots icr4sh, UAV Tech, Flint723
- * 
+ *
  * 2021_02 updated by KarateBrot: switched FFT with SDFT, multiple notches per axis
  * test pilots: Sugar K, bizmar
  */
@@ -41,6 +41,7 @@
 #include "common/maths.h"
 #include "common/sdft.h"
 #include "common/utils.h"
+#include "common/auto_notch.h"
 
 #include "config/feature.h"
 
@@ -123,9 +124,9 @@ typedef struct dynNotch_s {
 
     int maxCenterFreq;
     float centerFreq[XYZ_AXIS_COUNT][DYN_NOTCH_COUNT_MAX];
-    
+
     timeUs_t looptimeUs;
-    biquadFilter_t notch[XYZ_AXIS_COUNT][DYN_NOTCH_COUNT_MAX];
+    autoNotch_t notch[XYZ_AXIS_COUNT][DYN_NOTCH_COUNT_MAX];
 
 } dynNotch_t;
 
@@ -196,7 +197,7 @@ void dynNotchInit(const dynNotchConfig_t *config, const timeUs_t targetLooptimeU
         for (int p = 0; p < dynNotch.count; p++) {
             // any init value is fine, but evenly spreading centerFreqs across frequency range makes notch filters stick to peaks quicker
             dynNotch.centerFreq[axis][p] = (p + 0.5f) * (dynNotch.maxHz - dynNotch.minHz) / (float)dynNotch.count + dynNotch.minHz;
-            biquadFilterInit(&dynNotch.notch[axis][p], dynNotch.centerFreq[axis][p], dynNotch.looptimeUs, dynNotch.q, FILTER_NOTCH, 1.0f);
+            initAutoNotch(&dynNotch.notch[axis][p], dynNotch.centerFreq[axis][p], dynNotch.q, config->noise_limit, dynNotch.looptimeUs);
         }
     }
 }
@@ -258,11 +259,11 @@ static FAST_CODE_NOINLINE void dynNotchProcess(void)
     DEBUG_SET(DEBUG_FFT_TIME, 0, state.step);
 
     switch (state.step) {
-    
+
         case STEP_WINDOW: // 4.1us (3-6us) @ F722
         {
             sdftWinSq(&sdft[state.axis], sdftData);
-            
+
             // Get total vibrational power in dyn notch range for noise floor estimate in STEP_CALC_FREQUENCIES
             sdftNoiseThreshold = 0.0f;
             for (int bin = (sdftStartBin + 1); bin < sdftEndBin; bin++) {   // don't use startBin or endBin because they are not windowed properly
@@ -356,7 +357,7 @@ static FAST_CODE_NOINLINE void dynNotchProcess(void)
                     // Convert bin to frequency: freq = bin * binResoultion (bin 0 is 0Hz)
                     const float centerFreq = constrainf(meanBin * sdftResolutionHz, dynNotch.minHz, dynNotch.maxHz);
 
-                    // PT1 style smoothing moves notch center freqs rapidly towards big peaks and slowly away, up to 10x faster 
+                    // PT1 style smoothing moves notch center freqs rapidly towards big peaks and slowly away, up to 10x faster
                     const float cutoffMult = constrainf(peaks[p].value / sdftNoiseThreshold, 1.0f, 10.0f);
                     const float gain = pt1FilterGain(DYN_NOTCH_SMOOTH_HZ * cutoffMult, pt1LooptimeS); // dynamic PT1 k value
 
@@ -387,7 +388,8 @@ static FAST_CODE_NOINLINE void dynNotchProcess(void)
             for (int p = 0; p < dynNotch.count; p++) {
                 // Only update notch filter coefficients if the corresponding peak got its center frequency updated in the previous step
                 if (peaks[p].bin != 0 && peaks[p].value > sdftNoiseThreshold) {
-                    biquadFilterUpdate(&dynNotch.notch[state.axis][p], dynNotch.centerFreq[state.axis][p], dynNotch.looptimeUs, dynNotch.q, FILTER_NOTCH, 1.0f);
+                  // todo update autonotch crossfade value
+                    updateAutoNotch(&dynNotch.notch[state.axis][p], dynNotch.centerFreq[state.axis][p], dynNotch.q, 1.0f, dynNotch.looptimeUs);
                 }
             }
 
@@ -400,10 +402,10 @@ static FAST_CODE_NOINLINE void dynNotchProcess(void)
     state.step = (state.step + 1) % STEP_COUNT;
 }
 
-FAST_CODE float dynNotchFilter(const int axis, float value) 
+FAST_CODE float dynNotchFilter(const int axis, float value)
 {
     for (int p = 0; p < dynNotch.count; p++) {
-        value = biquadFilterApplyDF1(&dynNotch.notch[axis][p], value);
+        value = applyAutoNotch(&dynNotch.notch[axis][p], value);
     }
 
     return value;

src/main/flight/pid.c

@@ -121,7 +121,7 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
 
 #define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
 
-PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 3);
+PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 0);
 
 void resetPidProfile(pidProfile_t *pidProfile)
 {
@@ -1125,7 +1125,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
         if (feedforwardGain > 0) {
             // halve feedforward in Level mode since stick sensitivity is weaker by about half
             feedforwardGain *= FLIGHT_MODE(ANGLE_MODE) ? 0.5f : 1.0f;
-            // transition now calculated in feedforward.c when new RC data arrives 
+            // transition now calculated in feedforward.c when new RC data arrives
             float feedForward = feedforwardGain * pidSetpointDelta * pidRuntime.pidFrequency;
 
 #ifdef USE_FEEDFORWARD