mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-19 14:25:20 +03:00
Second dterm pt1 (#5458)
* Second PT1 on DTerm This PR replaces the default biquad filter with a second PT1 set to 200Hz. Basically allows the user to enable a second, set point configurable, PT1 type first order low-pass filter on DTerm. This is useful because most noise in most logs arises from D, not P. The default is set to on, at twice the normal Dterm setpoint. This provides greater Dterm cut than a single PT1, and twice the steepness of cut above the second setpoint. Modelling shows significant reductions in higher frequency Dterm noise with only minor additional delay. The improvement in noise performance will be less than for biquad, but the delay is considerably less. If with the default settings the overall noise improves a lot, it may be possible bring D both filtering set points to higher numbers (e.g. 140/280), or alternatively remove other filters such as the notch filters, while maintaining an adequate level of control over noise. * Update names, old defaults, fix whitespace Defaults restored to biquad with second PT1 off. ‘lpf’ retained as abbreviation for values, otherwise generally remove ‘Filter’ where redundant, replace ‘FilterLpf’ with ‘Lowpass’, etc, thanks Fujin and DieHertz * Remove underscore in lowpass_2, add hz to setpoint for lowpass Thanks DieHertz * completed replacing lpf with lowpass, added _hz to all lowpass set points in profile Thanks DieHertz * fix whitespace fixed whitespace in settings.c * whitespace attempt #57 * change lpf to lowpass where appropriate elsewhere Note did not change OSD abbreviations, they are still LPF, and did not change gyro_lpf anywhere. * second attempt at a simple PT1 implementation Basically copied from the DtermNotch implementation * Second PT1 on DTerm This PR replaces the default biquad filter with a second PT1 set to 200Hz. Basically allows the user to enable a second, set point configurable, PT1 type first order low-pass filter on DTerm. This is useful because most noise in most logs arises from D, not P. The default is set to on, at twice the normal Dterm setpoint. This provides greater Dterm cut than a single PT1, and twice the steepness of cut above the second setpoint. Modelling shows significant reductions in higher frequency Dterm noise with only minor additional delay. The improvement in noise performance will be less than for biquad, but the delay is considerably less. If with the default settings the overall noise improves a lot, it may be possible bring D both filtering set points to higher numbers (e.g. 140/280), or alternatively remove other filters such as the notch filters, while maintaining an adequate level of control over noise. * Rebase * Remove underscore in lowpass_2, add hz to setpoint for lowpass Thanks DieHertz * completed replacing lpf with lowpass, added _hz to all lowpass set points in profile Thanks DieHertz * fix whitespace fixed whitespace in settings.c * whitespace attempt #57 * change lpf to lowpass where appropriate elsewhere Note did not change OSD abbreviations, they are still LPF, and did not change gyro_lpf anywhere. * second attempt at a simple PT1 implementation Basically copied from the DtermNotch implementation * Whitespace fix - thanks, Ledvinap * Fix PG issue by moving added dterm_lowpass2_hz to bottom of struct * Got rid of redundant indirection * Fixed indentantion shifts
This commit is contained in:
ctzsnooze
Michael Keller
parent
d2bc4e5af5
commit
ab231b7c84
6 changed files with 70 additions and 62 deletions
|
|
@ -103,8 +103,9 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
|||
|
||||
.pidSumLimit = PIDSUM_LIMIT,
|
||||
.pidSumLimitYaw = PIDSUM_LIMIT_YAW,
|
||||
.yaw_lpf_hz = 0,
|
||||
.dterm_lpf_hz = 100, // filtering ON by default
|
||||
.yaw_lowpass_hz = 0,
|
||||
.dterm_lowpass_hz = 100, // filtering ON by default
|
||||
.dterm_lowpass2_hz = 0, // second Dterm LPF OFF by default
|
||||
.dterm_notch_hz = 260,
|
||||
.dterm_notch_cutoff = 160,
|
||||
.dterm_filter_type = FILTER_BIQUAD,
|
||||
|
|
@ -167,20 +168,22 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState)
|
|||
|
||||
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
|
||||
|
||||
static FAST_RAM filterApplyFnPtr dtermNotchFilterApplyFn;
|
||||
static FAST_RAM void *dtermFilterNotch[2];
|
||||
static FAST_RAM filterApplyFnPtr dtermLpfApplyFn;
|
||||
static FAST_RAM void *dtermFilterLpf[2];
|
||||
static FAST_RAM filterApplyFnPtr ptermYawFilterApplyFn;
|
||||
static FAST_RAM void *ptermYawFilter;
|
||||
|
||||
typedef union dtermFilterLpf_u {
|
||||
pt1Filter_t pt1Filter[2];
|
||||
biquadFilter_t biquadFilter[2];
|
||||
typedef union dtermLowpass_u {
|
||||
pt1Filter_t pt1Filter;
|
||||
biquadFilter_t biquadFilter;
|
||||
#if defined(USE_FIR_FILTER_DENOISE)
|
||||
firFilterDenoise_t denoisingFilter[2];
|
||||
firFilterDenoise_t denoisingFilter;
|
||||
#endif
|
||||
} dtermFilterLpf_t;
|
||||
} dtermLowpass_t;
|
||||
|
||||
static FAST_RAM filterApplyFnPtr dtermNotchApplyFn;
|
||||
static FAST_RAM biquadFilter_t dtermNotch[2];
|
||||
static FAST_RAM filterApplyFnPtr dtermLowpassApplyFn;
|
||||
static FAST_RAM dtermLowpass_t dtermLowpass[2];
|
||||
static FAST_RAM filterApplyFnPtr dtermLowpass2ApplyFn;
|
||||
static FAST_RAM pt1Filter_t dtermLowpass2[2];
|
||||
static FAST_RAM filterApplyFnPtr ptermYawLowpassApplyFn;
|
||||
static FAST_RAM pt1Filter_t ptermYawLowpass;
|
||||
|
||||
void pidInitFilters(const pidProfile_t *pidProfile)
|
||||
{
|
||||
|
|
@ -188,9 +191,9 @@ void pidInitFilters(const pidProfile_t *pidProfile)
|
|||
|
||||
if (targetPidLooptime == 0) {
|
||||
// no looptime set, so set all the filters to null
|
||||
dtermNotchFilterApplyFn = nullFilterApply;
|
||||
dtermLpfApplyFn = nullFilterApply;
|
||||
ptermYawFilterApplyFn = nullFilterApply;
|
||||
dtermNotchApplyFn = nullFilterApply;
|
||||
dtermLowpassApplyFn = nullFilterApply;
|
||||
ptermYawLowpassApplyFn = nullFilterApply;
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -208,58 +211,60 @@ void pidInitFilters(const pidProfile_t *pidProfile)
|
|||
}
|
||||
|
||||
if (dTermNotchHz != 0 && pidProfile->dterm_notch_cutoff != 0) {
|
||||
static biquadFilter_t biquadFilterNotch[2];
|
||||
dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||
dtermNotchApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||
const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff);
|
||||
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
|
||||
dtermFilterNotch[axis] = &biquadFilterNotch[axis];
|
||||
biquadFilterInit(dtermFilterNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH);
|
||||
biquadFilterInit(&dtermNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH);
|
||||
}
|
||||
} else {
|
||||
dtermNotchFilterApplyFn = nullFilterApply;
|
||||
dtermNotchApplyFn = nullFilterApply;
|
||||
}
|
||||
|
||||
//2nd Dterm Lowpass Filter
|
||||
if (pidProfile->dterm_lowpass2_hz == 0 || pidProfile->dterm_lowpass2_hz > pidFrequencyNyquist) {
|
||||
dtermLowpass2ApplyFn = nullFilterApply;
|
||||
} else {
|
||||
dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply;
|
||||
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
|
||||
pt1FilterInit(&dtermLowpass2[axis], pidProfile->dterm_lowpass2_hz, dT);
|
||||
}
|
||||
}
|
||||
|
||||
static dtermFilterLpf_t dtermFilterLpfUnion;
|
||||
if (pidProfile->dterm_lpf_hz == 0 || pidProfile->dterm_lpf_hz > pidFrequencyNyquist) {
|
||||
dtermLpfApplyFn = nullFilterApply;
|
||||
if (pidProfile->dterm_lowpass_hz == 0 || pidProfile->dterm_lowpass_hz > pidFrequencyNyquist) {
|
||||
dtermLowpassApplyFn = nullFilterApply;
|
||||
} else {
|
||||
switch (pidProfile->dterm_filter_type) {
|
||||
default:
|
||||
dtermLpfApplyFn = nullFilterApply;
|
||||
dtermLowpassApplyFn = nullFilterApply;
|
||||
break;
|
||||
case FILTER_PT1:
|
||||
dtermLpfApplyFn = (filterApplyFnPtr)pt1FilterApply;
|
||||
dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
|
||||
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
|
||||
dtermFilterLpf[axis] = &dtermFilterLpfUnion.pt1Filter[axis];
|
||||
pt1FilterInit(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, dT);
|
||||
pt1FilterInit(&dtermLowpass[axis].pt1Filter, pidProfile->dterm_lowpass_hz, dT);
|
||||
}
|
||||
break;
|
||||
case FILTER_BIQUAD:
|
||||
dtermLpfApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||
dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
|
||||
dtermFilterLpf[axis] = &dtermFilterLpfUnion.biquadFilter[axis];
|
||||
biquadFilterInitLPF(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
|
||||
biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, pidProfile->dterm_lowpass_hz, targetPidLooptime);
|
||||
}
|
||||
break;
|
||||
#if defined(USE_FIR_FILTER_DENOISE)
|
||||
case FILTER_FIR:
|
||||
dtermLpfApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate;
|
||||
dtermLowpassApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate;
|
||||
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
|
||||
dtermFilterLpf[axis] = &dtermFilterLpfUnion.denoisingFilter[axis];
|
||||
firFilterDenoiseInit(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
|
||||
firFilterDenoiseInit(&dtermLowpass[axis].denoisingFilter, pidProfile->dterm_lowpass_hz, targetPidLooptime);
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
static pt1Filter_t pt1FilterYaw;
|
||||
if (pidProfile->yaw_lpf_hz == 0 || pidProfile->yaw_lpf_hz > pidFrequencyNyquist) {
|
||||
ptermYawFilterApplyFn = nullFilterApply;
|
||||
if (pidProfile->yaw_lowpass_hz == 0 || pidProfile->yaw_lowpass_hz > pidFrequencyNyquist) {
|
||||
ptermYawLowpassApplyFn = nullFilterApply;
|
||||
} else {
|
||||
ptermYawFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;
|
||||
ptermYawFilter = &pt1FilterYaw;
|
||||
pt1FilterInit(ptermYawFilter, pidProfile->yaw_lpf_hz, dT);
|
||||
ptermYawLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
|
||||
pt1FilterInit(&ptermYawLowpass, pidProfile->yaw_lowpass_hz, dT);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -498,7 +503,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
|
|||
// -----calculate P component and add Dynamic Part based on stick input
|
||||
axisPID_P[axis] = Kp[axis] * errorRate * tpaFactor;
|
||||
if (axis == FD_YAW) {
|
||||
axisPID_P[axis] = ptermYawFilterApplyFn(ptermYawFilter, axisPID_P[axis]);
|
||||
axisPID_P[axis] = ptermYawLowpassApplyFn((filter_t *) &ptermYawLowpass, axisPID_P[axis]);
|
||||
}
|
||||
|
||||
// -----calculate I component
|
||||
|
|
@ -513,8 +518,9 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
|
|||
// -----calculate D component
|
||||
if (axis != FD_YAW) {
|
||||
// apply filters
|
||||
float gyroRateFiltered = dtermNotchFilterApplyFn(dtermFilterNotch[axis], gyroRate);
|
||||
gyroRateFiltered = dtermLpfApplyFn(dtermFilterLpf[axis], gyroRateFiltered);
|
||||
float gyroRateFiltered = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], gyroRate);
|
||||
gyroRateFiltered = dtermLowpassApplyFn((filter_t *) &dtermLowpass[axis], gyroRateFiltered);
|
||||
gyroRateFiltered = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateFiltered);
|
||||
|
||||
const float rD = dynCd * MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f) * currentPidSetpoint - gyroRateFiltered; // cr - y
|
||||
// Divide rate change by deltaT to get differential (ie dr/dt)
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue