diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 65ee10255..08661698e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -130,6 +130,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .dterm_notch_hz = 0, .dterm_notch_cutoff = 0, .dterm_filter_type = FILTER_PT1, + .dterm_filter2_type = FILTER_PT1, .itermWindupPointPercent = 40, .vbatPidCompensation = 0, .pidAtMinThrottle = PID_STABILISATION_ON, @@ -212,7 +213,7 @@ static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpassApplyFn; static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpass2ApplyFn; -static FAST_RAM_ZERO_INIT pt1Filter_t dtermLowpass2[XYZ_AXIS_COUNT]; +static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass2[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn; static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass; #if defined(USE_ITERM_RELAX) @@ -271,9 +272,22 @@ void pidInitFilters(const pidProfile_t *pidProfile) if (pidProfile->dterm_lowpass2_hz == 0 || pidProfile->dterm_lowpass2_hz > pidFrequencyNyquist) { dtermLowpass2ApplyFn = nullFilterApply; } else { - dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply; - for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { - pt1FilterInit(&dtermLowpass2[axis], pt1FilterGain(pidProfile->dterm_lowpass2_hz, dT)); + switch (pidProfile->dterm_filter2_type) { + default: + dtermLowpass2ApplyFn = nullFilterApply; + break; + case FILTER_PT1: + dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply; + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + pt1FilterInit(&dtermLowpass2[axis].pt1Filter, pt1FilterGain(pidProfile->dterm_lowpass2_hz, dT)); + } + break; + case FILTER_BIQUAD: + dtermLowpass2ApplyFn = (filterApplyFnPtr)biquadFilterApply; + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + biquadFilterInitLPF(&dtermLowpass2[axis].biquadFilter, pidProfile->dterm_lowpass2_hz, targetPidLooptime); + } + break; } } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 581ab7d86..25806bdcd 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -148,6 +148,7 @@ typedef struct pidProfile_s { uint8_t abs_control_gain; // How strongly should the absolute accumulated error be corrected for 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 } pidProfile_t; PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index f3f4d21ae..82114e47a 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -829,6 +829,7 @@ const clivalue_t valueTable[] = { // PG_PID_PROFILE { "dterm_lowpass_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DTERM_LOWPASS_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_filter_type) }, { "dterm_lowpass_hz", VAR_INT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lowpass_hz) }, + { "dterm_lowpass2_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DTERM_LOWPASS_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_filter2_type) }, { "dterm_lowpass2_hz", VAR_INT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lowpass2_hz) }, { "dterm_notch_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_hz) }, { "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_cutoff) },