From 233ac7fb3fbae381d9c04e2c07b6262466b8f7f0 Mon Sep 17 00:00:00 2001 From: Kenneth Mitchell Date: Sat, 27 Oct 2018 09:16:55 -0400 Subject: [PATCH] Allow width=0 to select single notch. Restore dyn lpf mins. --- src/main/flight/pid.c | 7 ++--- src/main/flight/pid.h | 1 + src/main/interface/settings.c | 6 ++--- src/main/interface/settings.h | 1 - src/main/sensors/gyro.c | 12 ++++++--- src/main/sensors/gyro.h | 5 ++-- src/main/sensors/gyro_filter_impl.h | 2 +- src/main/sensors/gyroanalyse.c | 41 ++++++++++++++++++----------- 8 files changed, 45 insertions(+), 30 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index beab19ed1..9857a899a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -167,11 +167,12 @@ void resetPidProfile(pidProfile_t *pidProfile) .abs_control_limit = 90, .abs_control_error_limit = 20, .antiGravityMode = ANTI_GRAVITY_SMOOTH, - .dyn_lpf_dterm_max_hz = 250, .dterm_lowpass_hz = 100, // dual PT1 filtering ON by default .dterm_lowpass2_hz = 200, // second Dterm LPF ON by default .dterm_filter_type = FILTER_PT1, .dterm_filter2_type = FILTER_PT1, + .dyn_lpf_dterm_min_hz = 150, + .dyn_lpf_dterm_max_hz = 250, .launchControlMode = LAUNCH_CONTROL_MODE_NORMAL, .launchControlThrottlePercent = 20, .launchControlAngleLimit = 0, @@ -544,7 +545,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) #endif #ifdef USE_DYN_LPF - if (pidProfile->dterm_lowpass_hz > 0 && pidProfile->dyn_lpf_dterm_max_hz > pidProfile->dterm_lowpass_hz) { + if (pidProfile->dyn_lpf_dterm_min_hz > 0 && pidProfile->dyn_lpf_dterm_max_hz > pidProfile->dyn_lpf_dterm_min_hz) { switch (pidProfile->dterm_filter_type) { case FILTER_PT1: dynLpfFilter = DYN_LPF_PT1; @@ -559,7 +560,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) } else { dynLpfFilter = DYN_LPF_NONE; } - dynLpfMin = pidProfile->dterm_lowpass_hz; + dynLpfMin = pidProfile->dyn_lpf_dterm_min_hz; dynLpfMax = pidProfile->dyn_lpf_dterm_max_hz; #endif diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 1d1ac6db2..89afb0c65 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -149,6 +149,7 @@ typedef struct pidProfile_s { uint8_t abs_control_limit; // Limit to the correction uint8_t abs_control_error_limit; // Limit to the accumulated error uint8_t dterm_filter2_type; // Filter selection for 2nd dterm + uint16_t dyn_lpf_dterm_min_hz; uint16_t dyn_lpf_dterm_max_hz; uint8_t launchControlMode; // Whether launch control is limited to pitch only (launch stand or top-mount) or all axes (on battery) uint8_t launchControlThrottlePercent; // Throttle percentage to trigger launch for launch control diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 830bfff73..346f28e2c 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -382,9 +382,6 @@ static const char * const lookupTableRcSmoothingDerivativeType[] = { #endif // USE_RC_SMOOTHING_FILTER #ifdef USE_GYRO_DATA_ANALYSE -static const char * const lookupTableDynamicFftLocation[] = { - "BEFORE_STATIC_FILTERS", "AFTER_STATIC_FILTERS" -}; static const char * const lookupTableDynamicFilterRange[] = { "HIGH", "MEDIUM", "LOW", "AUTO" }; @@ -504,7 +501,6 @@ const lookupTableEntry_t lookupTables[] = { LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDerivativeType), #endif // USE_RC_SMOOTHING_FILTER #ifdef USE_GYRO_DATA_ANALYSE - LOOKUP_TABLE_ENTRY(lookupTableDynamicFftLocation), LOOKUP_TABLE_ENTRY(lookupTableDynamicFilterRange), #endif // USE_GYRO_DATA_ANALYSE #ifdef USE_VTX_COMMON @@ -571,7 +567,9 @@ const clivalue_t valueTable[] = { { "dyn_notch_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_min_hz) }, #endif #ifdef USE_DYN_LPF + { "dyn_lpf_gyro_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_min_hz) }, { "dyn_lpf_gyro_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_max_hz) }, + { "dyn_lpf_dterm_min_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_min_hz) }, { "dyn_lpf_dterm_max_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_max_hz) }, #endif diff --git a/src/main/interface/settings.h b/src/main/interface/settings.h index 1a98969c3..423fe0c1c 100644 --- a/src/main/interface/settings.h +++ b/src/main/interface/settings.h @@ -113,7 +113,6 @@ typedef enum { TABLE_RC_SMOOTHING_DERIVATIVE_TYPE, #endif // USE_RC_SMOOTHING_FILTER #ifdef USE_GYRO_DATA_ANALYSE - TABLE_DYNAMIC_FFT_LOCATION, TABLE_DYNAMIC_FILTER_RANGE, #endif // USE_GYRO_DATA_ANALYSE #ifdef USE_VTX_COMMON diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 218f1d91b..11d534887 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -140,6 +140,7 @@ typedef struct gyroSensor_s { biquadFilter_t notchFilter2[XYZ_AXIS_COUNT]; filterApplyFnPtr notchFilterDynApplyFn; + filterApplyFnPtr notchFilterDynApplyFn2; biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT]; biquadFilter_t notchFilterDyn2[XYZ_AXIS_COUNT]; @@ -215,11 +216,12 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig) gyroConfig->gyro_offset_yaw = 0; gyroConfig->yaw_spin_recovery = true; gyroConfig->yaw_spin_threshold = 1950; + gyroConfig->dyn_lpf_gyro_min_hz = 150; + gyroConfig->dyn_lpf_gyro_max_hz = 450; gyroConfig->dyn_notch_range = DYN_NOTCH_RANGE_AUTO; gyroConfig->dyn_notch_width_percent = 8; gyroConfig->dyn_notch_q = 120; gyroConfig->dyn_notch_min_hz = 150; - gyroConfig->dyn_lpf_gyro_max_hz = 450; #ifdef USE_DYN_LPF gyroConfig->gyro_lowpass_hz = 150; gyroConfig->gyro_lowpass_type = FILTER_BIQUAD; @@ -557,7 +559,7 @@ static FAST_RAM_ZERO_INIT uint16_t dynLpfMax; static void dynLpfFilterInit() { - if (gyroConfig()->gyro_lowpass_hz > 0 && gyroConfig()->dyn_lpf_gyro_max_hz > gyroConfig()->gyro_lowpass_hz) { + if (gyroConfig()->dyn_lpf_gyro_min_hz > 0 && gyroConfig()->dyn_lpf_gyro_max_hz > gyroConfig()->dyn_lpf_gyro_min_hz) { switch (gyroConfig()->gyro_lowpass_type) { case FILTER_PT1: dynLpfFilter = DYN_LPF_PT1; @@ -572,7 +574,7 @@ static void dynLpfFilterInit() } else { dynLpfFilter = DYN_LPF_NONE; } - dynLpfMin = gyroConfig()->gyro_lowpass_hz; + dynLpfMin = gyroConfig()->dyn_lpf_gyro_min_hz; dynLpfMax = gyroConfig()->dyn_lpf_gyro_max_hz; } #endif @@ -693,9 +695,13 @@ static bool isDynamicFilterActive(void) static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) { gyroSensor->notchFilterDynApplyFn = nullFilterApply; + gyroSensor->notchFilterDynApplyFn2 = nullFilterApply; if (isDynamicFilterActive()) { gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2 + if(gyroConfig()->dyn_notch_width_percent != 0) { + gyroSensor->notchFilterDynApplyFn2 = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2 + } const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { biquadFilterInit(&gyroSensor->notchFilterDyn[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index c64d10e9f..fc021eecf 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -98,9 +98,10 @@ typedef struct gyroConfig_s { uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second - uint8_t dyn_notch_range; // ignore any FFT bin below this threshold + uint16_t dyn_lpf_gyro_min_hz; uint16_t dyn_lpf_gyro_max_hz; - uint8_t dyn_notch_width_percent; + uint8_t dyn_notch_range; // ignore any FFT bin below this threshold + uint8_t dyn_notch_width_percent; uint16_t dyn_notch_q; uint16_t dyn_notch_min_hz; } gyroConfig_t; diff --git a/src/main/sensors/gyro_filter_impl.h b/src/main/sensors/gyro_filter_impl.h index 5e741e19a..69111f5d4 100644 --- a/src/main/sensors/gyro_filter_impl.h +++ b/src/main/sensors/gyro_filter_impl.h @@ -32,7 +32,7 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor) } gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf); gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf); - gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn2[axis], gyroADCf); + gyroADCf = gyroSensor->notchFilterDynApplyFn2((filter_t *)&gyroSensor->notchFilterDyn2[axis], gyroADCf); } #endif diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index b58200030..4295f93dc 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/src/main/sensors/gyroanalyse.c @@ -57,12 +57,13 @@ static uint16_t FAST_RAM_ZERO_INIT fftSamplingRateHz; static float FAST_RAM_ZERO_INIT fftResolution; static uint8_t FAST_RAM_ZERO_INIT fftBinOffset; -static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMaxCenterHz; +static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxCtrHz; static uint8_t dynamicFilterRange; -static float FAST_RAM_ZERO_INIT dynamicFilterNotchQ; -static float FAST_RAM_ZERO_INIT dynamicFilterNotch1Center; -static float FAST_RAM_ZERO_INIT dynamicFilterNotch2Center; -static uint16_t FAST_RAM_ZERO_INIT dynFilterNotchMinHz; +static float FAST_RAM_ZERO_INIT dynNotchQ; +static float FAST_RAM_ZERO_INIT dynNotch1Ctr; +static float FAST_RAM_ZERO_INIT dynNotch2Ctr; +static uint16_t FAST_RAM_ZERO_INIT dynNotchMinHz; +static bool FAST_RAM dualNotch = true; // Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window static FAST_RAM_ZERO_INIT float hanningWindow[FFT_WINDOW_SIZE]; @@ -80,10 +81,14 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs) dynamicFilterRange = gyroConfig()->dyn_notch_range; fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_LOW; fftBinOffset = FFT_BIN_OFFSET; - dynamicFilterNotch1Center = 1 - gyroConfig()->dyn_notch_width_percent / 100.0f; - dynamicFilterNotch2Center = 1 + gyroConfig()->dyn_notch_width_percent / 100.0f; - dynamicFilterNotchQ = gyroConfig()->dyn_notch_q / 100.0f; - dynFilterNotchMinHz = gyroConfig()->dyn_notch_min_hz; + dynNotch1Ctr = 1 - gyroConfig()->dyn_notch_width_percent / 100.0f; + dynNotch2Ctr = 1 + gyroConfig()->dyn_notch_width_percent / 100.0f; + dynNotchQ = gyroConfig()->dyn_notch_q / 100.0f; + dynNotchMinHz = gyroConfig()->dyn_notch_min_hz; + + if (gyroConfig()->dyn_notch_width_percent == 0) { + dualNotch = false; + } if (dynamicFilterRange == DYN_NOTCH_RANGE_AUTO) { if (gyroConfig()->dyn_lpf_gyro_max_hz > 333) { @@ -110,7 +115,7 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs) fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE; - dynamicNotchMaxCenterHz = fftSamplingRateHz / 2; //Nyquist + dynNotchMaxCtrHz = fftSamplingRateHz / 2; //Nyquist for (int i = 0; i < FFT_WINDOW_SIZE; i++) { hanningWindow[i] = (0.5f - 0.5f * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1))); @@ -135,8 +140,8 @@ void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptime const float looptime = MAX(1000000u / fftSamplingRateHz, targetLooptimeUs * DYN_NOTCH_CALC_TICKS); for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { // any init value - state->centerFreq[axis] = dynamicNotchMaxCenterHz; - state->prevCenterFreq[axis] = dynamicNotchMaxCenterHz; + state->centerFreq[axis] = dynNotchMaxCtrHz; + state->prevCenterFreq[axis] = dynNotchMaxCtrHz; biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime); } } @@ -303,7 +308,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, } } // get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz) - float centerFreq = dynamicNotchMaxCenterHz; + float centerFreq = dynNotchMaxCtrHz; float fftMeanIndex = 0; // idx was shifted by 1 to start at 1, not 0 if (fftSum > 0) { @@ -313,7 +318,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, } else { centerFreq = state->prevCenterFreq[state->updateAxis]; } - centerFreq = fmax(centerFreq, dynFilterNotchMinHz); + centerFreq = fmax(centerFreq, dynNotchMinHz); centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq); state->prevCenterFreq[state->updateAxis] = state->centerFreq[state->updateAxis]; state->centerFreq[state->updateAxis] = centerFreq; @@ -335,8 +340,12 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, // 7us // calculate cutoffFreq and notch Q, update notch filter =1.8+((A2-150)*0.004) if (state->prevCenterFreq[state->updateAxis] != state->centerFreq[state->updateAxis]) { - biquadFilterUpdate(¬chFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis] * dynamicFilterNotch1Center, gyro.targetLooptime, dynamicFilterNotchQ, FILTER_NOTCH); - biquadFilterUpdate(¬chFilterDyn2[state->updateAxis], state->centerFreq[state->updateAxis] * dynamicFilterNotch2Center, gyro.targetLooptime, dynamicFilterNotchQ, FILTER_NOTCH); + if (dualNotch) { + biquadFilterUpdate(¬chFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch1Ctr, gyro.targetLooptime, dynNotchQ, FILTER_NOTCH); + biquadFilterUpdate(¬chFilterDyn2[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch2Ctr, gyro.targetLooptime, dynNotchQ, FILTER_NOTCH); + } else { + biquadFilterUpdate(¬chFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], gyro.targetLooptime, dynNotchQ, FILTER_NOTCH); + } } DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);