configurable Acc notch filter

src/main/fc/cli.c

@@ -536,6 +536,11 @@ static const clivalue_t valueTable[] = {
     { "current_adc_channel",        VAR_UINT8 | MASTER_VALUE, .config.minmax = {ADC_CHN_NONE, ADC_CHN_MAX}, PG_ADC_CHANNEL_CONFIG, offsetof(adcChannelConfig_t, adcFunctionChannel[ADC_CURRENT]) },
     { "airspeed_adc_channel",       VAR_UINT8 | MASTER_VALUE, .config.minmax = {ADC_CHN_NONE, ADC_CHN_MAX}, PG_ADC_CHANNEL_CONFIG, offsetof(adcChannelConfig_t, adcFunctionChannel[ADC_AIRSPEED]) },
 
+#ifdef USE_ACC_NOTCH
+    { "acc_notch_hz",               VAR_UINT8  | MASTER_VALUE, .config.minmax = {0, 255 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t,acc_notch_hz)  },
+    { "acc_notch_cutoff",           VAR_UINT8  | MASTER_VALUE, .config.minmax = {1, 255 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_notch_cutoff)  },
+#endif
+
 // PG_ACCELEROMETER_CONFIG
     { "align_acc",                  VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) },
     { "acc_hardware",               VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ACC_HARDWARE }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_hardware) },

src/main/fc/config.c

@@ -214,6 +214,13 @@ void validateAndFixConfig(void)
         pidProfileMutable()->dterm_soft_notch_hz = 0;
     }
 #endif
+
+#ifdef USE_ACC_NOTCH
+    if (accelerometerConfig()->acc_notch_cutoff >= accelerometerConfig()->acc_notch_hz) {
+        accelerometerConfigMutable()->acc_notch_hz = 0;
+    }
+#endif
+
     // Disable unused features
     featureClear(FEATURE_UNUSED_1 | FEATURE_UNUSED_2);
 

src/main/sensors/acceleration.c

@@ -70,7 +70,9 @@ static uint16_t calibratingA = 0;      // the calibration is done is the main lo
 
 static biquadFilter_t accFilter[XYZ_AXIS_COUNT];
 
+#ifdef USE_ACC_NOTCH
 static void *accNotchFilter[XYZ_AXIS_COUNT];
+#endif
 
 PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0);
 
@@ -79,7 +81,9 @@ void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
     RESET_CONFIG_2(accelerometerConfig_t, instance,
         .acc_align = ALIGN_DEFAULT,
         .acc_hardware = ACC_AUTODETECT,
-        .acc_lpf_hz = 15
+        .acc_lpf_hz = 15,
+        .acc_notch_hz = 0,
+        .acc_notch_cutoff = 0
     );
     RESET_CONFIG_2(flightDynamicsTrims_t, &instance->accZero,
         .raw[X] = 0,
@@ -444,7 +448,6 @@ static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const f
 
 void accUpdate(void)
 {
-    float temp;
     if (!acc.dev.read(&acc.dev)) {
         return;
     }
@@ -455,11 +458,18 @@ void accUpdate(void)
 
     if (accelerometerConfig()->acc_lpf_hz) {
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            temp = biquadFilterApply(&accFilter[axis], (float)acc.accADC[axis]);
+            acc.accADC[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)acc.accADC[axis]));
-            acc.accADC[axis] = lrintf(biquadFilterApply(accNotchFilter[axis], temp));
         }
     }
 
+#ifdef USE_ACC_NOTCH
+    if (accelerometerConfig()->acc_notch_hz) {
+        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            acc.accADC[axis] = lrintf(biquadFilterApply(accNotchFilter[axis], (float)acc.accADC[axis]));
+        }
+    }
+#endif
+
     if (!accIsCalibrationComplete()) {
         performAcclerationCalibration();
     }
@@ -482,16 +492,23 @@ void accSetCalibrationValues(void)
 
 void accInitFilters(void)
 {
-    static biquadFilter_t accFilterNotch[XYZ_AXIS_COUNT];
-
     if (acc.accTargetLooptime && accelerometerConfig()->acc_lpf_hz) {
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
             biquadFilterInitLPF(&accFilter[axis], accelerometerConfig()->acc_lpf_hz, acc.accTargetLooptime);
-
-            accNotchFilter[axis] = &accFilterNotch[axis];
-            biquadFilterInitNotch(accNotchFilter[axis], acc.accTargetLooptime, 75, 30);
         }    
     }
+
+#ifdef USE_ACC_NOTCH
+    static biquadFilter_t accFilterNotch[XYZ_AXIS_COUNT];
+
+    if (acc.accTargetLooptime && accelerometerConfig()->acc_notch_hz) {
+        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            accNotchFilter[axis] = &accFilterNotch[axis];
+            biquadFilterInitNotch(accNotchFilter[axis], acc.accTargetLooptime, accelerometerConfig()->acc_notch_hz, accelerometerConfig()->acc_notch_cutoff);
+        }
+    }
+#endif
+
 }
 
 bool accIsHealthy(void)

src/main/sensors/acceleration.h

@@ -52,6 +52,8 @@ typedef struct accelerometerConfig_s {
     uint16_t acc_lpf_hz;                    // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz
     flightDynamicsTrims_t accZero;          // Accelerometer offset
     flightDynamicsTrims_t accGain;          // Accelerometer gain to read exactly 1G
+    uint8_t acc_notch_hz;                   // Accelerometer notch filter frequency
+    uint8_t acc_notch_cutoff;               // Accelerometer notch filter cutoff frequency
 } accelerometerConfig_t;
 
 PG_DECLARE(accelerometerConfig_t, accelerometerConfig);

src/main/target/common.h

@@ -59,6 +59,7 @@
 #define USE_GYRO_NOTCH_1
 #define USE_GYRO_NOTCH_2
 #define USE_DTERM_NOTCH
+#define USE_ACC_NOTCH
 #define CMS
 #define USE_DASHBOARD
 #define USE_MSP_DISPLAYPORT