From fc480fab5b726523c20ed2dc4a219025b88ba845 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 7 Oct 2016 02:24:22 +0200 Subject: [PATCH] Default adjustments // Cleanups --- src/main/common/filter.c | 24 +++++++----------------- src/main/common/filter.h | 6 +++--- src/main/config/config.c | 4 ++-- src/main/flight/pid.c | 22 +++++++++++++--------- src/main/sensors/gyro.c | 14 +++++++++++--- 5 files changed, 36 insertions(+), 34 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 0d40d95999..75a1e324d8 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -115,25 +115,15 @@ float biquadFilterApply(biquadFilter_t *filter, float input) return result; } -int32_t filterApplyAverage(int32_t input, uint8_t averageCount, int32_t averageState[DELTA_MAX_SAMPLES]) { - int count; - int32_t averageSum = 0; - - for (count = averageCount-1; count > 0; count--) averageState[count] = averageState[count-1]; - averageState[0] = input; - for (count = 0; count < averageCount; count++) averageSum += averageState[count]; - - return averageSum / averageCount; -} - -float filterApplyAveragef(float input, uint8_t averageCount, float averageState[DELTA_MAX_SAMPLES]) { - int count; +/* 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 (count = averageCount-1; count > 0; count--) averageState[count] = averageState[count-1]; - averageState[0] = input; - for (count = 0; count < averageCount; count++) averageSum += averageState[count]; + 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 / averageCount; + return averageSum / count; } diff --git a/src/main/common/filter.h b/src/main/common/filter.h index c584a23c86..06fb64a318 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 40 typedef struct pt1Filter_s { float state; @@ -32,6 +32,7 @@ typedef struct biquadFilter_s { typedef enum { FILTER_PT1 = 0, FILTER_BIQUAD, + FILTER_DENOISE } filterType_e; typedef enum { @@ -48,6 +49,5 @@ 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); -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]); +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 ef6ac66bd4..7a8e9a7a51 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -247,7 +247,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; @@ -487,7 +487,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 dec9c8b572..92fb0a8ff1 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -108,7 +108,8 @@ static pt1Filter_t deltaFilter[3]; static pt1Filter_t yawFilter; static biquadFilter_t dtermFilterLpf[3]; static biquadFilter_t dtermFilterNotch[3]; -static bool dtermNotchInitialised, dtermBiquadLpfInitialised; +static float dtermFilterDenoise[XYZ_AXIS_COUNT][MAX_DENOISE_WINDOW_SIZE]; +static bool dtermNotchInitialised, dtermLpfInitialised; void initFilters(const pidProfile_t *pidProfile) { int axis; @@ -120,9 +121,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; } } } @@ -271,11 +272,12 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc 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; @@ -410,11 +412,13 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina // 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 28cdfb1640..60aeba4e89 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -48,6 +48,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; @@ -76,14 +77,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) @@ -186,8 +192,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]);