diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 5fd72965d8..e556482914 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -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; +} diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 9be86b495c..032d5c27cb 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -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); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 9fa0a7c0e4..9d08c12712 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -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; }