diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 2cef6fa84b..e291ae25c9 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -140,7 +140,7 @@ void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const flo firFilterInit2(filter, buf, bufLength, coeffs, bufLength); } -void firFilterUpdate(firFilter_t *filter, float input) +void filterFirUpdate(firFilter_t *filter, float input) { filter->buf[filter->index++] = input; // index is at the first empty buffer positon if (filter->index >= filter->bufLength) { @@ -269,12 +269,12 @@ int16_t firFilterInt16Get(const firFilter_t *filter, int index) return filter->buf[index]; } -void initDenoisingFilter(denoisingState_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime) { +void initFirFilter(firFilterState_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime) { filter->targetCount = constrain(lrintf((1.0f / (0.000001f * (float)targetLooptime)) / gyroSoftLpfHz), 1, MAX_DENOISE_WINDOW_SIZE); } /* prototype function for denoising of signal by dynamic moving average. Mainly for test purposes */ -float denoisingFilterUpdate(denoisingState_t *filter, float input) { +float firFilterUpdate(firFilterState_t *filter, float input) { filter->state[filter->index] = input; filter->movingSum += filter->state[filter->index++]; if (filter->index == filter->targetCount) @@ -287,3 +287,4 @@ float denoisingFilterUpdate(denoisingState_t *filter, float input) { return filter->movingSum / ++filter->filledCount + 1; } + diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 74afed4903..91057e3aa4 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -29,18 +29,18 @@ typedef struct biquadFilter_s { float d1, d2; } biquadFilter_t; -typedef struct dennoisingState_s { +typedef struct firFilterState_s { int filledCount; int targetCount; int index; float movingSum; float state[MAX_DENOISE_WINDOW_SIZE]; -} denoisingState_t; +} firFilterState_t; typedef enum { FILTER_PT1 = 0, FILTER_BIQUAD, - FILTER_DENOISE + FILTER_FIR } filterType_e; typedef enum { @@ -77,7 +77,7 @@ float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT) 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 filterFirUpdate(firFilter_t *filter, float input); void firFilterUpdateAverage(firFilter_t *filter, float input); float firFilterApply(const firFilter_t *filter); float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count); @@ -92,6 +92,6 @@ 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); -void initDenoisingFilter(denoisingState_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime); -float denoisingFilterUpdate(denoisingState_t *filter, float input); +void initFirFilter(firFilterState_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime); +float firFilterUpdate(firFilterState_t *filter, float input); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index c6d3570a94..5542182813 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -103,7 +103,7 @@ biquadFilter_t dtermFilterLpf[3]; biquadFilter_t dtermFilterNotch[3]; bool dtermNotchInitialised; bool dtermBiquadLpfInitialised; -denoisingState_t dtermDenoisingState[3]; +firFilterState_t dtermDenoisingState[3]; void initFilters(const pidProfile_t *pidProfile) { int axis; @@ -120,8 +120,8 @@ void initFilters(const pidProfile_t *pidProfile) { for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); } - if (pidProfile->dterm_filter_type == FILTER_DENOISE) { - for (axis = 0; axis < 3; axis++) initDenoisingFilter(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); + if (pidProfile->dterm_filter_type == FILTER_FIR) { + for (axis = 0; axis < 3; axis++) initFirFilter(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); } lowpassFilterType = pidProfile->dterm_filter_type; } diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c index f2cf711f55..aa428be0a5 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 denoisingState_t dtermDenoisingState[3]; +extern firFilterState_t dtermDenoisingState[3]; void initFilters(const pidProfile_t *pidProfile); float getdT(void); @@ -214,7 +214,7 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio else if (pidProfile->dterm_filter_type == FILTER_PT1) delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); else - delta = denoisingFilterUpdate(&dtermDenoisingState[axis], delta); + delta = firFilterUpdate(&dtermDenoisingState[axis], delta); } DTerm = Kd[axis] * delta * tpaFactor; diff --git a/src/main/flight/pid_legacy.c b/src/main/flight/pid_legacy.c index dc5d7d1a27..338249ae93 100644 --- a/src/main/flight/pid_legacy.c +++ b/src/main/flight/pid_legacy.c @@ -57,7 +57,7 @@ extern int32_t errorGyroI[3]; extern pt1Filter_t deltaFilter[3]; extern pt1Filter_t yawFilter; extern biquadFilter_t dtermFilterLpf[3]; -extern denoisingState_t dtermDenoisingState[3]; +extern firFilterState_t dtermDenoisingState[3]; void initFilters(const pidProfile_t *pidProfile); float getdT(void); @@ -183,7 +183,7 @@ void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, c else if (pidProfile->dterm_filter_type == FILTER_PT1) delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); else - delta = denoisingFilterUpdate(&dtermDenoisingState[axis], delta); + delta = firFilterUpdate(&dtermDenoisingState[axis], delta); delta = lrintf(deltaf); } diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 19de7132a9..272a6f193d 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -532,7 +532,7 @@ static const char * const lookupTableRcInterpolation[] = { }; static const char * const lookupTableLowpassType[] = { - "NORMAL", "HIGH", "DENOISE" + "PT1", "BIQUAD", "FIR" }; static const char * const lookupTableFailsafe[] = { diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 2baeaaa3ac..56b778e7a0 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -49,7 +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 denoisingState_t gyroDenoiseState[XYZ_AXIS_COUNT]; +static firFilterState_t gyroDenoiseState[XYZ_AXIS_COUNT]; static uint8_t gyroSoftLpfType; static uint16_t gyroSoftNotchHz1, gyroSoftNotchHz2; static float gyroSoftNotchQ1, gyroSoftNotchQ2; @@ -83,7 +83,7 @@ void gyroInit(void) else if (gyroSoftLpfType == FILTER_PT1) gyroDt = (float) gyro.targetLooptime * 0.000001f; else - initDenoisingFilter(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime); + initFirFilter(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime); } } @@ -198,7 +198,7 @@ void gyroUpdate(void) else if (gyroSoftLpfType == FILTER_PT1) gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], (float) gyroADC[axis], gyroSoftLpfHz, gyroDt); else - gyroADCf[axis] = denoisingFilterUpdate(&gyroDenoiseState[axis], (float) gyroADC[axis]); + gyroADCf[axis] = firFilterUpdate(&gyroDenoiseState[axis], (float) gyroADC[axis]); if (debugMode == DEBUG_NOTCH) debug[axis] = lrintf(gyroADCf[axis]); diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c index df29866454..c7e8a9c7ba 100755 --- a/src/main/target/NAZE/config.c +++ b/src/main/target/NAZE/config.c @@ -53,11 +53,11 @@ void targetConfiguration(master_t *config) for (int profileId = 0; profileId < 2; profileId++) { config->profile[profileId].pidProfile.P8[ROLL] = 55; - config->profile[profileId].pidProfile.I8[ROLL] = 40; - config->profile[profileId].pidProfile.D8[ROLL] = 20; - config->profile[profileId].pidProfile.P8[PITCH] = 55; - config->profile[profileId].pidProfile.I8[PITCH] = 40; - config->profile[profileId].pidProfile.D8[PITCH] = 20; + config->profile[profileId].pidProfile.I8[ROLL] = 50; + config->profile[profileId].pidProfile.D8[ROLL] = 25; + config->profile[profileId].pidProfile.P8[PITCH] = 65; + config->profile[profileId].pidProfile.I8[PITCH] = 60; + config->profile[profileId].pidProfile.D8[PITCH] = 28; config->profile[profileId].pidProfile.P8[YAW] = 180; config->profile[profileId].pidProfile.I8[YAW] = 45; config->profile[profileId].pidProfile.P8[PIDLEVEL] = 50; @@ -65,11 +65,11 @@ void targetConfiguration(master_t *config) config->profile[profileId].pidProfile.levelSensitivity = 1.0f; for (int rateProfileId = 0; rateProfileId < MAX_RATEPROFILES; rateProfileId++) { - config->profile[profileId].controlRateProfile[rateProfileId].rcRate8 = 120; - config->profile[profileId].controlRateProfile[rateProfileId].rcYawRate8 = 120; - config->profile[profileId].controlRateProfile[rateProfileId].rates[ROLL] = 99; - config->profile[profileId].controlRateProfile[rateProfileId].rates[PITCH] = 99; - config->profile[profileId].controlRateProfile[rateProfileId].rates[YAW] = 99; + config->profile[profileId].controlRateProfile[rateProfileId].rcRate8 = 110; + config->profile[profileId].controlRateProfile[rateProfileId].rcYawRate8 = 110; + config->profile[profileId].controlRateProfile[rateProfileId].rates[ROLL] = 80; + config->profile[profileId].controlRateProfile[rateProfileId].rates[PITCH] = 80; + config->profile[profileId].controlRateProfile[rateProfileId].rates[YAW] = 80; config->profile[profileId].pidProfile.setpointRelaxRatio = 100; } diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index e6e0dbf774..164376d6cd 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -163,6 +163,8 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM + // IO - assuming all IOs on 48pin package #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff