rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #6933 from kmitchel/dlpf2_biquad 

Implement biquad on 2nd dterm lowpass filter.
This commit is contained in:
Michael Keller 2018-10-19 01:32:19 +13:00 committed by GitHub
parent 7631e2419c 641d0a2e54
commit 9ab8370379
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 20 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/main/flight/pid.c
View File

@ -130,6 +130,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.dterm_notch_hz = 0, .dterm_notch_hz = 0,
.dterm_notch_cutoff = 0, .dterm_notch_cutoff = 0,
.dterm_filter_type = FILTER_PT1, .dterm_filter_type = FILTER_PT1,
.dterm_filter2_type = FILTER_PT1,
.itermWindupPointPercent = 40, .itermWindupPointPercent = 40,
.vbatPidCompensation = 0, .vbatPidCompensation = 0,
.pidAtMinThrottle = PID_STABILISATION_ON, .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 filterApplyFnPtr dtermLowpassApplyFn;
static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpass2ApplyFn; 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 filterApplyFnPtr ptermYawLowpassApplyFn;
static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass; static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass;
#if defined(USE_ITERM_RELAX) #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) { if (pidProfile->dterm_lowpass2_hz == 0 || pidProfile->dterm_lowpass2_hz > pidFrequencyNyquist) {
dtermLowpass2ApplyFn = nullFilterApply; dtermLowpass2ApplyFn = nullFilterApply;
} else { } else {
dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply; switch (pidProfile->dterm_filter2_type) {
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { default:
pt1FilterInit(&dtermLowpass2[axis], pt1FilterGain(pidProfile->dterm_lowpass2_hz, dT)); 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;
} }
} }

1
src/main/flight/pid.h
View File

@ -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_gain; // How strongly should the absolute accumulated error be corrected for
uint8_t abs_control_limit; // Limit to the correction uint8_t abs_control_limit; // Limit to the correction
uint8_t abs_control_error_limit; // Limit to the accumulated error uint8_t abs_control_error_limit; // Limit to the accumulated error
uint8_t dterm_filter2_type; // Filter selection for 2nd dterm
} pidProfile_t; } pidProfile_t;
PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles); PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles);

1
src/main/interface/settings.c
View File

@ -829,6 +829,7 @@ const clivalue_t valueTable[] = {
// PG_PID_PROFILE // 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_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_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_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_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) }, { "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_cutoff) },