Configurable Soft LPF values (gyro_lpf_hz and dterm_lpf_hz)

src/main/common/filter.c

@@ -27,6 +27,13 @@
 #include "drivers/gyro_sync.h"
 
 
+#define M_LN2_FLOAT	0.69314718055994530942f
+#define M_PI_FLOAT	3.14159265358979323846f
+
+
+#define BIQUAD_GAIN 6.0f          /* gain in db */
+#define BIQUAD_BANDWIDTH 1.9f     /* bandwidth in octaves */
+
 // PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
 float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dT) {
 
@@ -40,69 +47,48 @@ float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float
     return filter->state;
 }
 
-void setBiQuadCoefficients(int type, biquad_t *state) {
+/* sets up a biquad Filter */
+biquad_t *BiQuadNewLpf(uint8_t filterCutFreq)
+{
+	float samplingRate;
+    samplingRate = 1 / (targetLooptime * 0.000001f);
+
+    biquad_t *b;
+    float omega, sn, cs, alpha;
+    float a0, a1, a2, b0, b1, b2;
+
+    b = malloc(sizeof(biquad_t));
+    if (b == NULL)
+        return NULL;
+
+    /* setup variables */
+    omega = 2 * M_PI_FLOAT * (float) filterCutFreq / samplingRate;
+    sn = sinf(omega);
+    cs = cosf(omega);
+    alpha = sn * sinf(M_LN2_FLOAT /2 * BIQUAD_BANDWIDTH * omega /sn);
+
+    b0 = (1 - cs) /2;
+    b1 = 1 - cs;
+    b2 = (1 - cs) /2;
+    a0 = 1 + alpha;
+    a1 = -2 * cs;
+    a2 = 1 - alpha;
+
+    /* precompute the coefficients */
+    b->a0 = b0 /a0;
+    b->a1 = b1 /a0;
+    b->a2 = b2 /a0;
+    b->a3 = a1 /a0;
+    b->a4 = a2 /a0;
 
     /* zero initial samples */
-    state->x1=0;
+    b->x1 = b->x2 = 0;
-    state->x2=0;
+    b->y1 = b->y2 = 0;
-    state->y1=0;
-    state->y2=0;
 
-	/* set coefficients */
+    return b;
-	switch(type) {
-	    case(GYRO_FILTER):
-            switch (targetLooptime) {
-                case(SAMPLE_RATE_2KHZ):
-                    state->a0= 0.007820199;
-                    state->a1= 0.015640399;
-                    state->a2= 0.007820199;
-                    state->a3= -1.73472382;
-                    state->a4= 0.766004619;
-                    break;
-                case(SAMPLE_RATE_2K6HZ):
-                    state->a0= 0.004538377;
-                    state->a1= 0.009076754;
-                    state->a2= 0.004538377;
-                    state->a3= -1.80059391;
-                    state->a4= 0.818747415;
-                    break;
-                default:
-                case(SAMPLE_RATE_1KHZ):
-                    state->a0= 0.027859711;
-                    state->a1= 0.055719422;
-                    state->a2= 0.027859711;
-                    state->a3= -1.47547752;
-                    state->a4= 0.586916365;
-	        }
-            break;
-	    case(DELTA_FILTER):
-            switch (targetLooptime) {
-	            case(SAMPLE_RATE_2KHZ):
-                    state->a0= 0.003621679;
-                    state->a1= 0.007243357;
-                    state->a2= 0.003621679;
-                    state->a3= -1.82269350;
-                    state->a4= 0.837180216;
-                    break;
-	            case(SAMPLE_RATE_2K6HZ):
-                    state->a0= 0.002081573;
-                    state->a1= 0.004163147;
-                    state->a2= 0.002081573;
-                    state->a3= -1.86685796;
-                    state->a4= 0.875184256;
-                    break;
-	            default:
-	            case(SAMPLE_RATE_1KHZ):
-                    state->a0= 0.013359181;
-                    state->a1= 0.026718362;
-                    state->a2= 0.013359181;
-                    state->a3= -1.64745762;
-                    state->a4= 0.700894342;
-            }
-	}
 }
 
-/* Computes a BiQuad filter on a sample */
+/* Computes a biquad_t filter on a sample */
 float applyBiQuadFilter(float sample, biquad_t *state)
 {
     float result;

src/main/common/filter.h

@@ -29,19 +29,6 @@ typedef struct biquad_s {
     float x1, x2, y1, y2;
 } biquad_t;
 
-typedef enum {
-    GYRO_FILTER,
-    ACCELEROMETER_FILTER,
-    DELTA_FILTER
-} filterType_e;
-
-typedef enum {
-    SAMPLE_RATE_1KHZ = 1000,
-	SAMPLE_RATE_2KHZ = 500,
-	SAMPLE_RATE_2K6HZ = 375,
-	SAMPLE_RATE_4KHZ = 250
-} sampleRates_e;
-
 float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt);
-float applyBiQuadFilter(float sample, biquad_t * b);
+float applyBiQuadFilter(float sample, biquad_t *state);
-void setBiQuadCoefficients(int type, biquad_t *state);
+biquad_t *BiQuadNewLpf(uint8_t filterCutFreq);

src/main/config/config.c

@@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0;
 static uint8_t currentControlRateProfileIndex = 0;
 controlRateConfig_t *currentControlRateProfile;
 
-static const uint8_t EEPROM_CONF_VERSION = 117;
+static const uint8_t EEPROM_CONF_VERSION = 118;
 
 static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
 {
@@ -176,7 +176,8 @@ static void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->I8[PIDVEL] = 45;
     pidProfile->D8[PIDVEL] = 1;
 
-    pidProfile->gyro_soft_lpf = 1;   // filtering ON by default
+    pidProfile->gyro_lpf_hz = 60;    // filtering ON by default
+    pidProfile->dterm_lpf_hz = 50;   // filtering ON by default
     pidProfile->airModeInsaneAcrobilityFactor = 0;
 
     pidProfile->P_f[ROLL] = 1.5f;     // new PID with preliminary defaults test carefully
@@ -747,7 +748,7 @@ void activateConfig(void)
         &currentProfile->pidProfile
     );
 
-    useGyroConfig(&masterConfig.gyroConfig, currentProfile->pidProfile.gyro_soft_lpf);
+    useGyroConfig(&masterConfig.gyroConfig, &currentProfile->pidProfile.gyro_lpf_hz);
 
 #ifdef TELEMETRY
     telemetryUseConfig(&masterConfig.telemetryConfig);

src/main/flight/pid.c

@@ -113,8 +113,8 @@ void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile) {
 const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
 
 static airModePlus_t airModePlusAxisState[3];
-static bool deltaStateIsSet = false;
 static biquad_t deltaBiQuadState[3];
+static bool deltaStateIsSet;
 
 static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
@@ -127,8 +127,8 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
     float horizonLevelStrength = 1;
     static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
 
-    if (!deltaStateIsSet) {
+    if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
-        for (axis = 0; axis < 3; axis++) setBiQuadCoefficients(DELTA_FILTER, &deltaBiQuadState[axis]);
+        for (axis = 0; axis < 3; axis++) deltaBiQuadState[axis] = *(biquad_t *)BiQuadNewLpf(pidProfile->dterm_lpf_hz);
         deltaStateIsSet = true;
     }
 
@@ -214,7 +214,9 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         delta = RateError - lastError[axis];
         lastError[axis] = RateError;
 
+        if (deltaStateIsSet) {
             delta = applyBiQuadFilter(delta, &deltaBiQuadState[axis]);
+        }
 
         // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
         // would be scaled by different dt each time. Division by dT fixes that.
@@ -256,8 +258,8 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
 
     int8_t horizonLevelStrength = 100;
 
-    if (!deltaStateIsSet) {
+    if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
-        for (axis = 0; axis < 3; axis++) setBiQuadCoefficients(DELTA_FILTER, &deltaBiQuadState[axis]);
+        for (axis = 0; axis < 3; axis++) deltaBiQuadState[axis] = *(biquad_t *)BiQuadNewLpf(pidProfile->dterm_lpf_hz);
         deltaStateIsSet = true;
     }
 
@@ -345,7 +347,9 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
         delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
         lastError[axis] = RateError;
 
+        if (deltaStateIsSet) {
             delta = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis]));
