diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 3d95fadc24..ecd951af7b 100644 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -195,6 +195,13 @@ static void validateAndFixConfig(void) currentPidProfile->dterm_notch_hz = 0; } +#ifdef USE_DYN_LPF + //PRevent invalid dynamic lowpass + if (currentPidProfile->dyn_lpf_dterm_min_hz > currentPidProfile->dyn_lpf_dterm_max_hz) { + currentPidProfile->dyn_lpf_dterm_min_hz = 0; + } +#endif + if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) { featureDisable(FEATURE_3D); @@ -432,6 +439,12 @@ void validateAndFixGyroConfig(void) if (gyroConfig()->gyro_soft_notch_cutoff_2 >= gyroConfig()->gyro_soft_notch_hz_2) { gyroConfigMutable()->gyro_soft_notch_hz_2 = 0; } +#ifdef USE_DYN_LPF + //Prevent invalid dynamic lowpass filter + if (gyroConfig()->dyn_lpf_gyro_min_hz > gyroConfig()->dyn_lpf_gyro_max_hz) { + gyroConfigMutable()->dyn_lpf_gyro_min_hz = 0; + } +#endif if (gyroConfig()->gyro_hardware_lpf == GYRO_HARDWARE_LPF_1KHZ_SAMPLE) { pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a58894855d..75f0e64d51 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -315,14 +315,18 @@ void pidInitFilters(const pidProfile_t *pidProfile) } //1st Dterm Lowpass Filter - if (pidProfile->dterm_lowpass_hz == 0 || pidProfile->dterm_lowpass_hz > pidFrequencyNyquist) { - dtermLowpassApplyFn = nullFilterApply; - } else { + uint16_t dterm_lowpass_hz = pidProfile->dterm_lowpass_hz; + +#ifdef USE_DYN_LPF + dterm_lowpass_hz = MAX(pidProfile->dterm_lowpass_hz, pidProfile->dyn_lpf_dterm_min_hz); +#endif + + if (dterm_lowpass_hz > 0 && dterm_lowpass_hz < pidFrequencyNyquist) { switch (pidProfile->dterm_filter_type) { case FILTER_PT1: dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply; for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { - pt1FilterInit(&dtermLowpass[axis].pt1Filter, pt1FilterGain(pidProfile->dterm_lowpass_hz, dT)); + pt1FilterInit(&dtermLowpass[axis].pt1Filter, pt1FilterGain(dterm_lowpass_hz, dT)); } break; case FILTER_BIQUAD: @@ -332,13 +336,15 @@ void pidInitFilters(const pidProfile_t *pidProfile) dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply; #endif for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { - biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, pidProfile->dterm_lowpass_hz, targetPidLooptime); + biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, dterm_lowpass_hz, targetPidLooptime); } break; default: dtermLowpassApplyFn = nullFilterApply; break; } + } else { + dtermLowpassApplyFn = nullFilterApply; } //2nd Dterm Lowpass Filter @@ -609,7 +615,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) #endif #ifdef USE_DYN_LPF - if (pidProfile->dyn_lpf_dterm_min_hz > 0 && pidProfile->dyn_lpf_dterm_max_hz > pidProfile->dyn_lpf_dterm_min_hz) { + if (pidProfile->dyn_lpf_dterm_min_hz > 0) { switch (pidProfile->dterm_filter_type) { case FILTER_PT1: dynLpfFilter = DYN_LPF_PT1; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index cd6cf2fa5d..fd76fa976c 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -578,7 +578,7 @@ static FAST_RAM_ZERO_INIT uint16_t dynLpfMax; static void dynLpfFilterInit() { - if (gyroConfig()->dyn_lpf_gyro_min_hz > 0 && gyroConfig()->dyn_lpf_gyro_max_hz > gyroConfig()->dyn_lpf_gyro_min_hz) { + if (gyroConfig()->dyn_lpf_gyro_min_hz > 0) { switch (gyroConfig()->gyro_lowpass_type) { case FILTER_PT1: dynLpfFilter = DYN_LPF_PT1; @@ -736,11 +736,17 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) gyroInitSlewLimiter(gyroSensor); #endif + uint16_t gyro_lowpass_hz = gyroConfig()->gyro_lowpass_hz; + +#ifdef USE_DYN_LPF + gyro_lowpass_hz = MAX(gyroConfig()->gyro_lowpass_hz, gyroConfig()->dyn_lpf_gyro_min_hz); +#endif + gyroInitLowpassFilterLpf( gyroSensor, FILTER_LOWPASS, gyroConfig()->gyro_lowpass_type, - gyroConfig()->gyro_lowpass_hz + gyro_lowpass_hz ); gyroInitLowpassFilterLpf( @@ -1244,8 +1250,8 @@ void dynLpfGyroUpdate(float throttle) if (dynLpfFilter != DYN_LPF_NONE) { const unsigned int cutoffFreq = fmax(dynThrottle(throttle) * dynLpfMax, dynLpfMin); - DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq); if (dynLpfFilter == DYN_LPF_PT1) { + DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq); const float gyroDt = gyro.targetLooptime * 1e-6f; for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { #ifdef USE_MULTI_GYRO @@ -1260,6 +1266,7 @@ void dynLpfGyroUpdate(float throttle) #endif } } else if (dynLpfFilter == DYN_LPF_BIQUAD) { + DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq); 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) {