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..9857a899ab 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -167,12 +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 = 200, - .dyn_lpf_dterm_idle = 20, .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, @@ -180,8 +180,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 +464,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 +545,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->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; + 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->dyn_lpf_dterm_min_hz; + dynLpfMax = pidProfile->dyn_lpf_dterm_max_hz; #endif #ifdef USE_LAUNCH_CONTROL @@ -1299,17 +1293,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..89afb0c65d 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -149,8 +149,8 @@ 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 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 f3fb4aa192..6ab020875a 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -382,11 +382,8 @@ 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" + "HIGH", "MEDIUM", "LOW", "AUTO" }; #endif // USE_GYRO_DATA_ANALYSE @@ -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 @@ -565,15 +561,16 @@ 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_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_gyro_idle", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_idle) }, + { "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) }, - { "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/interface/settings.h b/src/main/interface/settings.h index 1a98969c37..423fe0c1cb 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 ab1d8b093a..11d5348870 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -140,7 +140,9 @@ 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]; // overflow and recovery timeUs_t overflowTimeUs; @@ -185,7 +187,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 +216,16 @@ 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_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; #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 +504,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 +553,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()->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; + 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()->dyn_lpf_gyro_min_hz; + dynLpfMax = gyroConfig()->dyn_lpf_gyro_max_hz; } #endif @@ -695,12 +695,17 @@ 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); + biquadFilterInit(&gyroSensor->notchFilterDyn2[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH); } } } @@ -1022,7 +1027,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 +1216,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 +1241,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..fc021eecfd 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,14 @@ 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 + uint16_t dyn_lpf_gyro_min_hz; uint16_t dyn_lpf_gyro_max_hz; - uint8_t dyn_lpf_gyro_idle; + 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; PG_DECLARE(gyroConfig_t, gyroConfig); @@ -126,5 +127,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..69111f5d48 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->notchFilterDynApplyFn2((filter_t *)&gyroSensor->notchFilterDyn2[axis], gyroADCf); } #endif diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index 845cafbd47..4295f93dc7 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/src/main/sensors/gyroanalyse.c @@ -51,23 +51,19 @@ #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 uint16_t FAST_RAM_ZERO_INIT dynNotchMaxCtrHz; static uint8_t dynamicFilterRange; +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]; @@ -82,14 +78,34 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs) gyroAnalyseInitialized = true; #endif - dynamicFilterRange = gyroConfig()->dyn_filter_range; - - fftSamplingRateHz = 1000; - if (dynamicFilterRange == DYN_FILTER_RANGE_HIGH) { - fftSamplingRateHz = 2000; + dynamicFilterRange = gyroConfig()->dyn_notch_range; + fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_LOW; + fftBinOffset = FFT_BIN_OFFSET; + 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; } - else if (dynamicFilterRange == DYN_FILTER_RANGE_MEDIUM) { - fftSamplingRateHz = 1333; + + 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 +114,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; + 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))); @@ -129,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); } } @@ -140,12 +151,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 +185,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 +199,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, @@ -297,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) { @@ -307,15 +318,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, dynNotchMinHz); 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 +338,15 @@ 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]) { + 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); state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT; @@ -355,4 +370,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);