+        }
 
         // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
         // would be scaled by different dt each time. Division by dT fixes that.

src/main/flight/pid.h

@@ -59,7 +59,8 @@ typedef struct pidProfile_s {
     uint8_t H_sensitivity;
 
     uint16_t airModeInsaneAcrobilityFactor; // Air mode acrobility factor
-    uint8_t gyro_soft_lpf;                  // Gyro FIR filter
+    uint8_t gyro_lpf_hz;                    // Gyro Soft filter in hz
+    uint8_t dterm_lpf_hz;                   // Delta Filter in hz
 
 #ifdef GTUNE
     uint8_t  gtune_lolimP[3];               // [0..200] Lower limit of P during G tune

src/main/io/serial_cli.c

@@ -657,7 +657,8 @@ const clivalue_t valueTable[] = {
     { "i_vel",                      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], .config.minmax = { 0,  200 } },
     { "d_vel",                      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], .config.minmax = { 0,  200 } },
 
-    { "gyro_soft_lpf",              VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.gyro_soft_lpf, .config.lookup = { TABLE_OFF_ON } },
+    { "gyro_lpf_hz",                VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_lpf_hz, .config.minmax = {0, 255 } },
+    { "dterm_lpf_hz",               VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 255 } },
     { "insane_acro_factor",         VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.airModeInsaneAcrobilityFactor, .config.minmax = {0, 100 } },
 
 #ifdef BLACKBOX

