Skip to content

Commit

Permalink
allow choosing your own q for dterm biquad
Browse files Browse the repository at this point in the history
  • Loading branch information
@Quick-Flash @nerdCopter
Quick-Flash authored and nerdCopter committed Jul 15, 2022
1 parent e86e758 commit a3ae306
Show file tree
Hide file tree
Showing 5 changed files with 21 additions and 6 deletions.
7 changes: 5 additions & 2 deletions src/main/cli/settings.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1072,8 +1072,10 @@ const clivalue_t valueTable[] = {
{ "dterm_lpf1_dyn_expo", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 10 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf1_dyn_expo) },
#endif
{ PARAM_NAME_DTERM_LPF1_TYPE, VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DTERM_LPF_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf1_type) },
{ "dterm_lpf1_biquad_q", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 10 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf1_biquad_q) },
{ PARAM_NAME_DTERM_LPF1_STATIC_HZ, VAR_INT16 | PROFILE_VALUE, .config.minmax = { 0, LPF_MAX_HZ }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf1_static_hz) },
{ PARAM_NAME_DTERM_LPF2_TYPE, VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DTERM_LPF_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf2_type) },
{ "dterm_lpf2_biquad_q", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 10 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf2_biquad_q) },
{ PARAM_NAME_DTERM_LPF2_STATIC_HZ, VAR_INT16 | PROFILE_VALUE, .config.minmax = { 0, LPF_MAX_HZ }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf2_static_hz) },
{ PARAM_NAME_DTERM_NOTCH_HZ, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, LPF_MAX_HZ }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_hz) },
{ PARAM_NAME_DTERM_NOTCH_CUTOFF, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, LPF_MAX_HZ }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_cutoff) },
Expand Down Expand Up @@ -1698,9 +1700,10 @@ const clivalue_t valueTable[] = {
#endif

#ifdef USE_RPM_FILTER
{ PARAM_NAME_RPM_FILTER_HARMONICS, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_harmonics) },
{ PARAM_NAME_RPM_FILTER_HARMONICS, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_harmonics) },
{ PARAM_NAME_RPM_FILTER_Q, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 250, 3000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_q) },
{ PARAM_NAME_RPM_FILTER_MIN_HZ, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 30, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_min_hz) },
{ PARAM_NAME_RPM_FILTER_Q, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 255 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, noise_limit) },
{ PARAM_NAME_RPM_FILTER_MIN_HZ, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 30, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_min_hz) },
{ PARAM_NAME_RPM_FILTER_FADE_RANGE_HZ, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_fade_range_hz) },
{ PARAM_NAME_RPM_FILTER_LPF_HZ, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 500 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_lpf_hz) },
#endif
Expand Down
6 changes: 6 additions & 0 deletions src/main/common/filter.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -185,6 +185,12 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint
const float cs = cos_approx(omega);
const float alpha = sn / (2.0f * Q);

// shift filterFreq of the biquad to satisfy -3dB cutoff condition for all Q values
if (filterType == FILTER_LPF) {
const float q = 2 * Q * Q;
filterFreq *= sqrtf((1 - q + sqrtf(2 * q * (q - 1) + 1)) / q);
}

