Rework Acc filtering

src/main/config/config.c

@@ -492,8 +492,7 @@ static void resetConf(void)
     resetRollAndPitchTrims(&masterConfig.accelerometerTrims);
 
     masterConfig.mag_declination = 0;
-    masterConfig.acc_lpf_hz = 20;
+    masterConfig.acc_lpf_hz = 10.0f;
-    masterConfig.accz_lpf_cutoff = 5.0f;
     masterConfig.accDeadband.xy = 40;
     masterConfig.accDeadband.z = 40;
     masterConfig.acc_unarmedcal = 1;
@@ -730,6 +729,7 @@ void activateConfig(void)
 
     useFailsafeConfig(&masterConfig.failsafeConfig);
     setAccelerationTrims(&masterConfig.accZero);
+    setAccelerationFilter(masterConfig.acc_lpf_hz);
 
     mixerUseConfigs(
 #ifdef USE_SERVOS
@@ -745,7 +745,6 @@ void activateConfig(void)
 
     imuRuntimeConfig.dcm_kp = masterConfig.dcm_kp / 10000.0f;
     imuRuntimeConfig.dcm_ki = masterConfig.dcm_ki / 10000.0f;
-    imuRuntimeConfig.acc_cut_hz = masterConfig.acc_lpf_hz;
     imuRuntimeConfig.acc_unarmedcal = masterConfig.acc_unarmedcal;
     imuRuntimeConfig.small_angle = masterConfig.small_angle;
 
@@ -753,7 +752,6 @@ void activateConfig(void)
         &imuRuntimeConfig,
         &currentProfile->pidProfile,
         &masterConfig.accDeadband,
-        masterConfig.accz_lpf_cutoff,
         masterConfig.throttle_correction_angle
     );
 

src/main/config/config_master.h

@@ -72,8 +72,7 @@ typedef struct master_t {
 
     rollAndPitchTrims_t accelerometerTrims; // accelerometer trim
 
-    uint8_t acc_lpf_hz;                     // Set the Low Pass Filter factor for ACC. Reducing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
+    float acc_lpf_hz;                       // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz
-    float accz_lpf_cutoff;                  // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz
     accDeadband_t accDeadband;
     barometerConfig_t barometerConfig;
     uint8_t acc_unarmedcal;                 // turn automatic acc compensation on/off

src/main/flight/imu.c

@@ -56,7 +56,6 @@
 // http://gentlenav.googlecode.com/files/fastRotations.pdf
 #define SPIN_RATE_LIMIT 20
 
-int16_t accSmooth[XYZ_AXIS_COUNT];
 int32_t accSum[XYZ_AXIS_COUNT];
 
 uint32_t accTimeSum = 0;        // keep track for integration of acc
@@ -111,14 +110,13 @@ void imuConfigure(
     imuRuntimeConfig_t *initialImuRuntimeConfig,
     pidProfile_t *initialPidProfile,
     accDeadband_t *initialAccDeadband,
-    float accz_lpf_cutoff,
     uint16_t throttle_correction_angle
 )
 {
     imuRuntimeConfig = initialImuRuntimeConfig;
     pidProfile = initialPidProfile;
     accDeadband = initialAccDeadband;
-    fc_acc = calculateAccZLowPassFilterRCTimeConstant(accz_lpf_cutoff);
+    fc_acc = calculateAccZLowPassFilterRCTimeConstant(5.0f); // Set to fix value
     throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
 }
 
@@ -377,11 +375,8 @@ static bool isMagnetometerHealthy(void)
 
 static void imuCalculateEstimatedAttitude(void)
 {
-    static biquad_t accLPFState[3];
-    static bool accStateIsSet;
     static uint32_t previousIMUUpdateTime;
     float rawYawError = 0;
-    int32_t axis;
     bool useAcc = false;
     bool useMag = false;
     bool useYaw = false;
@@ -390,20 +385,6 @@ static void imuCalculateEstimatedAttitude(void)
     uint32_t deltaT = currentTime - previousIMUUpdateTime;
     previousIMUUpdateTime = currentTime;
 
-    // Smooth and use only valid accelerometer readings
-    for (axis = 0; axis < 3; axis++) {
-        if (imuRuntimeConfig->acc_cut_hz) {
-            if (accStateIsSet) {
-                accSmooth[axis] = lrintf(applyBiQuadFilter((float) accADC[axis], &accLPFState[axis]));
-            } else {
-                for (axis = 0; axis < 3; axis++) BiQuadNewLpf(imuRuntimeConfig->acc_cut_hz, &accLPFState[axis], 1000);
-                accStateIsSet = true;
-            }
-        } else {
-            accSmooth[axis] = accADC[axis];
-        }
-    }
-
     if (imuIsAccelerometerHealthy()) {
         useAcc = true;
     }
@@ -445,9 +426,9 @@ void imuUpdateGyroAndAttitude(void)
     if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {
         imuCalculateEstimatedAttitude();
     } else {
-        accADC[X] = 0;
+        accSmooth[X] = 0;
-        accADC[Y] = 0;
+        accSmooth[Y] = 0;
-        accADC[Z] = 0;
+        accSmooth[Z] = 0;
     }
 }
 

src/main/flight/imu.h

@@ -21,7 +21,6 @@ extern int16_t throttleAngleCorrection;
 extern uint32_t accTimeSum;
 extern int accSumCount;
 extern float accVelScale;
-extern int16_t accSmooth[XYZ_AXIS_COUNT];
 extern int32_t accSum[XYZ_AXIS_COUNT];
 
 #define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
@@ -74,7 +73,6 @@ void imuConfigure(
     imuRuntimeConfig_t *initialImuRuntimeConfig,
     pidProfile_t *initialPidProfile,
     accDeadband_t *initialAccDeadband,
-    float accz_lpf_cutoff,
     uint16_t throttle_correction_angle
 );
 
@@ -84,7 +82,7 @@ void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims);
 void imuUpdateGyroAndAttitude(void);
 float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
 int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
