From 505502e7982e5d94b777044380b1a2a8c42f8489 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 9 Oct 2016 00:45:02 +0200 Subject: [PATCH] Cleanup // Fix some filter inits // More space Naze target --- src/main/common/filter.c | 18 ++++++++++-------- src/main/common/filter.h | 5 ++++- src/main/flight/pid.c | 14 ++++++-------- src/main/io/serial_cli.c | 10 +++++----- src/main/sensors/gyro.c | 26 ++++++++++++++------------ src/main/target/NAZE/target.h | 16 ++++++++-------- 6 files changed, 47 insertions(+), 42 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 8fb4ff319..36786199b 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -121,13 +121,15 @@ void initDenoisingFilter(denoisingState_t *filter, uint8_t gyroSoftLpfHz, uint16 /* prototype function for denoising of signal by dynamic moving average. Mainly for test purposes */ float denoisingFilterUpdate(denoisingState_t *filter, float input) { - int index; - float averageSum = 0.0f; - - for (index = filter->targetCount-1; index > 0; index--) filter->state[index] = filter->state[index-1]; - filter->state[0] = input; - for (int count = 0; count < filter->targetCount; index++) averageSum += filter->state[index]; - - return averageSum / filter->targetCount; + 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; } diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 5a4c9c9f8..4696edb32 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -15,7 +15,7 @@ * along with Cleanflight. If not, see . */ -#define MAX_DENOISE_WINDOW_SIZE 40 +#define MAX_DENOISE_WINDOW_SIZE 120 typedef struct pt1Filter_s { float state; @@ -30,7 +30,10 @@ typedef struct biquadFilter_s { } biquadFilter_t; typedef struct dennoisingState_s { + int filledCount; int targetCount; + int index; + float movingSum; float state[MAX_DENOISE_WINDOW_SIZE]; } denoisingState_t; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 8978be43f..eca6531a0 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -109,10 +109,11 @@ static pt1Filter_t yawFilter; static biquadFilter_t dtermFilterLpf[3]; static biquadFilter_t dtermFilterNotch[3]; static denoisingState_t dtermDenoisingState[3]; -static bool dtermNotchInitialised, dtermLpfInitialised; +static bool dtermNotchInitialised; void initFilters(const pidProfile_t *pidProfile) { int axis; + static uint8_t lowpassFilterType; if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) { float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff); @@ -120,18 +121,15 @@ void initFilters(const pidProfile_t *pidProfile) { dtermNotchInitialised = true; } - if (pidProfile->dterm_filter_type == FILTER_BIQUAD) { - if (pidProfile->dterm_lpf_hz && !dtermLpfInitialised) { + if ((pidProfile->dterm_filter_type != lowpassFilterType) && pidProfile->dterm_lpf_hz) { + if (pidProfile->dterm_filter_type == FILTER_BIQUAD) { for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); - dtermLpfInitialised = true; } - } - if (pidProfile->dterm_filter_type == FILTER_DENOISE) { - if (pidProfile->dterm_lpf_hz && !dtermLpfInitialised) { + if (pidProfile->dterm_filter_type == FILTER_DENOISE) { for (axis = 0; axis < 3; axis++) initDenoisingFilter(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); - dtermLpfInitialised = true; } + lowpassFilterType = pidProfile->dterm_filter_type; } } diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index b14e18223..05a500f8e 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -771,12 +771,12 @@ const clivalue_t valueTable[] = { { "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } }, { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } }, - { "gyro_lowpass_level", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_soft_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, + { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_soft_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } }, - { "gyro_notch_hz_1", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz_1, .config.minmax = { 0, 1000 } }, - { "gyro_notch_cutoff_1", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_cutoff_1, .config.minmax = { 1, 1000 } }, - { "gyro_notch_hz_2", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz_2, .config.minmax = { 0, 1000 } }, - { "gyro_notch_cutoff_2", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_cutoff_2, .config.minmax = { 1, 1000 } }, + { "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz_1, .config.minmax = { 0, 1000 } }, + { "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_cutoff_1, .config.minmax = { 1, 1000 } }, + { "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz_2, .config.minmax = { 0, 1000 } }, + { "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_cutoff_2, .config.minmax = { 1, 1000 } }, { "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } }, { "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } }, { "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } }, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 37c298658..3dd8f59c2 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -50,8 +50,8 @@ static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT], gyroFilterNotch_2[XYZ_A static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT]; static denoisingState_t gyroDenoiseState[XYZ_AXIS_COUNT]; static uint8_t gyroSoftLpfType; -static uint16_t gyroSoftNotchHz_1, gyroSoftNotchHz_2; -static float gyroSoftNotchQ_1, gyroSoftNotchQ_2; +static uint16_t gyroSoftNotchHz1, gyroSoftNotchHz2; +static float gyroSoftNotchQ1, gyroSoftNotchQ2; static uint8_t gyroSoftLpfHz; static uint16_t calibratingG = 0; static float gyroDt; @@ -66,11 +66,11 @@ void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, { gyroConfig = gyroConfigToUse; gyroSoftLpfHz = gyro_soft_lpf_hz; - gyroSoftNotchHz_1 = gyro_soft_notch_hz_1; - gyroSoftNotchHz_2 = gyro_soft_notch_hz_2; + gyroSoftNotchHz1 = gyro_soft_notch_hz_1; + gyroSoftNotchHz2 = gyro_soft_notch_hz_2; gyroSoftLpfType = gyro_soft_lpf_type; - gyroSoftNotchQ_1 = filterGetNotchQ(gyro_soft_notch_hz_1, gyro_soft_notch_cutoff_1); - gyroSoftNotchQ_2 = filterGetNotchQ(gyro_soft_notch_hz_2, gyro_soft_notch_cutoff_2); + gyroSoftNotchQ1 = filterGetNotchQ(gyro_soft_notch_hz_1, gyro_soft_notch_cutoff_1); + gyroSoftNotchQ2 = filterGetNotchQ(gyro_soft_notch_hz_2, gyro_soft_notch_cutoff_2); } void gyroInit(void) @@ -79,15 +79,17 @@ void gyroInit(void) for (int axis = 0; axis < 3; axis++) { if (gyroSoftLpfType == FILTER_BIQUAD) biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime); - else + else if (gyroSoftLpfType == FILTER_PT1) gyroDt = (float) gyro.targetLooptime * 0.000001f; + else + initDenoisingFilter(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime); } } - if ((gyroSoftNotchHz_1 || gyroSoftNotchHz_2) && gyro.targetLooptime) { + if ((gyroSoftNotchHz1 || gyroSoftNotchHz2) && 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); + biquadFilterInit(&gyroFilterNotch_1[axis], gyroSoftNotchHz1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH); + biquadFilterInit(&gyroFilterNotch_2[axis], gyroSoftNotchHz2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH); } } } @@ -200,10 +202,10 @@ void gyroUpdate(void) if (debugMode == DEBUG_NOTCH) debug[axis] = lrintf(gyroADCf[axis]); - if (gyroSoftNotchHz_1) + if (gyroSoftNotchHz1) gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_1[axis], gyroADCf[axis]); - if (gyroSoftNotchHz_2) + if (gyroSoftNotchHz2) gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_2[axis], gyroADCf[axis]); gyroADC[axis] = lrintf(gyroADCf[axis]); diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 55aa44635..f222b5b6b 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -95,14 +95,14 @@ #define ACC_BMA280_ALIGN CW0_DEG #define ACC_MPU6500_ALIGN CW0_DEG -#define BARO -#define USE_BARO_MS5611 -#define USE_BARO_BMP085 -#define USE_BARO_BMP280 - -#define MAG -#define USE_MAG_HMC5883 -#define MAG_HMC5883_ALIGN CW180_DEG +//#define BARO +//#define USE_BARO_MS5611 +//#define USE_BARO_BMP085 +//#define USE_BARO_BMP280 + +//#define MAG +//#define USE_MAG_HMC5883 +//#define MAG_HMC5883_ALIGN CW180_DEG //#define SONAR //#define SONAR_TRIGGER_PIN PB0 -- 2.11.4.GIT