diff --git a/src/main/common/filter.c b/src/main/common/filter.c index b68d04dc10..ff14d70597 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -31,7 +31,6 @@ #define M_LN2_FLOAT 0.69314718055994530942f #define M_PI_FLOAT 3.14159265358979323846f -#define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */ #define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - 2nd order butterworth*/ // NULL filter @@ -53,6 +52,7 @@ float pt1FilterGain(uint16_t f_cut, float dT) void pt1FilterInit(pt1Filter_t *filter, float k) { + filter->state = 0.0f; filter->k = k; } @@ -99,51 +99,6 @@ void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refr biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF); } -// Q coefficients for Butterworth filter of given order. Implicit odd section coefficient of 0.5 -// coefficients should be in ascending order -// generated by http://www.earlevel.com/main/2016/09/29/cascading-filters/ -static const float butterworthLpfQ[] = { -#if BIQUAD_LPF_ORDER_MAX >= 2 - 0.70710678, // 2nd -#endif -#if BIQUAD_LPF_ORDER_MAX >= 3 - 1.0, // 3rd -#endif -#if BIQUAD_LPF_ORDER_MAX >= 4 - 0.54119610, 1.3065630, // 4th -#endif -#if BIQUAD_LPF_ORDER_MAX >= 5 - 0.61803399, 1.6180340, // 5th -#endif -#if BIQUAD_LPF_ORDER_MAX >= 6 - 0.51763809, 0.70710678, 1.9318517 // 6th -#endif -}; -#define BUTTERWORTH_QINDEX(o) (((o) - 1) * ((o) - 1) / 4) - -// make sure that we have correct number of coefficients -STATIC_ASSERT(BUTTERWORTH_QINDEX(BIQUAD_LPF_ORDER_MAX + 1) == ARRAYLEN(butterworthLpfQ), butterworthLpfQ_mismatch); - -// setup cascade of biquad sections for Butterworth LPF filter -// sections is pointer to array of biquad sections, it must be long enough -// return number of sections used -int biquadFilterLpfCascadeInit(biquadFilter_t *sections, int order, float filterFreq, uint32_t refreshRate) -{ - biquadFilter_t *section = sections; - // single-pole section first - if (order % 2 == 1) { - biquadFilterInit(section, filterFreq, refreshRate, 0.5, FILTER_LPF1); - section++; - } - const float *Qptr = butterworthLpfQ + BUTTERWORTH_QINDEX(order); // first coefficient for given order - // 2 poles per section - for (int i = order; i >= 2; i -= 2) { - biquadFilterInit(section, filterFreq, refreshRate, *Qptr, FILTER_LPF); - Qptr++; section++; - } - return section - sections; -} - void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType) { // setup variables @@ -165,18 +120,6 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh a1 = -2 * cs; a2 = 1 - alpha; break; - case FILTER_LPF1: { - // 1st order Butterworth, H(s) = 1 / (1 + s), - // transformed according to http://www.iowahills.com/A4IIRBilinearTransform.html - const float T = 2.0f * sn / (cs + 1.0f); // T = 2 * tan(omega / 2) - b0 = T; - b1 = T; - b2 = 0; - a0 = T + 2.0f; - a1 = T - 2.0f; - a2 = 0; - break; - } case FILTER_NOTCH: b0 = 1; b1 = -2 * cs; @@ -249,180 +192,3 @@ FAST_CODE float biquadFilterApply(biquadFilter_t *filter, float input) filter->x2 = filter->b2 * input - filter->a2 * result; return result; } - -FAST_CODE float biquadCascadeFilterApply(biquadFilterCascade_t *filter, float input) -{ - for (int i = 0; i < filter->sections; i++ ) { - input = biquadFilterApply(&filter->biquad[i], input); - } - return input; -} - -/* - * FIR filter - */ -void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength) -{ - filter->buf = buf; - filter->bufLength = bufLength; - filter->coeffs = coeffs; - filter->coeffsLength = coeffsLength; - filter->movingSum = 0.0f; - filter->index = 0; - filter->count = 0; - memset(filter->buf, 0, sizeof(float) * filter->bufLength); -} - -/* - * FIR filter initialisation - * If the FIR filter is just to be used for averaging, then coeffs can be set to NULL - */ -void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs) -{ - firFilterInit2(filter, buf, bufLength, coeffs, bufLength); -} - -void firFilterUpdate(firFilter_t *filter, float input) -{ - filter->buf[filter->index++] = input; // index is at the first empty buffer positon - if (filter->index >= filter->bufLength) { - filter->index = 0; - } -} - -/* - * Update FIR filter maintaining a moving sum for quick moving average computation - */ -void firFilterUpdateAverage(firFilter_t *filter, float input) -{ - filter->movingSum += input; // sum of the last items, to allow quick moving average computation - filter->movingSum -= filter->buf[filter->index]; // subtract the value that "drops off" the end of the moving sum - filter->buf[filter->index++] = input; // index is at the first empty buffer positon - if (filter->index >= filter->bufLength) { - filter->index = 0; - } - if (filter->count < filter->bufLength) { - ++filter->count; - } -} - -FAST_CODE float firFilterApply(const firFilter_t *filter) -{ - float ret = 0.0f; - int ii = 0; - int index; - for (index = filter->index - 1; index >= 0; ++ii, --index) { - ret += filter->coeffs[ii] * filter->buf[index]; - } - for (index = filter->bufLength - 1; ii < filter->coeffsLength; ++ii, --index) { - ret += filter->coeffs[ii] * filter->buf[index]; - } - return ret; -} - -FAST_CODE float firFilterUpdateAndApply(firFilter_t *filter, float input) -{ - firFilterUpdate(filter, input); - return firFilterApply(filter); -} - -/* - * Returns average of the last items. - */ -float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count) -{ - float ret = 0.0f; - int index = filter->index; - for (int ii = 0; ii < filter->coeffsLength; ++ii) { - --index; - if (index < 0) { - index = filter->bufLength - 1; - } - ret += filter->buf[index]; - } - return ret / count; -} - -float firFilterCalcMovingAverage(const firFilter_t *filter) -{ - return filter->movingSum / filter->count; -} - -float firFilterLastInput(const firFilter_t *filter) -{ - // filter->index points to next empty item in buffer - const int index = filter->index == 0 ? filter->bufLength - 1 : filter->index - 1; - return filter->buf[index]; -} - -#if defined(USE_FIR_FILTER_DENOISE) -void firFilterDenoiseInit(firFilterDenoise_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime) -{ - memset(filter, 0, sizeof(firFilterDenoise_t)); - filter->targetCount = constrain(lrintf((1.0f / (0.000001f * (float)targetLooptime)) / gyroSoftLpfHz), 1, MAX_FIR_DENOISE_WINDOW_SIZE); -} - -// prototype function for denoising of signal by dynamic moving average. Mainly for test purposes -float firFilterDenoiseUpdate(firFilterDenoise_t *filter, float input) -{ - filter->state[filter->index] = input; - filter->movingSum += filter->state[filter->index++]; - if (filter->index == filter->targetCount) { - filter->index = 0; - } - filter->movingSum -= filter->state[filter->index]; - - if (filter->targetCount >= filter->filledCount) { - return filter->movingSum / filter->targetCount; - } else { - return filter->movingSum / ++filter->filledCount + 1; - } -} -#endif - -// ledvinap's proposed RC+FIR2 Biquad-- 1st order IIR, RC filter k -void biquadRCFIR2FilterInit(biquadFilter_t *filter, float k) -{ - filter->b0 = k / 2; - filter->b1 = k / 2; - filter->b2 = 0; - filter->a1 = -(1 - k); - filter->a2 = 0; -} - -// rs2k's fast "kalman" filter -void fastKalmanInit(fastKalman_t *filter, float k) -{ - filter->x = 0.0f; // set initial value, can be zero if unknown - filter->lastX = 0.0f; // set initial value, can be zero if unknown - filter->k = k; // "kalman" gain - half of RC coefficient, calculated externally -} - -FAST_CODE float fastKalmanUpdate(fastKalman_t *filter, float input) -{ - filter->x += (filter->x - filter->lastX); - filter->lastX = filter->x; - filter->x += filter->k * (input - filter->x); - return filter->x; -} - -void lmaSmoothingInit(laggedMovingAverage_t *filter, uint8_t windowSize, float weight) -{ - filter->movingWindowIndex = 0; - filter->windowSize = windowSize; - filter->weight = weight; -} - -FAST_CODE float lmaSmoothingUpdate(laggedMovingAverage_t *filter, float input) -{ - - filter->movingSum -= filter->buf[filter->movingWindowIndex]; - filter->buf[filter->movingWindowIndex] = input; - filter->movingSum += input; - - if (++filter->movingWindowIndex == filter->windowSize) { - filter->movingWindowIndex = 0; - } - - return input + (((filter->movingSum / filter->windowSize) - input) * filter->weight); -} diff --git a/src/main/common/filter.h b/src/main/common/filter.h index bd1389955f..db0cb115f0 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -20,16 +20,6 @@ #pragma once -// Don't use it on F1 and F3 to lower RAM usage -// FIR/Denoise filter can be cleaned up in the future as it is rarely used and used to be experimental -#if (defined(STM32F1) || defined(STM32F3)) -#define MAX_FIR_DENOISE_WINDOW_SIZE 1 -#else -#define MAX_FIR_DENOISE_WINDOW_SIZE 120 -#endif - -#define MAX_LMA_WINDOW_SIZE 12 - struct filter_s; typedef struct filter_s filter_t; @@ -50,103 +40,32 @@ typedef struct biquadFilter_s { float x1, x2, y1, y2; } biquadFilter_t; -#define BIQUAD_LPF_ORDER_MAX 6 - -typedef struct biquadFilterCascade_s { - int sections; - biquadFilter_t biquad[(BIQUAD_LPF_ORDER_MAX + 1) / 2]; // each section is of second order -} biquadFilterCascade_t; - -typedef struct firFilterDenoise_s { - int filledCount; - int targetCount; - int index; - float movingSum; - float state[MAX_FIR_DENOISE_WINDOW_SIZE]; -} firFilterDenoise_t; - -typedef struct fastKalman_s { - float k; // "kalman" gain - float x; // state - float lastX; // previous state -} fastKalman_t; - -typedef struct laggedMovingAverage_s { - uint16_t movingWindowIndex; - uint16_t windowSize; - float weight; - float movingSum; - float buf[MAX_LMA_WINDOW_SIZE]; -} laggedMovingAverage_t; - typedef enum { FILTER_PT1 = 0, FILTER_BIQUAD, - FILTER_FIR, - FILTER_BUTTERWORTH, - FILTER_BIQUAD_RC_FIR2, - FILTER_FAST_KALMAN } lowpassFilterType_e; typedef enum { FILTER_LPF, // 2nd order Butterworth section FILTER_NOTCH, FILTER_BPF, - FILTER_LPF1, // 1st order Butterworth section } biquadFilterType_e; -typedef struct firFilter_s { - float *buf; - const float *coeffs; - float movingSum; - uint8_t index; - uint8_t count; - uint8_t bufLength; - uint8_t coeffsLength; -} firFilter_t; - typedef float (*filterApplyFnPtr)(filter_t *filter, float input); float nullFilterApply(filter_t *filter, float input); - - void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate); void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType); void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType); -int biquadFilterLpfCascadeInit(biquadFilter_t *sections, int order, float filterFreq, uint32_t refreshRate); float biquadFilterApplyDF1(biquadFilter_t *filter, float input); float biquadFilterApply(biquadFilter_t *filter, float input); -float biquadCascadeFilterApply(biquadFilterCascade_t *filter, float input); float filterGetNotchQ(float centerFreq, float cutoffFreq); -void biquadRCFIR2FilterInit(biquadFilter_t *filter, float k); - -void fastKalmanInit(fastKalman_t *filter, float k); -float fastKalmanUpdate(fastKalman_t *filter, float input); - -void lmaSmoothingInit(laggedMovingAverage_t *filter, uint8_t windowSize, float weight); -float lmaSmoothingUpdate(laggedMovingAverage_t *filter, float input); - float pt1FilterGain(uint16_t f_cut, float dT); void pt1FilterInit(pt1Filter_t *filter, float k); float pt1FilterApply(pt1Filter_t *filter, float input); void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold); float slewFilterApply(slewFilter_t *filter, float input); - -void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs); -void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength); -void firFilterUpdate(firFilter_t *filter, float input); -void firFilterUpdateAverage(firFilter_t *filter, float input); -float firFilterApply(const firFilter_t *filter); -float firFilterUpdateAndApply(firFilter_t *filter, float input); -float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count); -float firFilterCalcMovingAverage(const firFilter_t *filter); -float firFilterLastInput(const firFilter_t *filter); - -#if defined(USE_FIR_FILTER_DENOISE) -void firFilterDenoiseInit(firFilterDenoise_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime); -float firFilterDenoiseUpdate(firFilterDenoise_t *filter, float input); -#endif diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 035f4e97e8..43e69b12f0 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -185,9 +185,6 @@ const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; typedef union dtermLowpass_u { pt1Filter_t pt1Filter; biquadFilter_t biquadFilter; -#if defined(USE_FIR_FILTER_DENOISE) - firFilterDenoise_t denoisingFilter; -#endif } dtermLowpass_t; static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermNotchApplyFn; @@ -263,14 +260,6 @@ void pidInitFilters(const pidProfile_t *pidProfile) biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, pidProfile->dterm_lowpass_hz, targetPidLooptime); } break; -#if defined(USE_FIR_FILTER_DENOISE) - case FILTER_FIR: - dtermLowpassApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate; - for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { - firFilterDenoiseInit(&dtermLowpass[axis].denoisingFilter, pidProfile->dterm_lowpass_hz, targetPidLooptime); - } - break; -#endif } } diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index f46dc13b25..284ada8fe9 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -265,22 +265,11 @@ static const char * const lookupTableRcInterpolationChannels[] = { static const char * const lookupTableLowpassType[] = { "PT1", "BIQUAD", -#if defined(USE_FIR_FILTER_DENOISE) - "FIR", -#else - NULL, -#endif - "BUTTERWORTH", - "BIQUAD_RC_FIR2", - "FAST_KALMAN" }; static const char * const lookupTableDtermLowpassType[] = { "PT1", "BIQUAD", -#if defined(USE_FIR_FILTER_DENOISE) - "FIR" -#endif }; @@ -436,20 +425,15 @@ const clivalue_t valueTable[] = { { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_type) }, { "gyro_lowpass_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_hz) }, - { "gyro_lowpass_order", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, GYRO_LPF_ORDER_MAX}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_order) }, { "gyro_lowpass2_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_type) }, { "gyro_lowpass2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_hz) }, - { "gyro_lowpass2_order", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, GYRO_LPF_ORDER_MAX}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_order) }, { "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_1) }, { "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_1) }, { "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_2) }, { "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_2) }, - { "gyro_lma_depth", VAR_UINT8 | MASTER_VALUE, .config.minmax = {0, 11}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lma_depth)}, - { "gyro_lma_weight", VAR_UINT8 | MASTER_VALUE, .config.minmax = {0, 100}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lma_weight)}, - { "gyro_calib_duration", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 50, 3000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroCalibrationDuration) }, { "gyro_calib_noise_limit", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroMovementCalibrationThreshold) }, { "gyro_offset_yaw", VAR_INT16 | MASTER_VALUE, .config.minmax = { -1000, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_offset_yaw) }, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index b16a64dcd1..53988afe1d 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -106,16 +106,9 @@ typedef struct gyroCalibration_s { bool firstArmingCalibrationWasStarted = false; -#if GYRO_LPF_ORDER_MAX > BIQUAD_LPF_ORDER_MAX -#error "GYRO_LPF_ORDER_MAX is larger than BIQUAD_LPF_ORDER_MAX" -#endif - typedef union gyroLowpassFilter_u { pt1Filter_t pt1FilterState; biquadFilter_t biquadFilterState; - biquadFilterCascade_t biquadFilterCascadeState; - firFilterDenoise_t denoiseFilterState; - fastKalman_t fastKalmanFilterState; } gyroLowpassFilter_t; typedef struct gyroSensor_s { @@ -130,15 +123,13 @@ typedef struct gyroSensor_s { filterApplyFnPtr lowpass2FilterApplyFn; gyroLowpassFilter_t lowpass2Filter[XYZ_AXIS_COUNT]; - // lagged moving average smoothing - filterApplyFnPtr lmaSmoothingApplyFn; - laggedMovingAverage_t lmaSmoothingFilter[XYZ_AXIS_COUNT]; - // notch filters filterApplyFnPtr notchFilter1ApplyFn; biquadFilter_t notchFilter1[XYZ_AXIS_COUNT]; + filterApplyFnPtr notchFilter2ApplyFn; biquadFilter_t notchFilter2[XYZ_AXIS_COUNT]; + filterApplyFnPtr notchFilterDynApplyFn; biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT]; @@ -149,7 +140,6 @@ typedef struct gyroSensor_s { timeUs_t yawSpinTimeUs; bool yawSpinDetected; #endif // USE_YAW_SPIN_RECOVERY - } gyroSensor_t; STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1; @@ -163,7 +153,7 @@ STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyroSensor1.gyroDev; #endif static void gyroInitSensorFilters(gyroSensor_t *gyroSensor); -static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz, int order); +static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz); #define DEBUG_GYRO_CALIBRATION 3 @@ -179,7 +169,7 @@ static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int typ #define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps for 2000dps gyro) #define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps for 2000dps gyro) -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 2); +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 3); #ifndef GYRO_CONFIG_USE_GYRO_DEFAULT #define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1 @@ -194,10 +184,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_32khz_hardware_lpf = GYRO_32KHZ_HARDWARE_LPF_NORMAL, .gyro_lowpass_type = FILTER_PT1, .gyro_lowpass_hz = 90, - .gyro_lowpass_order = 1, .gyro_lowpass2_type = FILTER_PT1, .gyro_lowpass2_hz = 0, - .gyro_lowpass2_order = 1, .gyro_high_fsr = false, .gyro_use_32khz = false, .gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT, @@ -207,8 +195,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_soft_notch_cutoff_2 = 100, .checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES, .gyro_offset_yaw = 0, - .gyro_lma_depth = 0, - .gyro_lma_weight = 100, .yaw_spin_recovery = true, .yaw_spin_threshold = 1950, ); @@ -638,7 +624,7 @@ bool gyroInit(void) return ret; } -void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz, int order) +void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz) { filterApplyFnPtr *lowpassFilterApplyFn; gyroLowpassFilter_t *lowpassFilter = NULL; @@ -684,51 +670,6 @@ void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, gyro.targetLooptime); } break; - case FILTER_BIQUAD_RC_FIR2: - *lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply; - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadRCFIR2FilterInit(&lowpassFilter[axis].biquadFilterState, gain); - } - break; - case FILTER_BUTTERWORTH: - *lowpassFilterApplyFn = (filterApplyFnPtr) biquadCascadeFilterApply; - if (order <= GYRO_LPF_ORDER_MAX) { - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - int *sections = &lowpassFilter[axis].biquadFilterCascadeState.sections; - biquadFilter_t *biquadSections = lowpassFilter[axis].biquadFilterCascadeState.biquad; - *sections = biquadFilterLpfCascadeInit(biquadSections, order, lpfHz, gyro.targetLooptime); - } - } - break; - case FILTER_FAST_KALMAN: - *lowpassFilterApplyFn = (filterApplyFnPtr) fastKalmanUpdate; - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - fastKalmanInit(&lowpassFilter[axis].fastKalmanFilterState, gain / 2); - } - break; -#if defined(USE_FIR_FILTER_DENOISE) - case FILTER_FIR: - *lowpassFilterApplyFn = (filterApplyFnPtr) firFilterDenoiseUpdate; - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - firFilterDenoiseInit(&lowpassFilter[axis].denoiseFilterState, lpfHz, gyro.targetLooptime); - } - break; - -#endif - } - } -} - -static void gyroInitLmaSmoothing(gyroSensor_t *gyroSensor, uint8_t depth, uint8_t weight) -{ - gyroSensor->lmaSmoothingApplyFn = nullFilterApply; - - if (depth && weight) { - const uint8_t windowSize = depth + 1; - const float lmaWeight = weight * 0.01f; - gyroSensor->lmaSmoothingApplyFn = (filterApplyFnPtr)lmaSmoothingUpdate; - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - lmaSmoothingInit(&gyroSensor->lmaSmoothingFilter[axis], windowSize, lmaWeight); } } } @@ -817,20 +758,16 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) gyroSensor, FILTER_LOWPASS, gyroConfig()->gyro_lowpass_type, - gyroConfig()->gyro_lowpass_hz, - gyroConfig()->gyro_lowpass_order + gyroConfig()->gyro_lowpass_hz ); gyroInitLowpassFilterLpf( gyroSensor, FILTER_LOWPASS2, gyroConfig()->gyro_lowpass2_type, - gyroConfig()->gyro_lowpass2_hz, - gyroConfig()->gyro_lowpass2_order + gyroConfig()->gyro_lowpass2_hz ); - gyroInitLmaSmoothing(gyroSensor, gyroConfig()->gyro_lma_depth, gyroConfig()->gyro_lma_weight); - gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2); #ifdef USE_GYRO_DATA_ANALYSE @@ -1118,7 +1055,6 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens float gyroADCf = gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale; gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf); - gyroADCf = gyroSensor->lmaSmoothingApplyFn((filter_t *)&gyroSensor->lmaSmoothingFilter[axis], gyroADCf); #ifdef USE_GYRO_DATA_ANALYSE gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf); @@ -1144,7 +1080,6 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens // apply lowpass2 filter gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf); - gyroADCf = gyroSensor->lmaSmoothingApplyFn((filter_t *)&gyroSensor->lmaSmoothingFilter[axis], gyroADCf); #ifdef USE_GYRO_DATA_ANALYSE // apply dynamic notch filter diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 877803d99b..f97dcaa60f 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -67,8 +67,6 @@ typedef enum { FILTER_LOWPASS2 } filterSlots; -#define GYRO_LPF_ORDER_MAX 6 - typedef struct gyroConfig_s { sensor_align_e gyro_align; // gyro alignment 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. @@ -80,18 +78,10 @@ typedef struct gyroConfig_s { bool gyro_use_32khz; uint8_t gyro_to_use; - // Lagged Moving Average smoother - uint8_t gyro_lma_depth; - uint8_t gyro_lma_weight; - // Lowpass primary/secondary uint8_t gyro_lowpass_type; uint8_t gyro_lowpass2_type; - // Order is used for the 'higher ordering' of cascaded butterworth/biquad sections - uint8_t gyro_lowpass_order; - uint8_t gyro_lowpass2_order; - uint16_t gyro_lowpass_hz; uint16_t gyro_lowpass2_hz; diff --git a/src/test/unit/common_filter_unittest.cc b/src/test/unit/common_filter_unittest.cc index bdba86134f..e8abf37965 100644 --- a/src/test/unit/common_filter_unittest.cc +++ b/src/test/unit/common_filter_unittest.cc @@ -29,121 +29,37 @@ extern "C" { #include "unittest_macros.h" #include "gtest/gtest.h" -TEST(FilterUnittest, TestFirFilterInit) +TEST(FilterUnittest, TestPt1FilterInit) { -#define BUFLEN 4 - float buf[BUFLEN]; - firFilter_t filter; + pt1Filter_t filter; + pt1FilterInit(&filter, 0.0f); + EXPECT_EQ(0, filter.k); - - firFilterInit(&filter, buf, BUFLEN, NULL); - - EXPECT_EQ(buf, filter.buf); - EXPECT_EQ(0, filter.coeffs); - EXPECT_EQ(0, filter.movingSum); - EXPECT_EQ(0, filter.index); - EXPECT_EQ(0, filter.count); - EXPECT_EQ(BUFLEN, filter.bufLength); - EXPECT_EQ(BUFLEN, filter.coeffsLength); + pt1FilterInit(&filter, 1.0f); + EXPECT_EQ(1.0, filter.k); } -TEST(FilterUnittest, TestFirFilterUpdateAverage) +TEST(FilterUnittest, TestPt1FilterGain) { -#define BUFLEN 4 - float buf[BUFLEN]; - const float coeffs[BUFLEN] = {1.0f, 1.0f, 1.0f, 1.0f}; - firFilter_t filter; - - firFilterInit(&filter, buf, BUFLEN, coeffs); - - firFilterUpdateAverage(&filter, 2.0f); - EXPECT_FLOAT_EQ(2.0f, filter.buf[0]); - EXPECT_FLOAT_EQ(2.0f, filter.movingSum); - EXPECT_EQ(1, filter.index); - EXPECT_EQ(1, filter.count); - EXPECT_EQ(2.0f, firFilterCalcMovingAverage(&filter)); - EXPECT_FLOAT_EQ(2.0f, firFilterCalcPartialAverage(&filter, 1)); - EXPECT_FLOAT_EQ(2.0f, firFilterApply(&filter)); - - firFilterUpdateAverage(&filter, 3.0f); - EXPECT_FLOAT_EQ(3.0f, filter.buf[1]); - EXPECT_FLOAT_EQ(5.0f, filter.movingSum); - EXPECT_EQ(2, filter.index); - EXPECT_EQ(2, filter.count); - EXPECT_EQ(2.5f, firFilterCalcMovingAverage(&filter)); - EXPECT_FLOAT_EQ(2.5f, firFilterCalcPartialAverage(&filter, 2)); - EXPECT_FLOAT_EQ(5.0f, firFilterApply(&filter)); - - firFilterUpdateAverage(&filter, 4.0f); - EXPECT_FLOAT_EQ(4.0f, filter.buf[2]); - EXPECT_FLOAT_EQ(9.0f, filter.movingSum); - EXPECT_EQ(3, filter.index); - EXPECT_EQ(3, filter.count); - EXPECT_EQ(3.0f, firFilterCalcMovingAverage(&filter)); - EXPECT_FLOAT_EQ(3.0f, firFilterCalcPartialAverage(&filter, 3)); - EXPECT_FLOAT_EQ(9.0f, firFilterApply(&filter)); - - firFilterUpdateAverage(&filter, 5.0f); - EXPECT_FLOAT_EQ(5.0f, filter.buf[3]); - EXPECT_FLOAT_EQ(14.0f, filter.movingSum); - EXPECT_EQ(0, filter.index); - EXPECT_EQ(4, filter.count); - EXPECT_EQ(3.5f, firFilterCalcMovingAverage(&filter)); - EXPECT_FLOAT_EQ(3.5f, firFilterCalcPartialAverage(&filter, 4)); - EXPECT_FLOAT_EQ(14.0f, firFilterApply(&filter)); - - firFilterUpdateAverage(&filter, 6.0f); - EXPECT_FLOAT_EQ(6.0f, filter.buf[0]); - EXPECT_FLOAT_EQ(18.0f, filter.movingSum); - EXPECT_EQ(1, filter.index); - EXPECT_EQ(BUFLEN, filter.count); - EXPECT_EQ(4.5f, firFilterCalcMovingAverage(&filter)); - EXPECT_FLOAT_EQ(4.5f, firFilterCalcPartialAverage(&filter, BUFLEN)); - EXPECT_FLOAT_EQ(18.0f, firFilterApply(&filter)); - - firFilterUpdateAverage(&filter, 7.0f); - EXPECT_FLOAT_EQ(7.0f, filter.buf[1]); - EXPECT_FLOAT_EQ(22.0f, filter.movingSum); - EXPECT_EQ(2, filter.index); - EXPECT_EQ(BUFLEN, filter.count); - EXPECT_EQ(5.5f, firFilterCalcMovingAverage(&filter)); - EXPECT_FLOAT_EQ(5.5f, firFilterCalcPartialAverage(&filter, BUFLEN)); - EXPECT_FLOAT_EQ(22.0f, firFilterApply(&filter)); + EXPECT_FLOAT_EQ(0.999949, pt1FilterGain(100, 31.25f)); + // handle cases over uint8_t boundary + EXPECT_FLOAT_EQ(0.99998301, pt1FilterGain(300, 31.25f)); } -TEST(FilterUnittest, TestFirFilterApply) +TEST(FilterUnittest, TestPt1FilterApply) { -#define BUFLEN 4 - float buf[BUFLEN]; - firFilter_t filter; - const float coeffs[BUFLEN] = {26.0f, 27.0f, 28.0f, 29.0f}; + pt1Filter_t filter; + pt1FilterInit(&filter, pt1FilterGain(100, 31.25f)); + EXPECT_EQ(0, filter.state); - float expected = 0.0f; - firFilterInit(&filter, buf, BUFLEN, coeffs); + pt1FilterApply(&filter, 1800.0f); + EXPECT_FLOAT_EQ(1799.9083, filter.state); - firFilterUpdate(&filter, 2.0f); - expected = 2.0f * 26.0f; - EXPECT_FLOAT_EQ(expected, firFilterApply(&filter)); + pt1FilterApply(&filter, -1800.0f); + EXPECT_FLOAT_EQ(-1799.8165, filter.state); - firFilterUpdate(&filter, 3.0f); - expected = 3.0f * 26.0f + 2.0 * 27.0; - EXPECT_FLOAT_EQ(expected, firFilterApply(&filter)); - - firFilterUpdate(&filter, 4.0f); - expected = 4.0f * 26.0f + 3.0 * 27.0 + 2.0 * 28.0; - EXPECT_FLOAT_EQ(expected, firFilterApply(&filter)); - - firFilterUpdate(&filter, 5.0f); - expected = 5.0f * 26.0f + 4.0 * 27.0 + 3.0 * 28.0 + 2.0f * 29.0f; - EXPECT_FLOAT_EQ(expected, firFilterApply(&filter)); - - firFilterUpdate(&filter, 6.0f); - expected = 6.0f * 26.0f + 5.0 * 27.0 + 4.0 * 28.0 + 3.0f * 29.0f; - EXPECT_FLOAT_EQ(expected, firFilterApply(&filter)); - - firFilterUpdate(&filter, 7.0f); - expected = 7.0f * 26.0f + 6.0 * 27.0 + 5.0 * 28.0 + 4.0f * 29.0f; - EXPECT_FLOAT_EQ(expected, firFilterApply(&filter)); + pt1FilterApply(&filter, -200.0f); + EXPECT_FLOAT_EQ(-200.08142, filter.state); } TEST(FilterUnittest, TestSlewFilterInit) diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index ff422b7eec..a88cdaf276 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -117,8 +117,6 @@ TEST(SensorGyro, Update) // turn off filters gyroConfigMutable()->gyro_lowpass_hz = 0; gyroConfigMutable()->gyro_lowpass2_hz = 0; - gyroConfigMutable()->gyro_lma_depth = 0; - gyroConfigMutable()->gyro_lma_weight = 0; gyroConfigMutable()->gyro_soft_notch_hz_1 = 0; gyroConfigMutable()->gyro_soft_notch_hz_2 = 0; gyroInit();