diff --git a/src/main/build/debug.c b/src/main/build/debug.c index bf5b201ad4..c2ecc276cf 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -76,4 +76,5 @@ const char * const debugModeNames[DEBUG_COUNT] = { "RX_SIGNAL_LOSS", "RC_SMOOTHING_RATE", "ANTI_GRAVITY", + "DYN_LPF", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index cbe0bb3576..5958dec110 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -94,6 +94,7 @@ typedef enum { DEBUG_RX_SIGNAL_LOSS, DEBUG_RC_SMOOTHING_RATE, DEBUG_ANTI_GRAVITY, + DEBUG_DYN_LPF, DEBUG_COUNT } debugType_e; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a49096f9e1..beab19ed11 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -167,8 +167,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .abs_control_limit = 90, .abs_control_error_limit = 20, .antiGravityMode = ANTI_GRAVITY_SMOOTH, - .dyn_lpf_dterm_max_hz = 200, - .dyn_lpf_dterm_idle = 20, + .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, @@ -180,8 +179,8 @@ void resetPidProfile(pidProfile_t *pidProfile) .launchControlAllowTriggerReset = true, ); #ifdef USE_DYN_LPF - pidProfile->dterm_lowpass_hz = 120; - pidProfile->dterm_lowpass2_hz = 180; + pidProfile->dterm_lowpass_hz = 150; + pidProfile->dterm_lowpass2_hz = 150; pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_filter2_type = FILTER_BIQUAD; #endif @@ -464,11 +463,9 @@ void pidUpdateAntiGravityThrottleFilter(float throttle) } #ifdef USE_DYN_LPF -static FAST_RAM int8_t dynLpfFilter = DYN_LPF_NONE; -static FAST_RAM_ZERO_INIT float dynLpfIdle; -static FAST_RAM_ZERO_INIT float dynLpfIdlePoint; -static FAST_RAM_ZERO_INIT float dynLpfInvIdlePointScaled; +static FAST_RAM uint8_t dynLpfFilter = DYN_LPF_NONE; static FAST_RAM_ZERO_INIT uint16_t dynLpfMin; +static FAST_RAM_ZERO_INIT uint16_t dynLpfMax; #endif void pidInitConfig(const pidProfile_t *pidProfile) @@ -547,27 +544,23 @@ void pidInitConfig(const pidProfile_t *pidProfile) #endif #ifdef USE_DYN_LPF - if (pidProfile->dyn_lpf_dterm_idle > 0 && pidProfile->dyn_lpf_dterm_max_hz > 0) { - if (pidProfile->dterm_lowpass_hz > 0 ) { - dynLpfMin = pidProfile->dterm_lowpass_hz; - switch (pidProfile->dterm_filter_type) { - case FILTER_PT1: - dynLpfFilter = DYN_LPF_PT1; - break; - case FILTER_BIQUAD: - dynLpfFilter = DYN_LPF_BIQUAD; - break; - default: - dynLpfFilter = DYN_LPF_NONE; - break; - } + if (pidProfile->dterm_lowpass_hz > 0 && pidProfile->dyn_lpf_dterm_max_hz > pidProfile->dterm_lowpass_hz) { + switch (pidProfile->dterm_filter_type) { + case FILTER_PT1: + dynLpfFilter = DYN_LPF_PT1; + break; + case FILTER_BIQUAD: + dynLpfFilter = DYN_LPF_BIQUAD; + break; + default: + dynLpfFilter = DYN_LPF_NONE; + break; } } else { dynLpfFilter = DYN_LPF_NONE; } - dynLpfIdle = pidProfile->dyn_lpf_dterm_idle / 100.0f; - dynLpfIdlePoint = (dynLpfIdle - (dynLpfIdle * dynLpfIdle * dynLpfIdle) / 3.0f) * 1.5f; - dynLpfInvIdlePointScaled = 1 / (1 - dynLpfIdlePoint) * (pidProfile->dyn_lpf_dterm_max_hz - dynLpfMin); + dynLpfMin = pidProfile->dterm_lowpass_hz; + dynLpfMax = pidProfile->dyn_lpf_dterm_max_hz; #endif #ifdef USE_LAUNCH_CONTROL @@ -1299,17 +1292,13 @@ bool pidAntiGravityEnabled(void) void dynLpfDTermUpdate(float throttle) { if (dynLpfFilter != DYN_LPF_NONE) { - uint16_t cutoffFreq = dynLpfMin; - if (throttle > dynLpfIdle) { - const float dynThrottle = (throttle - (throttle * throttle * throttle) / 3.0f) * 1.5f; - cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePointScaled; - } + const unsigned int cutoffFreq = fmax(dynThrottle(throttle) * dynLpfMax, dynLpfMin); if (dynLpfFilter == DYN_LPF_PT1) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { pt1FilterUpdateCutoff(&dtermLowpass[axis].pt1Filter, pt1FilterGain(cutoffFreq, dT)); } - } else { + } else if (dynLpfFilter == DYN_LPF_BIQUAD) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { biquadFilterUpdateLPF(&dtermLowpass[axis].biquadFilter, cutoffFreq, targetPidLooptime); } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 329fbb773e..1d1ac6db2d 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -150,7 +150,6 @@ typedef struct pidProfile_s { 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_max_hz; - uint8_t dyn_lpf_dterm_idle; 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 uint8_t launchControlAngleLimit; // Optional launch control angle limit (requires ACC) diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 807e0b99a4..830bfff73c 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -386,7 +386,7 @@ static const char * const lookupTableDynamicFftLocation[] = { "BEFORE_STATIC_FILTERS", "AFTER_STATIC_FILTERS" }; static const char * const lookupTableDynamicFilterRange[] = { - "HIGH", "MEDIUM", "LOW" + "HIGH", "MEDIUM", "LOW", "AUTO" }; #endif // USE_GYRO_DATA_ANALYSE @@ -565,15 +565,14 @@ const clivalue_t valueTable[] = { { "gyro_to_use", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) }, #endif #if defined(USE_GYRO_DATA_ANALYSE) - { "dyn_fft_location", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FFT_LOCATION }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_fft_location) }, - { "dyn_filter_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 99 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_width_percent) }, - { "dyn_filter_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FILTER_RANGE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_range) }, + { "dyn_notch_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FILTER_RANGE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_range) }, + { "dyn_notch_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 20 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_width_percent) }, + { "dyn_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_q) }, + { "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_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_max_hz) }, - { "dyn_lpf_gyro_idle", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_idle) }, { "dyn_lpf_dterm_max_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_max_hz) }, - { "dyn_lpf_dterm_idle", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_idle) }, #endif // PG_ACCELEROMETER_CONFIG diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index ab1d8b093a..218f1d91b1 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -141,6 +141,7 @@ typedef struct gyroSensor_s { filterApplyFnPtr notchFilterDynApplyFn; biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT]; + biquadFilter_t notchFilterDyn2[XYZ_AXIS_COUNT]; // overflow and recovery timeUs_t overflowTimeUs; @@ -185,7 +186,7 @@ static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int typ #define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps for 2000dps gyro) #define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps for 2000dps gyro) -PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 5); +PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 6); #ifndef GYRO_CONFIG_USE_GYRO_DEFAULT #define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1 @@ -214,13 +215,15 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig) gyroConfig->gyro_offset_yaw = 0; gyroConfig->yaw_spin_recovery = true; gyroConfig->yaw_spin_threshold = 1950; - gyroConfig->dyn_filter_width_percent = 40; - gyroConfig->dyn_fft_location = DYN_FFT_AFTER_STATIC_FILTERS; - gyroConfig->dyn_filter_range = DYN_FILTER_RANGE_MEDIUM; - gyroConfig->dyn_lpf_gyro_max_hz = 400; - gyroConfig->dyn_lpf_gyro_idle = 20; + 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 = 120; + gyroConfig->gyro_lowpass_hz = 150; + gyroConfig->gyro_lowpass_type = FILTER_BIQUAD; + gyroConfig->gyro_lowpass2_hz = 0; #endif } @@ -499,6 +502,7 @@ bool gyroInit(void) case DEBUG_GYRO_RAW: case DEBUG_GYRO_SCALED: case DEBUG_GYRO_FILTERED: + case DEBUG_DYN_LPF: gyroDebugMode = debugMode; break; case DEBUG_DUAL_GYRO: @@ -547,35 +551,29 @@ bool gyroInit(void) } #ifdef USE_DYN_LPF -static FAST_RAM int8_t dynLpfFilter = DYN_LPF_NONE; -static FAST_RAM_ZERO_INIT float dynLpfIdle; -static FAST_RAM_ZERO_INIT float dynLpfIdlePoint; -static FAST_RAM_ZERO_INIT float dynLpfInvIdlePointScaled; +static FAST_RAM uint8_t dynLpfFilter = DYN_LPF_NONE; static FAST_RAM_ZERO_INIT uint16_t dynLpfMin; +static FAST_RAM_ZERO_INIT uint16_t dynLpfMax; static void dynLpfFilterInit() { - if (gyroConfig()->dyn_lpf_gyro_idle > 0 && gyroConfig()->dyn_lpf_gyro_max_hz > 0) { - if (gyroConfig()->gyro_lowpass_hz > 0 ) { - dynLpfMin = gyroConfig()->gyro_lowpass_hz; - switch (gyroConfig()->gyro_lowpass_type) { - case FILTER_PT1: - dynLpfFilter = DYN_LPF_PT1; - break; - case FILTER_BIQUAD: - dynLpfFilter = DYN_LPF_BIQUAD; - break; - default: - dynLpfFilter = DYN_LPF_NONE; - break; - } - } + if (gyroConfig()->gyro_lowpass_hz > 0 && gyroConfig()->dyn_lpf_gyro_max_hz > gyroConfig()->gyro_lowpass_hz) { + switch (gyroConfig()->gyro_lowpass_type) { + case FILTER_PT1: + dynLpfFilter = DYN_LPF_PT1; + break; + case FILTER_BIQUAD: + dynLpfFilter = DYN_LPF_BIQUAD; + break; + default: + dynLpfFilter = DYN_LPF_NONE; + break; + } } else { dynLpfFilter = DYN_LPF_NONE; } - dynLpfIdle = gyroConfig()->dyn_lpf_gyro_idle / 100.0f; - dynLpfIdlePoint = (dynLpfIdle - (dynLpfIdle * dynLpfIdle * dynLpfIdle) / 3.0f) * 1.5f; - dynLpfInvIdlePointScaled = 1 / (1 - dynLpfIdlePoint) * (gyroConfig()->dyn_lpf_gyro_max_hz - dynLpfMin); + dynLpfMin = gyroConfig()->gyro_lowpass_hz; + dynLpfMax = gyroConfig()->dyn_lpf_gyro_max_hz; } #endif @@ -701,6 +699,7 @@ static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) 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); + biquadFilterInit(&gyroSensor->notchFilterDyn2[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH); } } } @@ -1022,7 +1021,7 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens #ifdef USE_GYRO_DATA_ANALYSE if (isDynamicFilterActive()) { - gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn); + gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn, gyroSensor->notchFilterDyn2); } #endif @@ -1211,14 +1210,17 @@ uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg) #endif // USE_GYRO_REGISTER_DUMP #ifdef USE_DYN_LPF + +float dynThrottle(float throttle) { + return throttle * (1 - (throttle * throttle) / 3.0f) * 1.5f; +} + void dynLpfGyroUpdate(float throttle) { if (dynLpfFilter != DYN_LPF_NONE) { - uint16_t cutoffFreq = dynLpfMin; - if (throttle > dynLpfIdle) { - const float dynThrottle = (throttle - (throttle * throttle * throttle) / 3.0f) * 1.5f; - cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePointScaled; - } + const unsigned int cutoffFreq = fmax(dynThrottle(throttle) * dynLpfMax, dynLpfMin); + + DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq); if (dynLpfFilter == DYN_LPF_PT1) { const float gyroDt = gyro.targetLooptime * 1e-6f; for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { @@ -1233,7 +1235,7 @@ void dynLpfGyroUpdate(float throttle) pt1FilterUpdateCutoff(&gyroSensor1.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt)); #endif } - } else { + } else if (dynLpfFilter == DYN_LPF_BIQUAD) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { #ifdef USE_MULTI_GYRO if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 7d35e8681c..c64d10e9fc 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -43,15 +43,15 @@ typedef enum { } gyroOverflowCheck_e; enum { - DYN_FFT_BEFORE_STATIC_FILTERS = 0, - DYN_FFT_AFTER_STATIC_FILTERS + DYN_NOTCH_RANGE_HIGH = 0, + DYN_NOTCH_RANGE_MEDIUM, + DYN_NOTCH_RANGE_LOW, + DYN_NOTCH_RANGE_AUTO } ; -enum { - DYN_FILTER_RANGE_HIGH = 0, - DYN_FILTER_RANGE_MEDIUM, - DYN_FILTER_RANGE_LOW -} ; +#define DYN_NOTCH_RANGE_HZ_HIGH 2000 +#define DYN_NOTCH_RANGE_HZ_MEDIUM 1333 +#define DYN_NOTCH_RANGE_HZ_LOW 1000 enum { DYN_LPF_NONE = 0, @@ -96,13 +96,13 @@ typedef struct gyroConfig_s { uint8_t yaw_spin_recovery; int16_t yaw_spin_threshold; - uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second + uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second - uint8_t dyn_filter_width_percent; - uint8_t dyn_fft_location; // before or after static filters - uint8_t dyn_filter_range; // ignore any FFT bin below this threshold + uint8_t dyn_notch_range; // ignore any FFT bin below this threshold uint16_t dyn_lpf_gyro_max_hz; - uint8_t dyn_lpf_gyro_idle; + uint8_t dyn_notch_width_percent; + uint16_t dyn_notch_q; + uint16_t dyn_notch_min_hz; } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); @@ -126,5 +126,6 @@ bool gyroYawSpinDetected(void); uint16_t gyroAbsRateDps(int axis); uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg); #ifdef USE_DYN_LPF +float dynThrottle(float throttle); void dynLpfGyroUpdate(float throttle); #endif diff --git a/src/main/sensors/gyro_filter_impl.h b/src/main/sensors/gyro_filter_impl.h index faf2564693..5e741e19a3 100644 --- a/src/main/sensors/gyro_filter_impl.h +++ b/src/main/sensors/gyro_filter_impl.h @@ -12,10 +12,9 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor) if (axis == X) { GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf)); + GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 0, lrintf(gyroADCf)); } } - - float gyroDataForAnalysis = gyroADCf; #endif // apply static notch filters and software lowpass filters @@ -26,17 +25,14 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor) #ifdef USE_GYRO_DATA_ANALYSE if (isDynamicFilterActive()) { - if (gyroConfig()->dyn_fft_location == DYN_FFT_AFTER_STATIC_FILTERS) { - gyroDataForAnalysis = gyroADCf; - } - if (axis == X) { - GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroDataForAnalysis)); - GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroDataForAnalysis)); + GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); + GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf)); + GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 3, lrintf(gyroADCf)); } - - gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroDataForAnalysis); + gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf); gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf); + gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn2[axis], gyroADCf); } #endif diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index 845cafbd47..b582000301 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/src/main/sensors/gyroanalyse.c @@ -51,23 +51,18 @@ #define FFT_BIN_OFFSET 2 // smoothing frequency for FFT centre frequency #define DYN_NOTCH_SMOOTH_FREQ_HZ 50 -// notch centre point will not go below sample rate divided by these dividers, resulting in range limits: -// HIGH : 133/166-1000Hz, MEDIUM -> 89/111-666Hz, LOW -> 67/83-500Hz -#define DYN_NOTCH_MIN_CENTRE_DIV 12 -// divider to get lowest allowed notch cutoff frequency -// otherwise cutoff is user configured percentage below centre frequency -#define DYN_NOTCH_MIN_CUTOFF_DIV 15 // we need 4 steps for each axis #define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4) 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 dynamicNotchMinCenterHz; static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMaxCenterHz; -static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMinCutoffHz; -static float FAST_RAM_ZERO_INIT dynamicFilterWidthFactor; 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; // Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window static FAST_RAM_ZERO_INIT float hanningWindow[FFT_WINDOW_SIZE]; @@ -82,14 +77,30 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs) gyroAnalyseInitialized = true; #endif - dynamicFilterRange = gyroConfig()->dyn_filter_range; - - fftSamplingRateHz = 1000; - if (dynamicFilterRange == DYN_FILTER_RANGE_HIGH) { - fftSamplingRateHz = 2000; - } - else if (dynamicFilterRange == DYN_FILTER_RANGE_MEDIUM) { - fftSamplingRateHz = 1333; + 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; + + if (dynamicFilterRange == DYN_NOTCH_RANGE_AUTO) { + if (gyroConfig()->dyn_lpf_gyro_max_hz > 333) { + fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_MEDIUM; + } + if (gyroConfig()->dyn_lpf_gyro_max_hz > 610) { + fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH; + fftBinOffset = 1; + } + } else { + if (dynamicFilterRange == DYN_NOTCH_RANGE_HIGH) { + fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH; + fftBinOffset = 1; + } + else if (dynamicFilterRange == DYN_NOTCH_RANGE_MEDIUM) { + fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_MEDIUM; + } } // If we get at least 3 samples then use the default FFT sample frequency // otherwise we need to calculate a FFT sample frequency to ensure we get 3 samples (gyro loops < 4K) @@ -98,13 +109,8 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs) fftSamplingRateHz = MIN((gyroLoopRateHz / 3), fftSamplingRateHz); fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE; - fftBinOffset = FFT_BIN_OFFSET; dynamicNotchMaxCenterHz = fftSamplingRateHz / 2; //Nyquist - dynamicNotchMinCenterHz = fftSamplingRateHz / DYN_NOTCH_MIN_CENTRE_DIV; - dynamicNotchMinCutoffHz = fftSamplingRateHz / DYN_NOTCH_MIN_CUTOFF_DIV; - dynamicFilterWidthFactor = (100.0f - gyroConfig()->dyn_filter_width_percent) / 100; - 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))); @@ -140,12 +146,12 @@ void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float state->oversampledGyroAccumulator[axis] += sample; } -static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn); +static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2); /* * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function */ -void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn) +void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2) { // samples should have been pushed by `gyroDataAnalysePush` // if gyro sampling is > 1kHz, accumulate multiple samples @@ -174,7 +180,7 @@ void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn) // calculate FFT and update filters if (state->updateTicks > 0) { - gyroDataAnalyseUpdate(state, notchFilterDyn); + gyroDataAnalyseUpdate(state, notchFilterDyn, notchFilterDyn2); --state->updateTicks; } } @@ -188,7 +194,7 @@ void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t /* * Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds */ -static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn) +static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2) { enum { STEP_ARM_CFFT_F32, @@ -307,15 +313,15 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, } else { centerFreq = state->prevCenterFreq[state->updateAxis]; } - // constrain and low-pass smooth centre frequency - centerFreq = constrain(centerFreq, dynamicNotchMinCenterHz, dynamicNotchMaxCenterHz); + centerFreq = fmax(centerFreq, dynFilterNotchMinHz); centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq); - centerFreq = constrain(centerFreq, dynamicNotchMinCenterHz, dynamicNotchMaxCenterHz); + state->prevCenterFreq[state->updateAxis] = state->centerFreq[state->updateAxis]; state->centerFreq[state->updateAxis] = centerFreq; if (state->updateAxis == 0) { - DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100)); - DEBUG_SET(DEBUG_FFT_FREQ, 0, state->centerFreq[state->updateAxis]); + DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100)); + DEBUG_SET(DEBUG_FFT_FREQ, 0, state->centerFreq[state->updateAxis]); + DEBUG_SET(DEBUG_DYN_LPF, 1, state->centerFreq[state->updateAxis]); } if (state->updateAxis == 1) { DEBUG_SET(DEBUG_FFT_FREQ, 1, state->centerFreq[state->updateAxis]); @@ -327,11 +333,11 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, case STEP_UPDATE_FILTERS: { // 7us - // calculate cutoffFreq and notch Q, update notch filter - const float cutoffFreq = fmax(state->centerFreq[state->updateAxis] * dynamicFilterWidthFactor, dynamicNotchMinCutoffHz); - const float notchQ = filterGetNotchQ(state->centerFreq[state->updateAxis], cutoffFreq); - biquadFilterUpdate(¬chFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], gyro.targetLooptime, notchQ, FILTER_NOTCH); - + // 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); + } DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT; @@ -355,4 +361,5 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, state->updateStep = (state->updateStep + 1) % STEP_COUNT; } + #endif // USE_GYRO_DATA_ANALYSE diff --git a/src/main/sensors/gyroanalyse.h b/src/main/sensors/gyroanalyse.h index a1b8e7a175..c708429724 100644 --- a/src/main/sensors/gyroanalyse.h +++ b/src/main/sensors/gyroanalyse.h @@ -58,4 +58,4 @@ STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlyi void gyroDataAnalyseStateInit(gyroAnalyseState_t *gyroAnalyse, uint32_t targetLooptime); void gyroDataAnalysePush(gyroAnalyseState_t *gyroAnalyse, int axis, float sample); -void gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse, biquadFilter_t *notchFilterDyn); +void gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2);