Made filter naming, parameters and state consistent

src/main/common/filter.c

@@ -17,76 +17,81 @@
 
 #include <stdbool.h>
 #include <stdint.h>
-#include <stdlib.h>
 #include <math.h>
 
-#include "common/axis.h"
 #include "common/filter.h"
 #include "common/maths.h"
 
-#define M_LN2_FLOAT	0.69314718055994530942f
+#define M_LN2_FLOAT 0.69314718055994530942f
-#define M_PI_FLOAT	3.14159265358979323846f
+#define M_PI_FLOAT  3.14159265358979323846f
 
 #define BIQUAD_BANDWIDTH 1.9f     /* bandwidth in octaves */
 #define BIQUAD_Q 1.0f / sqrtf(2.0f)     /* quality factor - butterworth*/
 
-// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
+// PT1 Low Pass filter
-float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dT) {
 
-	// Pre calculate and store RC
+void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT)
-	if (!filter->RC) {
+{
-		filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut );
+    filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut );
-	}
+    filter->dT = dT;
+}
 
-    filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
+float pt1FilterApply(pt1Filter_t *filter, float input)
+{
+    filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state);
+    return filter->state;
+}
+
+float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT)
+{
+    // Pre calculate and store RC
+    if (!filter->RC) {
+        filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut );
+        filter->dT = dT;
+    }
+
+    filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state);
 
     return filter->state;
 }
 
 /* sets up a biquad Filter */
-void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate)
+void biquadFilterInit(biquadFilter_t *filter, float filterCutFreq, uint32_t refreshRate)
 {
-    float sampleRate;
+    const float sampleRate = 1 / ((float)refreshRate * 0.000001f);
 
-    sampleRate = 1 / ((float)refreshRate * 0.000001f);
+    // setup variables
-
+    const float omega = 2 * M_PI_FLOAT * filterCutFreq / sampleRate;
-    float omega, sn, cs, alpha;
+    const float sn = sinf(omega);
-    float a0, a1, a2, b0, b1, b2;
+    const float cs = cosf(omega);
-
-    /* setup variables */
-    omega = 2 * M_PI_FLOAT * filterCutFreq / sampleRate;
-    sn = sinf(omega);
-    cs = cosf(omega);
     //this is wrong, should be hyperbolic sine
     //alpha = sn * sinf(M_LN2_FLOAT /2 * BIQUAD_BANDWIDTH * omega /sn);
-    alpha = sn / (2 * BIQUAD_Q);
+    const float alpha = sn / (2 * BIQUAD_Q);
 
-    b0 = (1 - cs) / 2;
+    const float b0 = (1 - cs) / 2;
-    b1 = 1 - cs;
+    const float b1 = 1 - cs;
-    b2 = (1 - cs) / 2;
+    const float b2 = (1 - cs) / 2;
-    a0 = 1 + alpha;
+    const float a0 = 1 + alpha;
-    a1 = -2 * cs;
+    const float a1 = -2 * cs;
-    a2 = 1 - alpha;
+    const float a2 = 1 - alpha;
 
-    /* precompute the coefficients */
+    // precompute the coefficients
-    newState->b0 = b0 / a0;
+    filter->b0 = b0 / a0;
-    newState->b1 = b1 / a0;
+    filter->b1 = b1 / a0;
-    newState->b2 = b2 / a0;
+    filter->b2 = b2 / a0;
-    newState->a1 = a1 / a0;
+    filter->a1 = a1 / a0;
-    newState->a2 = a2 / a0;
+    filter->a2 = a2 / a0;
 
-    /* zero initial samples */
+    // zero initial samples
-    newState->d1 = newState->d2 = 1;
+    filter->d1 = filter->d2 = 0;
 }
 
 /* Computes a biquad_t filter on a sample */
-float applyBiQuadFilter(float sample, biquad_t *state) //direct form 2 transposed
+float biquadFilterApply(biquadFilter_t *filter, float input)
 {
-    float result;
+    const float result = filter->b0 * input + filter->d1;
-
+    filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2;
-    result = state->b0 * sample + state->d1;
+    filter->d2 = filter->b2 * input - filter->a2 * result;
-    state->d1 = state->b1 * sample - state->a1 * result + state->d2;
-    state->d2 = state->b2 * sample - state->a2 * result;
     return result;
 }
 