-float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);
+float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_hz);
 
 int16_t imuCalculateHeading(t_fp_vector *vec);
 

src/main/io/serial_cli.c

@@ -658,10 +658,9 @@ const clivalue_t valueTable[] = {
 #endif
 
     { "acc_hardware",               VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.acc_hardware, .config.lookup = { TABLE_ACC_HARDWARE } },
-    { "acc_lpf_hz",                 VAR_UINT8  | MASTER_VALUE, &masterConfig.acc_lpf_hz, .config.minmax = { 0,  200 } },
+    { "acc_lpf_hz",                 VAR_FLOAT  | MASTER_VALUE, &masterConfig.acc_lpf_hz, .config.minmax = { 0,  400 } },
     { "accxy_deadband",             VAR_UINT8  | MASTER_VALUE, &masterConfig.accDeadband.xy, .config.minmax = { 0,  100 } },
     { "accz_deadband",              VAR_UINT8  | MASTER_VALUE, &masterConfig.accDeadband.z, .config.minmax = { 0,  100 } },
-    { "accz_lpf_cutoff",            VAR_FLOAT  | MASTER_VALUE, &masterConfig.accz_lpf_cutoff, .config.minmax = { 1,  20 } },
     { "acc_unarmedcal",             VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, &masterConfig.acc_unarmedcal, .config.lookup = { TABLE_OFF_ON } },
     { "acc_trim_pitch",             VAR_INT16  | MASTER_VALUE, &masterConfig.accelerometerTrims.values.pitch, .config.minmax = { -300,  300 } },
     { "acc_trim_roll",              VAR_INT16  | MASTER_VALUE, &masterConfig.accelerometerTrims.values.roll, .config.minmax = { -300,  300 } },

src/main/main.c

@@ -651,7 +651,6 @@ int main(void) {
 
     setTaskEnabled(TASK_GYROPID, true);
     if(sensors(SENSOR_ACC)) {
-        uint32_t accTargetLooptime = 0;
         setTaskEnabled(TASK_ACCEL, true);
         switch(targetLooptime) {
             case(500):

src/main/sensors/acceleration.c

@@ -17,14 +17,17 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <math.h>
 
 #include "platform.h"
 
 #include "common/axis.h"
+#include "common/filter.h"
 
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
 #include "drivers/system.h"
+#include "drivers/gyro_sync.h"
 
 #include "sensors/battery.h"
 #include "sensors/sensors.h"
@@ -35,11 +38,12 @@
 
 #include "sensors/acceleration.h"
 
-int32_t accADC[XYZ_AXIS_COUNT];
+int32_t accSmooth[XYZ_AXIS_COUNT];
 
 acc_t acc;                       // acc access functions
 sensor_align_e accAlign = 0;
 uint16_t acc_1G = 256;          // this is the 1G measured acceleration.
+uint32_t accTargetLooptime;
 
 uint16_t calibratingA = 0;      // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
 
@@ -51,6 +55,10 @@ extern bool AccInflightCalibrationActive;
 
 static flightDynamicsTrims_t *accelerationTrims;
 
+static float accLpfCutHz = 0;
+static biquad_t accFilterState[XYZ_AXIS_COUNT];
+static bool accFilterInitialised = false;
+
 void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
 {
     calibratingA = calibrationCyclesRequired;
@@ -89,10 +97,10 @@ void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
             a[axis] = 0;
 
         // Sum up CALIBRATING_ACC_CYCLES readings
-        a[axis] += accADC[axis];
+        a[axis] += accSmooth[axis];
 
         // Reset global variables to prevent other code from using un-calibrated data
-        accADC[axis] = 0;
+        accSmooth[axis] = 0;
         accelerationTrims->raw[axis] = 0;
     }
 
@@ -131,9 +139,9 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri
             if (InflightcalibratingA == 50)
                 b[axis] = 0;
             // Sum up 50 readings
-            b[axis] += accADC[axis];
+            b[axis] += accSmooth[axis];
             // Clear global variables for next reading
-            accADC[axis] = 0;
+            accSmooth[axis] = 0;
             accelerationTrims->raw[axis] = 0;
         }
         // all values are measured
@@ -165,9 +173,9 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri
 
 void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims)
 {
-    accADC[X] -= accelerationTrims->raw[X];
+    accSmooth[X] -= accelerationTrims->raw[X];
-    accADC[Y] -= accelerationTrims->raw[Y];
+    accSmooth[Y] -= accelerationTrims->raw[Y];
-    accADC[Z] -= accelerationTrims->raw[Z];
+    accSmooth[Z] -= accelerationTrims->raw[Z];
 }
 
 void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
@@ -179,9 +187,22 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
         return;
     }
 
-    for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) accADC[axis] = accADCRaw[axis];
+    for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) accSmooth[axis] = accADCRaw[axis];
 
