diff --git a/src/main/config/simplified_tuning.c b/src/main/config/simplified_tuning.c index 0d2e0b63b4..167f0120d0 100644 --- a/src/main/config/simplified_tuning.c +++ b/src/main/config/simplified_tuning.c @@ -65,22 +65,50 @@ static void calculateNewPidValues(pidProfile_t *pidProfile) static void calculateNewDTermFilterValues(pidProfile_t *pidProfile) { - pidProfile->dterm_lpf1_dyn_min_hz = constrain(DTERM_LPF1_DYN_MIN_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ); - pidProfile->dterm_lpf1_dyn_max_hz = constrain(DTERM_LPF1_DYN_MAX_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ); - pidProfile->dterm_lpf1_static_hz = constrain(DTERM_LPF1_DYN_MIN_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ); - pidProfile->dterm_lpf2_static_hz = constrain(DTERM_LPF2_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, LPF_MAX_HZ); - pidProfile->dterm_lpf1_type = FILTER_PT1; - pidProfile->dterm_lpf2_type = FILTER_PT1; + if (pidProfile->dterm_lpf1_dyn_min_hz) { + pidProfile->dterm_lpf1_dyn_min_hz = constrain(DTERM_LPF1_DYN_MIN_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ); + pidProfile->dterm_lpf1_dyn_max_hz = constrain(DTERM_LPF1_DYN_MAX_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ); + } + + if (pidProfile->dterm_lpf1_static_hz) { + pidProfile->dterm_lpf1_static_hz = constrain(DTERM_LPF1_DYN_MIN_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ); + } + + if (pidProfile->dterm_lpf2_static_hz) { + pidProfile->dterm_lpf2_static_hz = constrain(DTERM_LPF2_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, LPF_MAX_HZ); + } + + if (!pidProfile->dterm_lpf1_type) { + pidProfile->dterm_lpf1_type = FILTER_PT1; + } + + if (!pidProfile->dterm_lpf2_type) { + pidProfile->dterm_lpf2_type = FILTER_PT1; + } } static void calculateNewGyroFilterValues() { - gyroConfigMutable()->gyro_lpf1_dyn_min_hz = constrain(GYRO_LPF1_DYN_MIN_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ); - gyroConfigMutable()->gyro_lpf1_dyn_max_hz = constrain(GYRO_LPF1_DYN_MAX_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ); - gyroConfigMutable()->gyro_lpf1_static_hz = constrain(GYRO_LPF1_DYN_MIN_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ); - gyroConfigMutable()->gyro_lpf2_static_hz = constrain(GYRO_LPF2_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, LPF_MAX_HZ); - gyroConfigMutable()->gyro_lpf1_type = FILTER_PT1; - gyroConfigMutable()->gyro_lpf2_type = FILTER_PT1; + if (gyroConfigMutable()->gyro_lpf1_dyn_min_hz) { + gyroConfigMutable()->gyro_lpf1_dyn_min_hz = constrain(GYRO_LPF1_DYN_MIN_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ); + gyroConfigMutable()->gyro_lpf1_dyn_max_hz = constrain(GYRO_LPF1_DYN_MAX_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ); + } + + if (gyroConfigMutable()->gyro_lpf1_static_hz) { + gyroConfigMutable()->gyro_lpf1_static_hz = constrain(GYRO_LPF1_DYN_MIN_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ); + } + + if (gyroConfigMutable()->gyro_lpf2_static_hz) { + gyroConfigMutable()->gyro_lpf2_static_hz = constrain(GYRO_LPF2_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, LPF_MAX_HZ); + } + + if (!gyroConfigMutable()->gyro_lpf1_type) { + gyroConfigMutable()->gyro_lpf1_type = FILTER_PT1; + } + + if (!gyroConfigMutable()->gyro_lpf2_type) { + gyroConfigMutable()->gyro_lpf2_type = FILTER_PT1; + } } void applySimplifiedTuning(pidProfile_t *pidProfile)