Refactor BiQuad // Set filters to floats

src/main/common/filter.c

@@ -24,30 +24,23 @@
 #include "common/filter.h"
 #include "common/maths.h"
 
-#include "drivers/gyro_sync.h"
-
-
 #define M_LN2_FLOAT	0.69314718055994530942f
 #define M_PI_FLOAT	3.14159265358979323846f
 
 #define BIQUAD_BANDWIDTH 1.9f     /* bandwidth in octaves */
 
 /* sets up a biquad Filter */
-void BiQuadNewLpf(uint8_t filterCutFreq, biquad_t *newState, float refreshRate)
+void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate)
 {
-	float samplingRate;
+    float sampleRate;
 
-    if (!refreshRate) {
+    sampleRate = 1 / ((float)refreshRate * 0.000001f);
-        samplingRate = 1 / (targetLooptime * 0.000001f);
-    } else {
-        samplingRate = refreshRate;
-    }
 
     float omega, sn, cs, alpha;
     float a0, a1, a2, b0, b1, b2;
 
     /* setup variables */
-    omega = 2 * M_PI_FLOAT * (float) filterCutFreq / samplingRate;
+    omega = 2 * M_PI_FLOAT * filterCutFreq / sampleRate;
     sn = sinf(omega);
     cs = cosf(omega);
     alpha = sn * sinf(M_LN2_FLOAT /2 * BIQUAD_BANDWIDTH * omega /sn);
@@ -60,11 +53,11 @@ void BiQuadNewLpf(uint8_t filterCutFreq, biquad_t *newState, float refreshRate)
     a2 = 1 - alpha;
 
     /* precompute the coefficients */
-    newState->a0 = b0 /a0;
+    newState->b0 = b0 /a0;
-    newState->a1 = b1 /a0;
+    newState->b1 = b1 /a0;
-    newState->a2 = b2 /a0;
+    newState->b2 = b2 /a0;
-    newState->a3 = a1 /a0;
+    newState->a1 = a1 /a0;
-    newState->a4 = a2 /a0;
+    newState->a2 = a2 /a0;
 
     /* zero initial samples */
     newState->x1 = newState->x2 = 0;
@@ -77,8 +70,8 @@ float applyBiQuadFilter(float sample, biquad_t *state)
     float result;
 
     /* compute result */
-    result = state->a0 * sample + state->a1 * state->x1 + state->a2 * state->x2 -
+    result = state->b0 * sample + state->b1 * state->x1 + state->b2 * state->x2 -
-        state->a3 * state->y1 - state->a4 * state->y2;
+        state->a1 * state->y1 - state->a2 * state->y2;
 
     /* shift x1 to x2, sample to x1 */
     state->x2 = state->x1;

src/main/common/filter.h

@@ -17,9 +17,9 @@
 
 /* this holds the data required to update samples thru a filter */
 typedef struct biquad_s {
-    float a0, a1, a2, a3, a4;
+    float b0, b1, b2, a1, a2;
     float x1, x2, y1, y2;
 } biquad_t;
 
 float applyBiQuadFilter(float sample, biquad_t *state);
-void BiQuadNewLpf(uint8_t filterCutFreq, biquad_t *newState, float refreshRate);
+void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate);

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 = 119;
+static const uint8_t EEPROM_CONF_VERSION = 120;
 
 static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
 {
@@ -176,7 +176,6 @@ static void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->I8[PIDVEL] = 45;
     pidProfile->D8[PIDVEL] = 1;
 
-    pidProfile->gyro_lpf_hz = 60;    // filtering ON by default
     pidProfile->dterm_lpf_hz = 0;    // filtering ON by default
     pidProfile->deltaFromGyro = 1;
     pidProfile->airModeInsaneAcrobilityFactor = 0;
@@ -406,6 +405,7 @@ static void resetConf(void)
     masterConfig.dcm_kp = 2500;                // 1.0 * 10000
     masterConfig.dcm_ki = 0;                    // 0.003 * 10000
     masterConfig.gyro_lpf = 1;                 // 188HZ
+    masterConfig.gyro_soft_lpf_hz = 60;
 
     resetAccelerometerTrims(&masterConfig.accZero);
 
@@ -770,7 +770,7 @@ void activateConfig(void)
         &currentProfile->pidProfile
     );
 
-    useGyroConfig(&masterConfig.gyroConfig, currentProfile->pidProfile.gyro_lpf_hz);
+    useGyroConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz);
 
 #ifdef TELEMETRY
     telemetryUseConfig(&masterConfig.telemetryConfig);

src/main/config/config_master.h

