Second PT1 on DTerm #5427
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -105,6 +105,7 @@ void resetPidProfile(pidProfile_t *pidProfile) | |
| .pidSumLimitYaw = PIDSUM_LIMIT_YAW, | ||
| .yaw_lpf_hz = 0, | ||
| .dterm_lpf_hz = 100, // filtering ON by default | ||
| .dterm_lpf2_hz = 0, // second Dterm LPF OFF by default | ||
| .dterm_notch_hz = 260, | ||
| .dterm_notch_cutoff = 160, | ||
| .dterm_filter_type = FILTER_BIQUAD, | ||
|
|
@@ -167,28 +168,30 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState) | |
|
|
||
| const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; | ||
|
|
||
| static FAST_RAM filterApplyFnPtr dtermNotchApplyFn; | ||
| static FAST_RAM void *dtermNotch[2]; | ||
| static FAST_RAM filterApplyFnPtr dtermLowpassApplyFn; | ||
| static FAST_RAM void *dtermLowpass[2]; | ||
| static FAST_RAM filterApplyFnPtr dtermLowpass2ApplyFn; | ||
| static FAST_RAM void *dtermLowpass2[2]; | ||
| static FAST_RAM filterApplyFnPtr ptermYawLowpassApplyFn; | ||
| static FAST_RAM void *ptermYawLowpass; | ||
|
|
||
| typedef union dtermLowpass_u { | ||
| pt1Filter_t pt1Filter[2]; | ||
| biquadFilter_t biquadFilter[2]; | ||
| firFilterDenoise_t denoisingFilter[2]; | ||
| } dtermLowpass_t; | ||
|
|
||
| void pidInitFilters(const pidProfile_t *pidProfile) | ||
| { | ||
| BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2 | ||
|
|
||
| if (targetPidLooptime == 0) { | ||
| // no looptime set, so set all the filters to null | ||
| dtermNotchApplyFn = nullFilterApply; | ||
| dtermLowpassApplyFn = nullFilterApply; | ||
| ptermYawLowpassApplyFn = nullFilterApply; | ||
| return; | ||
| } | ||
|
|
||
|
|
@@ -207,55 +210,67 @@ void pidInitFilters(const pidProfile_t *pidProfile) | |
|
|
||
| if (dTermNotchHz != 0 && pidProfile->dterm_notch_cutoff != 0) { | ||
| static biquadFilter_t biquadFilterNotch[2]; | ||
| dtermNotchApplyFn = (filterApplyFnPtr)biquadFilterApply; | ||
| const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff); | ||
| for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { | ||
| dtermNotch[axis] = &biquadFilterNotch[axis]; | ||
| biquadFilterInit(dtermNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH); | ||
| } | ||
| } else { | ||
| dtermNotchApplyFn = nullFilterApply; | ||
| } | ||
|
|
||
| //2nd Dterm Lowpass Filter | ||
| static pt1Filter_t dtermLowpass2State; | ||
| if (pidProfile->dterm_lpf2_hz == 0 || pidProfile->dterm_lpf2_hz > pidFrequencyNyquist) { | ||
| dtermLowpass2ApplyFn = nullFilterApply; | ||
| } else { | ||
| dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply; | ||
| for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { | ||
| dtermLowpass2[axis] = &dtermLowpass2State; | ||
| pt1FilterInit(dtermLowpass2[axis], pidProfile->dterm_lpf2_hz, dT); | ||
| } | ||
| } | ||
|
|
||
| static dtermLowpass_t dtermLowpassUnion; | ||
|
There was a problem hiding this comment. Same here, this effectively makes state shared for all axes There was a problem hiding this comment. OK I just did some re-naming, that's all, to 'dtermLowpass2State' from previous name of 'pt1FilterD', as suggested by Fujin. The code itself is copied from yaw p lowpass further down. There was a problem hiding this comment. I checked, I've just used exactly the same method as for the yaw code and the other filter code as in release 3.3. Hope there's nothing fundamentally wrong, but if there is, we will need to fix it. There was a problem hiding this comment. Yaw is just one axis, so it's not shared by definition. There was a problem hiding this comment. Have a look at how it's done in Fujin's PR for gyro lowpass, you need to create state for PID filters in advance. I can help when I get home There was a problem hiding this comment. @ctzsnooze : the problem is here: https://github.com/betaflight/betaflight/pull/5427/files#r173423911 There was a problem hiding this comment. OK sorry I have to wait for actual code suggestion, too dumb to know how to do that. There was a problem hiding this comment. Would this work better? There was a problem hiding this comment. dtermLowpass_t contains 2-element arrays for filter state (roll+pitch) Use There was a problem hiding this comment. I just pushed this, hope it is now OK? Basically same structure as the preceding dterm notch. |
||
| if (pidProfile->dterm_lpf_hz == 0 || pidProfile->dterm_lpf_hz > pidFrequencyNyquist) { | ||
| dtermLowpassApplyFn = nullFilterApply; | ||
| } else { | ||
| switch (pidProfile->dterm_filter_type) { | ||
| default: | ||
| dtermLowpassApplyFn = nullFilterApply; | ||
| break; | ||
| case FILTER_PT1: | ||
| dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply; | ||
| for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { | ||
| dtermLowpass[axis] = &dtermLowpassUnion.pt1Filter[axis]; | ||
| pt1FilterInit(dtermLowpass[axis], pidProfile->dterm_lpf_hz, dT); | ||
| } | ||
| break; | ||
| case FILTER_BIQUAD: | ||
| dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply; | ||
| for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { | ||
| dtermLowpass[axis] = &dtermLowpassUnion.biquadFilter[axis]; | ||
| biquadFilterInitLPF(dtermLowpass[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); | ||
| } | ||
| break; | ||
| case FILTER_FIR: | ||
| dtermLowpassApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate; | ||
| for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { | ||
| dtermLowpass[axis] = &dtermLowpassUnion.denoisingFilter[axis]; | ||
| firFilterDenoiseInit(dtermLowpass[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); | ||
| } | ||
| break; | ||
| } | ||
| } | ||
|
|
||
| static pt1Filter_t pt1LowpassYaw; | ||
| if (pidProfile->yaw_lpf_hz == 0 || pidProfile->yaw_lpf_hz > pidFrequencyNyquist) { | ||
| ptermYawLowpassApplyFn = nullFilterApply; | ||
| } else { | ||
| ptermYawLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply; | ||
| ptermYawLowpass = &pt1LowpassYaw; | ||
| pt1FilterInit(ptermYawLowpass, pidProfile->yaw_lpf_hz, dT); | ||
| } | ||
| } | ||
|
|
||
|
|
@@ -494,7 +509,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] = ptermYawLowpassApplyFn(ptermYawLowpass, axisPID_P[axis]); | ||
| } | ||
|
|
||
| // -----calculate I component | ||
|
|
@@ -509,10 +524,11 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an | |
| // -----calculate D component | ||
| if (axis != FD_YAW) { | ||
| // apply filters | ||
| float gyroRateFiltered = dtermNotchApplyFn(dtermNotch[axis], gyroRate); | ||
| gyroRateFiltered = dtermLowpassApplyFn(dtermLowpass[axis], gyroRateFiltered); | ||
| gyroRateFiltered = dtermLowpass2ApplyFn(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) | ||
| float delta = (rD - previousRateError[axis]) / deltaT; | ||
|
|
||
|
|
||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This falls short of using the same state for all axes, just like @fujin's PR before