-    alignSensors(accADC, accADC, accAlign);
+    if (accLpfCutHz) {
+        if (!accFilterInitialised) {
+            if (accTargetLooptime) {  /* Initialisation needs to happen once sample rate is known */
+                for (axis = 0; axis < 3; axis++) BiQuadNewLpf(accLpfCutHz, &accFilterState[axis], accTargetLooptime);
+                accFilterInitialised = true;
+            }
+        }
+
+        if (accFilterInitialised) {
+            for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) accSmooth[axis] = lrintf(applyBiQuadFilter((float) accSmooth[axis], &accFilterState[axis]));
+        }
+    }
+
+    alignSensors(accSmooth, accSmooth, accAlign);
 
     if (!isAccelerationCalibrationComplete()) {
         performAcclerationCalibration(rollAndPitchTrims);
@@ -198,3 +219,8 @@ void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse)
 {
     accelerationTrims = accelerationTrimsToUse;
 }
+
+void setAccelerationFilter(float initialAccLpfCutHz)
+{
+    accLpfCutHz = initialAccLpfCutHz;
+}

src/main/sensors/acceleration.h

@@ -36,8 +36,9 @@ typedef enum {
 extern sensor_align_e accAlign;
 extern acc_t acc;
 extern uint16_t acc_1G;
+extern uint32_t accTargetLooptime;
 
-extern int32_t accADC[XYZ_AXIS_COUNT];
+extern int32_t accSmooth[XYZ_AXIS_COUNT];
 
 typedef struct rollAndPitchTrims_s {
     int16_t roll;
@@ -54,3 +55,4 @@ void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
 void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims);
 void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims);
 void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse);
+void setAccelerationFilter(float initialAccLpfCutHz);