diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 1db323062a..020670b03c 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -237,3 +237,15 @@ int16_t firFilterInt16Get(const firFilter_t *filter, int index) return filter->buf[index]; } +/* prototype function for denoising of signal by dynamic moving average. Mainly for test purposes */ +float denoisingFilterUpdate(float input, uint8_t count, float filter[MAX_DENOISE_WINDOW_SIZE]) { + int index; + float averageSum = 0.0f; + + for (index = count-1; index > 0; index--) filter[index] = filter[index-1]; + filter[0] = input; + for (count = 0; count < count; index++) averageSum += filter[index]; + + return averageSum / count; +} + diff --git a/src/main/common/filter.h b/src/main/common/filter.h index c63f1cad19..e5d11257f0 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -15,7 +15,7 @@ * along with Cleanflight. If not, see . */ -#define DELTA_MAX_SAMPLES 12 +#define MAX_DENOISE_WINDOW_SIZE 6 typedef struct pt1Filter_s { float state; @@ -32,7 +32,7 @@ typedef struct biquadFilter_s { typedef enum { FILTER_PT1 = 0, FILTER_BIQUAD, - FILTER_FIR, + FILTER_DENOISE } filterType_e; typedef enum { @@ -81,4 +81,5 @@ float firFilterInt16CalcPartialAverage(const firFilterInt16_t *filter, uint8_t c float firFilterInt16CalcAverage(const firFilterInt16_t *filter); int16_t firFilterInt16LastInput(const firFilterInt16_t *filter); int16_t firFilterInt16Get(const firFilter_t *filter, int index); +float denoisingFilterUpdate(float input, uint8_t count, float filter[MAX_DENOISE_WINDOW_SIZE]); diff --git a/src/main/config/config.c b/src/main/config/config.c index f272dc2980..fae35a7075 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -181,7 +181,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; // Betaflight PID controller parameters - pidProfile->setpointRelaxRatio = 70; + pidProfile->setpointRelaxRatio = 85; pidProfile->dtermSetpointWeight = 200; pidProfile->yawRateAccelLimit = 220; pidProfile->rateAccelLimit = 0; @@ -432,7 +432,7 @@ void createDefaultConfig(master_t *config) config->pid_process_denom = 2; #endif config->gyro_soft_type = FILTER_PT1; - config->gyro_soft_lpf_hz = 80; + config->gyro_soft_lpf_hz = 90; config->gyro_soft_notch_hz_1 = 400; config->gyro_soft_notch_cutoff_1 = 300; config->gyro_soft_notch_hz_2 = 0; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 44b6751faf..2b3c5683d2 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -103,9 +103,10 @@ biquadFilter_t dtermFilterLpf[3]; biquadFilter_t dtermFilterNotch[3]; bool dtermNotchInitialised; bool dtermBiquadLpfInitialised; +float dtermFilterDenoise[XYZ_AXIS_COUNT][MAX_DENOISE_WINDOW_SIZE]; +bool dtermNotchInitialised, dtermLpfInitialised; -void initFilters(const pidProfile_t *pidProfile) -{ +void initFilters(const pidProfile_t *pidProfile) { int axis; if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) { @@ -115,9 +116,9 @@ void initFilters(const pidProfile_t *pidProfile) } if (pidProfile->dterm_filter_type == FILTER_BIQUAD) { - if (pidProfile->dterm_lpf_hz && !dtermBiquadLpfInitialised) { + if (pidProfile->dterm_lpf_hz && !dtermLpfInitialised) { for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); - dtermBiquadLpfInitialised = true; + dtermLpfInitialised = true; } } } diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c index 705649f0bc..436f3431c3 100644 --- a/src/main/flight/pid_betaflight.c +++ b/src/main/flight/pid_betaflight.c @@ -60,7 +60,7 @@ extern pt1Filter_t yawFilter; extern biquadFilter_t dtermFilterLpf[3]; extern biquadFilter_t dtermFilterNotch[3]; extern bool dtermNotchInitialised; -extern bool dtermBiquadLpfInitialised; +extern float dtermFilterDenoise[XYZ_AXIS_COUNT][MAX_DENOISE_WINDOW_SIZE]; void initFilters(const pidProfile_t *pidProfile); float getdT(void); @@ -209,11 +209,12 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); if (pidProfile->dterm_lpf_hz) { - if (dtermBiquadLpfInitialised) { + if (pidProfile->dterm_filter_type == FILTER_BIQUAD) delta = biquadFilterApply(&dtermFilterLpf[axis], delta); - } else { + else if (pidProfile->dterm_filter_type == FILTER_PT1) delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); - } + else + delta = denoisingFilterUpdate(delta, 3, dtermFilterDenoise[axis]); } DTerm = Kd[axis] * delta * tpaFactor; diff --git a/src/main/flight/pid_legacy.c b/src/main/flight/pid_legacy.c index 24c3aac56d..75dc0461e9 100644 --- a/src/main/flight/pid_legacy.c +++ b/src/main/flight/pid_legacy.c @@ -57,8 +57,7 @@ extern int32_t errorGyroI[3]; extern pt1Filter_t deltaFilter[3]; extern pt1Filter_t yawFilter; extern biquadFilter_t dtermFilterLpf[3]; -extern bool dtermBiquadLpfInitialised; - +extern float dtermFilterDenoise[XYZ_AXIS_COUNT][MAX_DENOISE_WINDOW_SIZE]; void initFilters(const pidProfile_t *pidProfile); float getdT(void); @@ -179,11 +178,13 @@ void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, c // Filter delta if (pidProfile->dterm_lpf_hz) { float deltaf = delta; // single conversion - if (dtermBiquadLpfInitialised) { + if (pidProfile->dterm_filter_type == FILTER_BIQUAD) delta = biquadFilterApply(&dtermFilterLpf[axis], delta); - } else { + else if (pidProfile->dterm_filter_type == FILTER_PT1) delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); - } + else + delta = denoisingFilterUpdate(delta, 3, dtermFilterDenoise[axis]); + delta = lrintf(deltaf); } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 36c12b0ad7..437c80b103 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -49,6 +49,7 @@ static const gyroConfig_t *gyroConfig; static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT]; static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT], gyroFilterNotch_2[XYZ_AXIS_COUNT]; static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT]; +static float gyroFilterDenoise[XYZ_AXIS_COUNT][MAX_DENOISE_WINDOW_SIZE]; static uint8_t gyroSoftLpfType; static uint16_t gyroSoftNotchHz_1, gyroSoftNotchHz_2; static float gyroSoftNotchQ_1, gyroSoftNotchQ_2; @@ -77,14 +78,19 @@ void gyroInit(void) { if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known for (int axis = 0; axis < 3; axis++) { - biquadFilterInit(&gyroFilterNotch_1[axis], gyroSoftNotchHz_1, gyro.targetLooptime, gyroSoftNotchQ_1, FILTER_NOTCH); - biquadFilterInit(&gyroFilterNotch_2[axis], gyroSoftNotchHz_2, gyro.targetLooptime, gyroSoftNotchQ_2, FILTER_NOTCH); if (gyroSoftLpfType == FILTER_BIQUAD) biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime); else gyroDt = (float) gyro.targetLooptime * 0.000001f; } } + + if ((gyroSoftNotchHz_1 || gyroSoftNotchHz_2) && gyro.targetLooptime) { + for (int axis = 0; axis < 3; axis++) { + biquadFilterInit(&gyroFilterNotch_1[axis], gyroSoftNotchHz_1, gyro.targetLooptime, gyroSoftNotchQ_1, FILTER_NOTCH); + biquadFilterInit(&gyroFilterNotch_2[axis], gyroSoftNotchHz_2, gyro.targetLooptime, gyroSoftNotchQ_2, FILTER_NOTCH); + } + } } bool isGyroCalibrationComplete(void) @@ -187,8 +193,10 @@ void gyroUpdate(void) if (gyroSoftLpfType == FILTER_BIQUAD) gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], (float) gyroADC[axis]); - else + else if (gyroSoftLpfType == FILTER_BIQUAD) gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], (float) gyroADC[axis], gyroSoftLpfHz, gyroDt); + else + gyroADCf[axis] = denoisingFilterUpdate((float) gyroADC[axis], 3, gyroFilterDenoise[axis]); if (debugMode == DEBUG_NOTCH) debug[axis] = lrintf(gyroADCf[axis]);