src/main/common/filter.h

@@ -17,20 +17,26 @@
 
 #define DELTA_MAX_SAMPLES 12
 
-typedef struct filterStatePt1_s {
+typedef struct pt1Filter_s {
-	float state;
+    float state;
-	float RC;
+    float RC;
-	float constdT;
+    float dT;
-} filterStatePt1_t;
+} pt1Filter_t;
 
 /* this holds the data required to update samples thru a filter */
-typedef struct biquad_s {
+typedef struct biquadFilter_s {
     float b0, b1, b2, a1, a2;
     float d1, d2;
-} biquad_t;
+} biquadFilter_t;
+
+
+void biquadFilterInit(biquadFilter_t *filter, float filterCutFreq, uint32_t refreshRate);
+float biquadFilterApply(biquadFilter_t *filter, float input);
+
+void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT);
+float pt1FilterApply(pt1Filter_t *filter, float input);
+float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT);
 
-float applyBiQuadFilter(float sample, biquad_t *state);
-float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dt);
 int32_t filterApplyAverage(int32_t input, uint8_t averageCount, int32_t averageState[DELTA_MAX_SAMPLES]);
 float filterApplyAveragef(float input, uint8_t averageCount, float averageState[DELTA_MAX_SAMPLES]);
-void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate);
+

src/main/flight/imu.c

@@ -28,7 +28,6 @@
 #include "debug.h"
 
 #include "common/axis.h"
-#include "common/filter.h"
 
 #include "drivers/system.h"
 #include "drivers/sensor.h"

src/main/flight/mixer.c

@@ -946,7 +946,7 @@ void filterServos(void)
 #ifdef USE_SERVOS
     static int16_t servoIdx;
     static bool servoFilterIsSet;
-    static biquad_t servoFilterState[MAX_SUPPORTED_SERVOS];
+    static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS];
 
 #if defined(MIXER_DEBUG)
     uint32_t startTime = micros();
@@ -955,11 +955,11 @@ void filterServos(void)
     if (mixerConfig->servo_lowpass_enable) {
         for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
             if (!servoFilterIsSet) {
-                BiQuadNewLpf(mixerConfig->servo_lowpass_freq, &servoFilterState[servoIdx], targetPidLooptime);
+                biquadFilterInit(&servoFilter[servoIdx], mixerConfig->servo_lowpass_freq, targetPidLooptime);
                 servoFilterIsSet = true;
             }
 
-            servo[servoIdx] = lrintf(applyBiQuadFilter((float) servo[servoIdx], &servoFilterState[servoIdx]));
+            servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx]));
             // Sanity check
             servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);
         }

src/main/flight/pid.c

@@ -112,8 +112,8 @@ float getdT (void)
 
 const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
 
-static filterStatePt1_t deltaFilterState[3];
+static pt1Filter_t deltaFilter[3];
-static filterStatePt1_t yawFilterState;
+static pt1Filter_t yawFilter;
 
 #ifndef SKIP_PID_LUXFLOAT
 static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
@@ -205,7 +205,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat
 
         //-----calculate D-term
         if (axis == YAW) {
-            if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
+            if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
 
             axisPID[axis] = lrintf(PTerm + ITerm);
 
@@ -230,7 +230,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat
             delta *= (1.0f / getdT());
 
             // Filter delta
-            if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1(delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT());
+            if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
 
             DTerm = constrainf(luxDTermScale * delta * (float)pidProfile->D8[axis] * tpaFactor, -300.0f, 300.0f);
 
@@ -344,7 +344,7 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin
 
         //-----calculate D-term
         if (axis == YAW) {
-            if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
+            if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
 
             axisPID[axis] = PTerm + ITerm;
 
@@ -369,7 +369,7 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin
             delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5;
 
             // Filter delta
-            if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT());
+            if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], (float)delta, pidProfile->dterm_lpf_hz, getdT());
 
             DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
 

src/main/sensors/acceleration.c

@@ -47,7 +47,6 @@ uint32_t accTargetLooptime;
 static 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.
 
 extern uint16_t InflightcalibratingA;
-extern bool AccInflightCalibrationArmed;
 extern bool AccInflightCalibrationMeasurementDone;
 extern bool AccInflightCalibrationSavetoEEProm;
 extern bool AccInflightCalibrationActive;
