Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-12 19:10:27 +03:00
Code Issues Wiki Activity

Experimental implementation of elliptic filter for gyro sensor on antialiasing phase

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2025-02-18 15:48:15 +01:00
parent 0e2f06d145
commit 43ca2ceed5
3 changed files with 31 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

13
src/main/common/filter.c
View file

@ -350,3 +350,16 @@ void assignFilterApplyFn(uint8_t filterType, float cutoffFrequency, filterApplyF
}
}
}
double ellipticFilterApply(const uint8_t filterOrder, double input, const double *a_coeffs, const double *b_coeffs, double *filterState) {
// Calculate the output
double output = b_coeffs[0] * input + filterState[0];
// Update the state variables
for (int i = 0; i < filterOrder - 1; i++) {
filterState[i] = b_coeffs[i + 1] * input - a_coeffs[i + 1] * output + filterState[i + 1];
}
filterState[filterOrder - 1] = b_coeffs[filterOrder] * input - a_coeffs[filterOrder] * output;
return output;
}

1
src/main/common/filter.h
View file

@ -139,3 +139,4 @@ float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input);
void initFilter(uint8_t filterType, filter_t *filter, float cutoffFrequency, uint32_t refreshRate);
void assignFilterApplyFn(uint8_t filterType, float cutoffFrequency, filterApplyFnPtr *applyFn);
double ellipticFilterApply(const uint8_t filterOrder, double input, const double *a_coeffs, const double *b_coeffs, double *filterState);

18
src/main/sensors/gyro.c
View file

@ -91,6 +91,19 @@ STATIC_FASTRAM filter_t gyroLpf2State[XYZ_AXIS_COUNT];
STATIC_FASTRAM filterApplyFnPtr gyroLuluApplyFn;
STATIC_FASTRAM filter_t gyroLuluState[XYZ_AXIS_COUNT];
//Elliptic filter coefficients for antialiasing
#define ELLIPTIC_ANTIALIASING_FILTER_ORDER 6
STATIC_FASTRAM double ellipticAntialiasingCoeffA[] = {
1.0, -5.13215, 10.9852, -12.2934, 7.51828, -2.49295, 0.3785
};
STATIC_FASTRAM double ellipticAntialiasingCoeffB[] = {
0.005897, -0.023357, 0.038222, -0.028058, 0.038222, -0.023357, 0.005897
};
STATIC_FASTRAM double ellipticAntialiasingFilterState[ELLIPTIC_ANTIALIASING_FILTER_ORDER] = {0};
#ifdef USE_DYNAMIC_FILTERS
EXTENDED_FASTRAM gyroAnalyseState_t gyroAnalyseState;
@ -562,7 +575,10 @@ void FAST_CODE NOINLINE gyroUpdate(void)
/*
* First gyro LPF is the only filter applied with the full gyro sampling speed
*/
gyroADCf = gyroLpfApplyFn((filter_t *) &gyroLpfState[axis], gyroADCf);
// gyroADCf = gyroLpfApplyFn((filter_t *) &gyroLpfState[axis], gyroADCf);
// Apply elliptic antialiasing filter
gyroADCf = ellipticFilterApply(ELLIPTIC_ANTIALIASING_FILTER_ORDER, gyroADCf, ellipticAntialiasingCoeffA, ellipticAntialiasingCoeffB, ellipticAntialiasingFilterState);
gyro.gyroADCf[axis] = gyroADCf;
}