From 1fdaaa3bf8e8ac81ffac08b0e87dbb2ed69450ef Mon Sep 17 00:00:00 2001 From: Mark Haslinghuis Date: Sat, 30 Oct 2021 01:47:58 +0200 Subject: [PATCH] Change simplified tuning filter slider calculation Add dterm part Remove unneeded resets Save lowpass_type with simplified tuning --- src/main/config/simplified_tuning.c | 52 ++++++++++++++++++++++++++++--------- 1 file changed, 40 insertions(+), 12 deletions(-) diff --git a/src/main/config/simplified_tuning.c b/src/main/config/simplified_tuning.c index 0d2e0b63b..167f0120d 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) -- 2.11.4.GIT