@@ -51,6 +51,7 @@ typedef struct master_t {
     uint8_t acc_hardware;                   // Which acc hardware to use on boards with more than one device
     uint8_t acc_for_fast_looptime;          // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes.
     uint8_t gyro_lpf;                      // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
+    float gyro_soft_lpf_hz;                 // Biqyad gyro lpf hz
     uint16_t dcm_kp;                        // DCM filter proportional gain ( x 10000)
     uint16_t dcm_ki;                        // DCM filter integral gain ( x 10000)
 

src/main/flight/pid.c

@@ -142,7 +142,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
     float horizonLevelStrength = 1;
 
     if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
-        for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0);
+        for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetLooptime);
         deltaStateIsSet = true;
     }
 
@@ -284,7 +284,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
     int8_t horizonLevelStrength = 100;
 
     if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
-        for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0);
+        for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetLooptime);
         deltaStateIsSet = true;
     }
 

src/main/flight/pid.h

@@ -60,8 +60,7 @@ typedef struct pidProfile_s {
     uint8_t H_sensitivity;
 
     uint16_t airModeInsaneAcrobilityFactor; // Air mode acrobility factor
-    uint8_t gyro_lpf_hz;                    // Gyro Soft filter in hz
+    float dterm_lpf_hz;                     // Delta Filter in hz
-    uint8_t dterm_lpf_hz;                   // Delta Filter in hz
     uint8_t deltaFromGyro;                  // Alternative delta Calculation
 
 #ifdef GTUNE

src/main/io/serial_cli.c

@@ -601,6 +601,7 @@ const clivalue_t valueTable[] = {
     { "max_angle_inclination",      VAR_UINT16 | MASTER_VALUE,  &masterConfig.max_angle_inclination, .config.minmax = { 100,  900 } },
 
     { "gyro_lpf",                   VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
+    { "gyro_soft_lpf",              VAR_FLOAT  | MASTER_VALUE,  &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0,  500 } },
     { "moron_threshold",            VAR_UINT8  | MASTER_VALUE,  &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0,  128 } },
     { "imu_dcm_kp",                 VAR_UINT16 | MASTER_VALUE,  &masterConfig.dcm_kp, .config.minmax = { 0,  50000 } },
     { "imu_dcm_ki",                 VAR_UINT16 | MASTER_VALUE,  &masterConfig.dcm_ki, .config.minmax = { 0,  50000 } },
@@ -705,8 +706,7 @@ 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 } },
 
-    { "soft_gyro_lpf_hz",           VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_lpf_hz, .config.minmax = {0, 255 } },
+    { "dterm_lpf_hz",               VAR_FLOAT  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
-    { "dterm_lpf_hz",               VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 255 } },
     { "acro_plus_factor",           VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.airModeInsaneAcrobilityFactor, .config.minmax = {0, 100 } },
 
 #ifdef BLACKBOX

src/main/mw.c

@@ -184,7 +184,7 @@ void filterRc(void){
 
     /* Initialize cycletime filter */
     if (!filterIsSet) {
-        BiQuadNewLpf(1, &filteredCycleTimeState, 0);
+        BiQuadNewLpf(0.5f, &filteredCycleTimeState, targetLooptime);
         filterIsSet = true;
     }
 

src/main/sensors/gyro.c

@@ -41,15 +41,15 @@ int16_t gyroADC[XYZ_AXIS_COUNT];
 int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
 
 static gyroConfig_t *gyroConfig;
-static biquad_t gyroBiQuadState[3];
+static biquad_t gyroFilterState[3];
 static bool gyroFilterStateIsSet;
-static uint8_t gyroLpfCutFreq;
+static float gyroLpfCutFreq;
 int axis;
 
 gyro_t gyro;                      // gyro access functions
 sensor_align_e gyroAlign = 0;
 
-void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz)
+void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz)
 {
     gyroConfig = gyroConfigToUse;
     gyroLpfCutFreq = gyro_lpf_hz;
@@ -57,7 +57,7 @@ void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz)
 
 void initGyroFilterCoefficients(void) {
     if (gyroLpfCutFreq && targetLooptime) {  /* Initialisation needs to happen once samplingrate is known */
-        for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroBiQuadState[axis], 0);
+        for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroFilterState[axis], targetLooptime);
         gyroFilterStateIsSet = true;
     }
 }
@@ -140,10 +140,10 @@ void gyroUpdate(void)
     alignSensors(gyroADC, gyroADC, gyroAlign);
 
     if (gyroLpfCutFreq) {
-        if (!gyroFilterStateIsSet) {
+        if (!gyroFilterStateIsSet) initGyroFilterCoefficients(); /* initialise filter coefficients */
-            initGyroFilterCoefficients();
+
-        } else {
+        if (gyroFilterStateIsSet) {
-            for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], &gyroBiQuadState[axis]));
+            for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], &gyroFilterState[axis]));
         }
     }
 

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 gyro_lpf_hz);
+void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz);
 void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
 void gyroUpdate(void);
 bool isGyroCalibrationComplete(void);