@@ -55,7 +54,7 @@ extern bool AccInflightCalibrationActive;
 static flightDynamicsTrims_t *accelerationTrims;
 
 static float accLpfCutHz = 0;
-static biquad_t accFilterState[XYZ_AXIS_COUNT];
+static biquadFilter_t accFilter[XYZ_AXIS_COUNT];
 static bool accFilterInitialised = false;
 
 void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
@@ -87,9 +86,8 @@ void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims)
 void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
 {
     static int32_t a[3];
-    int axis;
 
-    for (axis = 0; axis < 3; axis++) {
+    for (int axis = 0; axis < 3; axis++) {
 
         // Reset a[axis] at start of calibration
         if (isOnFirstAccelerationCalibrationCycle())
@@ -179,14 +177,13 @@ void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims)
 
 void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
 {
-	int16_t accADCRaw[XYZ_AXIS_COUNT];
+    int16_t accADCRaw[XYZ_AXIS_COUNT];
-	int axis;
 
     if (!acc.read(accADCRaw)) {
         return;
     }
 
-    for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         if (debugMode == DEBUG_ACCELEROMETER) debug[axis] = accADCRaw[axis];
         accSmooth[axis] = accADCRaw[axis];
     }
@@ -194,13 +191,17 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
     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);
+                for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+                    biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime);
+                }
                 accFilterInitialised = true;
             }
         }
 
         if (accFilterInitialised) {
-            for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) accSmooth[axis] = lrintf(applyBiQuadFilter((float) accSmooth[axis], &accFilterState[axis]));
+            for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+                accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)accSmooth[axis]));
+            }
         }
     }
 

src/main/sensors/battery.c

@@ -63,20 +63,19 @@ uint16_t batteryAdcToVoltage(uint16_t src)
 
 static void updateBatteryVoltage(void)
 {
-    uint16_t vbatSample;
+    static biquadFilter_t vbatFilter;
-    static biquad_t vbatFilterState;
+    static bool vbatFilterIsInitialised;
-    static bool vbatFilterStateIsSet;
 
     // store the battery voltage with some other recent battery voltage readings
-    vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY);
+    uint16_t vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY);
 
     if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample;
 
-    if (!vbatFilterStateIsSet) {
+    if (!vbatFilterIsInitialised) {
-        BiQuadNewLpf(VBATT_LPF_FREQ, &vbatFilterState, 50000); //50HZ Update
+        biquadFilterInit(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update
-        vbatFilterStateIsSet = true;
+        vbatFilterIsInitialised = true;
     }
-    vbatSample = applyBiQuadFilter(vbatSample, &vbatFilterState);
+    vbatSample = biquadFilterApply(&vbatFilter, vbatSample);
     vbat = batteryAdcToVoltage(vbatSample);
 
     if (debugMode == DEBUG_BATTERY) debug[1] = vbat;

src/main/sensors/gyro.c

@@ -43,7 +43,7 @@ float gyroADCf[XYZ_AXIS_COUNT];
 
 static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
 static const gyroConfig_t *gyroConfig;
-static biquad_t gyroFilterState[XYZ_AXIS_COUNT];
+static biquadFilter_t gyroFilter[XYZ_AXIS_COUNT];
 static uint8_t gyroSoftLpfHz;
 static uint16_t calibratingG = 0;
 
@@ -57,7 +57,7 @@ void gyroInit(void)
 {
     if (gyroSoftLpfHz && gyro.targetLooptime) {  // Initialisation needs to happen once samplingrate is known
         for (int axis = 0; axis < 3; axis++) {
-            BiQuadNewLpf(gyroSoftLpfHz, &gyroFilterState[axis], gyro.targetLooptime);
+            biquadFilterInit(&gyroFilter[axis], gyroSoftLpfHz, gyro.targetLooptime);
         }
     }
 }
@@ -157,7 +157,7 @@ void gyroUpdate(void)
 
     if (gyroSoftLpfHz) {
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            gyroADCf[axis] = applyBiQuadFilter((float)gyroADC[axis], &gyroFilterState[axis]);
+            gyroADCf[axis] = biquadFilterApply(&gyroFilter[axis], (float)gyroADC[axis]);
             gyroADC[axis] = lrintf(gyroADCf[axis]);
         }
     } else {