switch (filterType) {
case FILTER_LPF:
// 2nd order Butterworth (with Q=1/sqrt(2)) / Butterworth biquad section with Q
Expand Down
4 changes: 3 additions & 1 deletion src/main/flight/pid.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -180,7 +180,9 @@ void resetPidProfile(pidProfile_t *pidProfile)
// reset the lowpass filter type to PT1 overriding the desired BIQUAD setting.
.dterm_lpf2_static_hz = DTERM_LPF2_HZ_DEFAULT, // second Dterm LPF ON by default
.dterm_lpf1_type = FILTER_PT1,
.dterm_lpf1_biquad_q = 141,
.dterm_lpf2_type = FILTER_PT1,
.dterm_lpf2_biquad_q = 141,
.dterm_lpf1_dyn_min_hz = DTERM_LPF1_DYN_MIN_HZ_DEFAULT,
.dterm_lpf1_dyn_max_hz = DTERM_LPF1_DYN_MAX_HZ_DEFAULT,
.launchControlMode = LAUNCH_CONTROL_MODE_NORMAL,
Expand Down Expand Up @@ -1261,7 +1263,7 @@ void dynLpfDTermUpdate(float throttle)
break;
case DYN_LPF_BIQUAD:
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterUpdateLPF(&pidRuntime.dtermLowpass[axis].biquadFilter, cutoffFreq, targetPidLooptime);
biquadFilterUpdate(&pidRuntime.dtermLowpass[axis].biquadFilter, cutoffFreq, targetPidLooptime, pidRuntime.dtermLowpassBiquadQ, FILTER_LPF, 1.0f);
}
break;
case DYN_LPF_PT2:
Expand Down
4 changes: 4 additions & 0 deletions src/main/flight/pid.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -139,6 +139,7 @@ typedef struct pidProfile_s {
pidf_t pid[PID_ITEM_COUNT];

uint8_t dterm_lpf1_type; // Filter type for dterm lowpass 1
uint8_t dterm_lpf1_biquad_q; // q value for the dterm lowpass 1 biquad
uint8_t itermWindupPointPercent; // iterm windup threshold, percent motor saturation
uint16_t pidSumLimit;
uint16_t pidSumLimitYaw;
Expand Down Expand Up @@ -180,6 +181,7 @@ typedef struct pidProfile_s {
uint8_t abs_control_error_limit; // Limit to the accumulated error
uint8_t abs_control_cutoff; // Cutoff frequency for path estimation in abs control
uint8_t dterm_lpf2_type; // Filter type for 2nd dterm lowpass
uint8_t dterm_lpf2_biquad_q; // q value for the dterm lowpass 1 biquad
uint16_t dterm_lpf1_dyn_min_hz; // Dterm lowpass filter 1 min hz when in dynamic mode
uint16_t dterm_lpf1_dyn_max_hz; // Dterm lowpass filter 1 max hz when in dynamic mode
uint8_t launchControlMode; // Whether launch control is limited to pitch only (launch stand or top-mount) or all axes (on battery)
Expand Down Expand Up @@ -364,6 +366,8 @@ typedef struct pidRuntime_s {
uint8_t dynLpfCurveExpo;
#endif

float dtermLowpassBiquadQ;

#ifdef USE_LAUNCH_CONTROL
uint8_t launchControlMode;
uint8_t launchControlAngleLimit;
Expand Down
6 changes: 3 additions & 3 deletions src/main/flight/pid_init.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,6 +103,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)

//1st Dterm Lowpass Filter
uint16_t dterm_lpf1_init_hz = pidProfile->dterm_lpf1_static_hz;
pidRuntime.dtermLowpassBiquadQ = pidProfile->dterm_lpf1_biquad_q / 100.0f;

#ifdef USE_DYN_LPF
if (pidProfile->dterm_lpf1_dyn_min_hz) {
Expand All @@ -126,7 +127,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
#endif
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
biquadFilterInitLPF(&pidRuntime.dtermLowpass[axis].biquadFilter, dterm_lpf1_init_hz, targetPidLooptime);
biquadFilterInit(&pidRuntime.dtermLowpass[axis].biquadFilter, dterm_lpf1_init_hz, targetPidLooptime, pidRuntime.dtermLowpassBiquadQ, FILTER_LPF, 1.0f);
}
} else {
pidRuntime.dtermLowpassApplyFn = nullFilterApply;
Expand Down Expand Up @@ -165,7 +166,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
if (pidProfile->dterm_lpf2_static_hz < pidFrequencyNyquist) {
pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
biquadFilterInitLPF(&pidRuntime.dtermLowpass2[axis].biquadFilter, pidProfile->dterm_lpf2_static_hz, targetPidLooptime);
biquadFilterInit(&pidRuntime.dtermLowpass2[axis].biquadFilter, pidProfile->dterm_lpf2_static_hz, targetPidLooptime, pidProfile->dterm_lpf2_biquad_q / 100.0f, FILTER_LPF, 1.0f);
}
} else {
pidRuntime.dtermLowpassApplyFn = nullFilterApply;
Expand Down Expand Up @@ -440,4 +441,3 @@ void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex)
memcpy(pidProfilesMutable(dstPidProfileIndex), pidProfilesMutable(srcPidProfileIndex), sizeof(pidProfile_t));
}
}

0 comments on commit a3ae306

Please sign in to comment.