Implement PT1 filter option for accelerometer

docs/Cli.md

@@ -408,6 +408,7 @@ A shorter form is also supported to enable and disable functions using `serial <
 |  gyro_lpf_hz  | 60 | Software-based filter to remove mechanical vibrations from the gyro signal. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. |
 |  gyro_lpf_type  | BIQUAD | Specifies the type of the software LPF of the gyro signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. |
 |  acc_lpf_hz  | 15 | Software-based filter to remove mechanical vibrations from the accelerometer measurements. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. |
+|  acc_lpf_type  | BIQUAD | Specifies the type of the software LPF of the acc signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. |
 |  dterm_lpf_hz  | 40 |  |
 |  yaw_lpf_hz  | 30 |  |
 |  gyro_stage2_lowpass_hz  | 0 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz). Currently experimental |

src/main/fc/settings.yaml

@@ -221,7 +221,11 @@ groups:
       - name: acc_hardware
         table: acc_hardware
       - name: acc_lpf_hz
+        min: 0
         max: 200
+      - name: acc_lpf_type
+        field: acc_soft_lpf_type
+        table: filter_type
       - name: acczero_x
         field: accZero.raw[X]
         min: INT16_MIN

src/main/sensors/acceleration.c

@@ -75,7 +75,9 @@ STATIC_FASTRAM zeroCalibrationVector_t zeroCalibration;
 
 STATIC_FASTRAM int32_t accADC[XYZ_AXIS_COUNT];
 
-STATIC_FASTRAM biquadFilter_t accFilter[XYZ_AXIS_COUNT];
+STATIC_FASTRAM filter_t accFilter[XYZ_AXIS_COUNT];
+STATIC_FASTRAM filterApplyFnPtr accSoftLpfFilterApplyFn;
+STATIC_FASTRAM void *accSoftLpfFilter[XYZ_AXIS_COUNT];
 
 STATIC_FASTRAM pt1Filter_t accVibeFloorFilter[XYZ_AXIS_COUNT];
 STATIC_FASTRAM pt1Filter_t accVibeFilter[XYZ_AXIS_COUNT];
@@ -85,7 +87,7 @@ STATIC_FASTRAM filterApplyFnPtr accNotchFilterApplyFn;
 STATIC_FASTRAM void *accNotchFilter[XYZ_AXIS_COUNT];
 #endif
 
-PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 2);
+PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 3);
 
 void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
 {
@@ -94,7 +96,8 @@ void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
         .acc_hardware = ACC_AUTODETECT,
         .acc_lpf_hz = 15,
         .acc_notch_hz = 0,
-        .acc_notch_cutoff = 1
+        .acc_notch_cutoff = 1,
+        .acc_soft_lpf_type = FILTER_BIQUAD
     );
     RESET_CONFIG_2(flightDynamicsTrims_t, &instance->accZero,
         .raw[X] = 0,
@@ -552,11 +555,10 @@ void accUpdate(void)
     }
 
     // Filter acceleration
-    if (accelerometerConfig()->acc_lpf_hz) {
-        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            acc.accADCf[axis] = biquadFilterApply(&accFilter[axis], acc.accADCf[axis]);
-        }
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        acc.accADCf[axis] = accSoftLpfFilterApplyFn(accSoftLpfFilter[axis], acc.accADCf[axis]);
     }
+
 
 #ifdef USE_ACC_NOTCH
     if (accelerometerConfig()->acc_notch_hz) {
@@ -597,11 +599,29 @@ void accSetCalibrationValues(void)
 }
 
 void accInitFilters(void)
-{
+{   
+    accSoftLpfFilterApplyFn = nullFilterApply;
+
     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);
+
+        switch (accelerometerConfig()->acc_soft_lpf_type) 
+        {
+        case FILTER_PT1:
+            accSoftLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;
+            for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+                accSoftLpfFilter[axis] = &accFilter[axis].pt1;
+                pt1FilterInit(accSoftLpfFilter[axis], accelerometerConfig()->acc_lpf_hz, acc.accTargetLooptime * 1e-6f);
+            }
+            break;
+        case FILTER_BIQUAD:
+            accSoftLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
+            for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+                accSoftLpfFilter[axis] = &accFilter[axis].biquad;
+                biquadFilterInitLPF(accSoftLpfFilter[axis], accelerometerConfig()->acc_lpf_hz, acc.accTargetLooptime);
+            }
+            break;
         }
+
     }
 
     const float accDt = acc.accTargetLooptime * 1e-6f;

src/main/sensors/acceleration.h

@@ -67,6 +67,7 @@ typedef struct accelerometerConfig_s {
     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
+    uint8_t acc_soft_lpf_type;              // Accelerometer LPF type 
 } accelerometerConfig_t;
 
 PG_DECLARE(accelerometerConfig_t, accelerometerConfig);