src/main/sensors/gyro.c

@@ -17,8 +17,10 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <math.h>
 
 #include "platform.h"
+#include "debug.h"
 
 #include "common/axis.h"
 #include "common/maths.h"
@@ -26,6 +28,7 @@
 
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
+#include "drivers/gyro_sync.h"
 #include "sensors/sensors.h"
 #include "io/beeper.h"
 #include "io/statusindicator.h"
@@ -39,18 +42,23 @@ int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
 
 static gyroConfig_t *gyroConfig;
 static biquad_t gyroBiQuadState[3];
-static bool useSoftFilter;
+static bool gyroFilterStateIsSet;
+static uint8_t gyroLpfCutFreq;
+int axis;
 
 gyro_t gyro;                      // gyro access functions
 sensor_align_e gyroAlign = 0;
 
-void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t useFilter)
+void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t *gyro_lpf_hz)
 {
-    int axis;
     gyroConfig = gyroConfigToUse;
-    if (useFilter) {
+    gyroLpfCutFreq = *gyro_lpf_hz;
-        useSoftFilter = true;
+}
-        for (axis = 0; axis < 3; axis++) setBiQuadCoefficients(GYRO_FILTER, &gyroBiQuadState[axis]);
+
+void initGyroFilterCoefficients(void) {
+    if (gyroLpfCutFreq && targetLooptime) {  /* Initialisation needs to happen once samplingrate is known */
+        for (axis = 0; axis < 3; axis++) gyroBiQuadState[axis] = *(biquad_t *)BiQuadNewLpf(gyroLpfCutFreq);
+        gyroFilterStateIsSet = true;
     }
 }
 
@@ -129,14 +137,16 @@ void gyroUpdate(void)
         return;
     }
 
-    if (useSoftFilter) {
-        int axis;
-        //for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) filterApplyFIR(&gyroADC[axis], gyroFIRState[axis], gyroFIRTable);
-        for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = applyBiQuadFilter((float) gyroADC[axis], &gyroBiQuadState[axis]);
-    }
-
     alignSensors(gyroADC, gyroADC, gyroAlign);
 
+    if (gyroLpfCutFreq) {
+        if (!gyroFilterStateIsSet) {
+            initGyroFilterCoefficients();
+        } else {
+            for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], &gyroBiQuadState[axis]));
+        }
+    }
+
     if (!isGyroCalibrationComplete()) {
         performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold);
     }

src/main/sensors/gyro.h

@@ -39,7 +39,7 @@ typedef struct gyroConfig_s {
     uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
 } gyroConfig_t;
 
-void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t useFilter);
+void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t *gyro_lpf_hz);
 void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
 void gyroUpdate(void);
 bool isGyroCalibrationComplete(void);