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

Implement biquad on 2nd dterm lowpass filter.

This commit is contained in:
Kenneth Mitchell 2018-10-14 12:02:53 -04:00
parent 799704cee3
commit 641d0a2e54
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_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;
}
}

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_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);

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

@ -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) },