Second PT1 on DTerm #5427
Second PT1 on DTerm #5427
Conversation
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.
- fixed TBS BST "Rates" was missing problem
@@ -171,6 +172,8 @@ 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 dtermLpf2ApplyFn; | |||
static FAST_RAM void *dtermLpf2Filter[2]; |
DieHertz
Mar 9, 2018
Member
Please make the naming uniform with existing code, e.gm dtermFilterLpf2
instead.
Or better, let's rename old and new to dtermLowpass
and dtermLowpass2
. Why have the world filter twice in one name? :)
Please make the naming uniform with existing code, e.gm dtermFilterLpf2
instead.
Or better, let's rename old and new to dtermLowpass
and dtermLowpass2
. Why have the world filter twice in one name? :)
fujin
Mar 9, 2018
Member
lowpass
/lowpass2
is also the naming I went with in the dual-stage PR.
lowpass
/lowpass2
is also the naming I went with in the dual-stage PR.
fujin
Mar 9, 2018
Member
makes me wonder if we should be using FAST_RAM
in gyroSensor for similar storage as well...
makes me wonder if we should be using FAST_RAM
in gyroSensor for similar storage as well...
DieHertz
Mar 9, 2018
Member
Don't dive into that, I'm working on it as a part of F7 optimization branch
Don't dive into that, I'm working on it as a part of F7 optimization branch
ctzsnooze
Mar 9, 2018
Author
Contributor
OK will change to to dtermLowpass and dtermLowpass2
OK will change to to dtermLowpass and dtermLowpass2
@@ -637,6 +637,7 @@ const clivalue_t valueTable[] = { | |||
|
|||
// PG_PID_PROFILE | |||
{ "dterm_lowpass_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_filter_type) }, | |||
{ "dterm_lowpass_2", VAR_INT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf_2_hz) }, |
DieHertz
Mar 9, 2018
Member
Should be dterm_lowpass2_type
Should be dterm_lowpass2_type
ctzsnooze
Mar 9, 2018
Author
Contributor
With this implementation, the type can't be changed, it's set to a PT1, so the user enters the value in Hz.
Maybe it should be 'dterm_lowpass_2_hz'
With this implementation, the type can't be changed, it's set to a PT1, so the user enters the value in Hz.
Maybe it should be 'dterm_lowpass_2_hz'
DieHertz
Mar 9, 2018
Member
No underscore before 2, please, to conform with gyro lowpass and notch
No underscore before 2, please, to conform with gyro lowpass and notch
ctzsnooze
Mar 9, 2018
Author
Contributor
done
done
@@ -216,6 +219,18 @@ void pidInitFilters(const pidProfile_t *pidProfile) | |||
} else { | |||
dtermNotchFilterApplyFn = nullFilterApply; | |||
} | |||
|
|||
//extra Dterm PT1 filter | |||
static pt1Filter_t ExtraPt1FilterD; |
fujin
Mar 9, 2018
Member
remove 'extra',
will we ever allow others than pt1?
remove 'extra',
will we ever allow others than pt1?
ctzsnooze
Mar 9, 2018
•
Author
Contributor
I struggled with the name for this. At the moment it's an additional/extra Dterm filter, it is always a PT1, and the user can only enable/set the cutoff point.
I based the name on the current syntax for the yaw filter:
static pt1Filter_t pt1FilterYaw;
would dtermFilterLpf2 be better?
I struggled with the name for this. At the moment it's an additional/extra Dterm filter, it is always a PT1, and the user can only enable/set the cutoff point.
I based the name on the current syntax for the yaw filter:
static pt1Filter_t pt1FilterYaw;
would dtermFilterLpf2 be better?
fujin
Mar 9, 2018
Member
It may be possible to refactor the new gyroInitLowpassLpf() function written to support dual/multi-stage dterm/yaw lowpass of varying type, we can do further renaming there after this is tested/merged.
for now, I suggest, e.g.:
static filter_t *dtermLowpassState
static pt1Filter_t dtermLowpass2State
Extra
is fine for now as well if we want to revisit.
It may be possible to refactor the new gyroInitLowpassLpf() function written to support dual/multi-stage dterm/yaw lowpass of varying type, we can do further renaming there after this is tested/merged.
for now, I suggest, e.g.:
static filter_t *dtermLowpassState
static pt1Filter_t dtermLowpass2State
Extra
is fine for now as well if we want to revisit.
ctzsnooze
Mar 9, 2018
Author
Contributor
Hmm it has to be different from the other names for the same thing
Hmm it has to be different from the other names for the same thing
@@ -75,6 +75,7 @@ typedef struct pidProfile_s { | |||
|
|||
uint16_t yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy | |||
uint16_t dterm_lpf_hz; // Delta Filter in hz | |||
uint16_t dterm_lpf_2_hz; // Extra PT1 Filter on D in hz |
fujin
Mar 9, 2018
Member
I'd been standardizing names and suggest the following:
dterm_lowpass_hz
dterm_lowpass2_hz
I'd been standardizing names and suggest the following:
dterm_lowpass_hz
dterm_lowpass2_hz
ctzsnooze
Mar 9, 2018
Author
Contributor
yes no problem
yes no problem
ctzsnooze
Mar 9, 2018
Author
Contributor
Actually no, decided to keep 'lpf' for the units themselves (the actual values), in CLI they are all 'lpf', so I will keep 'dterm_lpf_hz' etc here. I think perhaps it's easier to read that way, 'lpf' is a value, 'lowpass' is a filter.
Actually no, decided to keep 'lpf' for the units themselves (the actual values), in CLI they are all 'lpf', so I will keep 'dterm_lpf_hz' etc here. I think perhaps it's easier to read that way, 'lpf' is a value, 'lowpass' is a filter.
DieHertz
Mar 9, 2018
Member
I suggested renaming it in CLI as well to make it uniform with gyro_lowpass_hz
I suggested renaming it in CLI as well to make it uniform with gyro_lowpass_hz
@@ -511,6 +526,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an | |||
// apply filters | |||
float gyroRateFiltered = dtermNotchFilterApplyFn(dtermFilterNotch[axis], gyroRate); | |||
gyroRateFiltered = dtermLpfApplyFn(dtermFilterLpf[axis], gyroRateFiltered); | |||
gyroRateFiltered = dtermLpf2ApplyFn(dtermLpf2Filter[axis], gyroRateFiltered); |
fujin
Mar 9, 2018
Member
whitespace/indentation here
whitespace/indentation here
ctzsnooze
Mar 9, 2018
Author
Contributor
fixed (I think, pls check)
fixed (I think, pls check)
//extra Dterm PT1 filter | ||
static pt1Filter_t ExtraPt1FilterD; | ||
if (pidProfile->dterm_lpf_2_hz == 0 || pidProfile->dterm_lpf_2_hz > pidFrequencyNyquist) { | ||
dtermLpf2ApplyFn = nullFilterApply; |
fujin
Mar 9, 2018
Member
better to set this regardless and then do the following block in the if(){} instead of if/else.
better to set this regardless and then do the following block in the if(){} instead of if/else.
@@ -105,9 +105,10 @@ void resetPidProfile(pidProfile_t *pidProfile) | |||
.pidSumLimitYaw = PIDSUM_LIMIT_YAW, | |||
.yaw_lpf_hz = 0, | |||
.dterm_lpf_hz = 100, // filtering ON by default | |||
.dterm_lpf_2_hz = 200, // second PT1 ON by default |
fujin
Mar 9, 2018
Member
let's default to 0
(off) for now.
let's default to 0
(off) for now.
ctzsnooze
Mar 9, 2018
•
Author
Contributor
OK.
But if feedback is very positive during release candidate stage, the advantage of providing a suitable default is considerable for new users.
OK.
But if feedback is very positive during release candidate stage, the advantage of providing a suitable default is considerable for new users.
fujin
Mar 9, 2018
Member
We are not at release candidate stage presently but I would support/entertain a PR setting more aggressive defaults leading up to (or during) that phase. The general guidance I have received is to implement the functionality (default to off).
We are not at release candidate stage presently but I would support/entertain a PR setting more aggressive defaults leading up to (or during) that phase. The general guidance I have received is to implement the functionality (default to off).
ctzsnooze
Mar 9, 2018
Author
Contributor
no problem
no problem
.dterm_notch_hz = 260, | ||
.dterm_notch_cutoff = 160, | ||
.dterm_filter_type = FILTER_BIQUAD, | ||
.dterm_filter_type = FILTER_PT1, |
fujin
Mar 9, 2018
Member
changing defaults is generally not recommended at the same time as implementation
changing defaults is generally not recommended at the same time as implementation
DieHertz
Mar 9, 2018
Member
I second that, let's collect more report first that for most people PT1+PT1 are better than Biquad
I second that, let's collect more report first that for most people PT1+PT1 are better than Biquad
ctzsnooze
Mar 9, 2018
•
Author
Contributor
OK, but if the user feedback is very positive, then it would be best to enable it by default. I am very confident it is better with the suggested defaults than biquad, the current default.
OK, but if the user feedback is very positive, then it would be best to enable it by default. I am very confident it is better with the suggested defaults than biquad, the current default.
@ctzsnooze : One property of chaining PT1 is that resulting filter Q is always 1/2 - critically damped. With higher Q (1/sqrt(2) for 2nd order Butterworth) it is possible to extend passband closer to cutoff frequency ... |
Looks really promising! I'm excited to test this both in combination with and instead of dual-stage gyro LPF's. Great work. |
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
@Ledivinap yes that's a good thing about second order filters. I plan to compare a single biquad to dual separated PT1's, hopefully to establish cutoffs that actually give filtering equivalence in logs, and then to see if anyone can tell the difference in flight. Theoretically will be less delay from separated PT1's, but also less filtering. At least this build allows a bit more fine tuning of how we filter D. |
@@ -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_lpf_2_hz = 0, // second Dterm LPF OFF by default |
DieHertz
Mar 9, 2018
Member
There's still an underscore before 2
There's still an underscore before 2
@@ -75,6 +75,7 @@ typedef struct pidProfile_s { | |||
|
|||
uint16_t yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy | |||
uint16_t dterm_lpf_hz; // Delta Filter in hz | |||
uint16_t dterm_lpf_2_hz; // Extra PT1 Filter on D in hz |
DieHertz
Mar 9, 2018
Member
I suggested renaming it in CLI as well to make it uniform with gyro_lowpass_hz
I suggested renaming it in CLI as well to make it uniform with gyro_lowpass_hz
Thanks, there remains the point about underscore, and I think making lowpass naming conventions for gyro and Dterm uniform is beneficial. |
Thanks for all your feedback Fujin and DieHertz! Definitely more readable. |
{ "dterm_lowpass_2", VAR_INT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf_2_hz) }, | ||
{ "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf_hz) }, | ||
{ "dterm_lowpass_hz", VAR_INT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf_hz) }, | ||
{ "dterm_lowpass2_hz", VAR_INT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf2_hz) }, |
DieHertz
Mar 9, 2018
Member
Looks like there's a redundant indent :)
Looks like there's a redundant indent :)
@DieHertz - Ah OK got it, removed those underscores. Added _hz in CLI name for setpoint i.e. now dterm_lowpass_hz and dterm_lowpass2_hz |
…ints in profile Thanks DieHertz
Replaced 'lpf' within pid.c and related CLI names with 'lowpass', added _hz to CLI names for settings in hz, all should be consistent now, hope to have fixed whitespace too. |
fixed whitespace in settings.c
Good job, will have a look soon |
Just noticed the static state, was it added by your changes or present before? |
} else { | ||
dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply; | ||
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { | ||
dtermLowpass2[axis] = &dtermLowpass2State; |
} | ||
} | ||
|
||
static dtermLowpass_t dtermLowpassUnion; |
DieHertz
Mar 9, 2018
Member
Same here, this effectively makes state shared for all axes
Same here, this effectively makes state shared for all axes
ctzsnooze
Mar 9, 2018
•
Author
Contributor
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.
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.
ctzsnooze
Mar 9, 2018
Author
Contributor
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.
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.
DieHertz
Mar 9, 2018
Member
Yaw is just one axis, so it's not shared by definition.
In your code roll&pitch get shared Dterm state for lowpass.
Yaw is just one axis, so it's not shared by definition.
In your code roll&pitch get shared Dterm state for lowpass.
DieHertz
Mar 9, 2018
•
Member
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
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
ledvinap
Mar 9, 2018
Contributor
@ctzsnooze : the problem is here: https://github.com/betaflight/betaflight/pull/5427/files#r173423911
@ctzsnooze : the problem is here: https://github.com/betaflight/betaflight/pull/5427/files#r173423911
ctzsnooze
Mar 9, 2018
Author
Contributor
OK sorry I have to wait for actual code suggestion, too dumb to know how to do that.
OK sorry I have to wait for actual code suggestion, too dumb to know how to do that.
ctzsnooze
Mar 9, 2018
•
Author
Contributor
Would this work better?
//2nd Dterm Lowpass Filter
if (pidProfile->dterm_lowpass2_hz == 0 || pidProfile->dterm_lowpass2_hz > pidFrequencyNyquist) {
dtermLowpass2ApplyFn = nullFilterApply;
} else {
static pt1Filter_t dtermLowpass2State[2];
dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply;
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
dtermLowpass2[axis] = &dtermLowpass2State[axis];
pt1FilterInit(dtermLowpass2[axis], pidProfile->dterm_lowpass2_hz, dT);
}
}
Would this work better?
//2nd Dterm Lowpass Filter
if (pidProfile->dterm_lowpass2_hz == 0 || pidProfile->dterm_lowpass2_hz > pidFrequencyNyquist) {
dtermLowpass2ApplyFn = nullFilterApply;
} else {
static pt1Filter_t dtermLowpass2State[2];
dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply;
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
dtermLowpass2[axis] = &dtermLowpass2State[axis];
pt1FilterInit(dtermLowpass2[axis], pidProfile->dterm_lowpass2_hz, dT);
}
}
ledvinap
Mar 9, 2018
Contributor
dtermLowpass_t contains 2-element arrays for filter state (roll+pitch)
pt1Filter_t is single state
Use static pt1Filter_t dtermLowpass2State[2];
or define type
dtermLowpass_t contains 2-element arrays for filter state (roll+pitch)
pt1Filter_t is single state
Use static pt1Filter_t dtermLowpass2State[2];
or define type
ctzsnooze
Mar 9, 2018
•
Author
Contributor
I just pushed this, hope it is now OK? Basically same structure as the preceding dterm notch.
//2nd Dterm Lowpass Filter
static pt1Filter_t dtermFilterLowpass2[2];
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++) {
dtermLowpass2[axis] = &dtermFilterLowpass2[axis];
pt1FilterInit(dtermLowpass2[axis], pidProfile->dterm_lowpass2_hz, dT);
}
}
I just pushed this, hope it is now OK? Basically same structure as the preceding dterm notch.
//2nd Dterm Lowpass Filter
static pt1Filter_t dtermFilterLowpass2[2];
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++) {
dtermLowpass2[axis] = &dtermFilterLowpass2[axis];
pt1FilterInit(dtermLowpass2[axis], pidProfile->dterm_lowpass2_hz, dT);
}
}
I can help you once I get home ;-) |
Note did not change OSD abbreviations, they are still LPF, and did not change gyro_lpf anywhere.
After disarming and allowing the gyros to settle with no motion, temporarily increase the dcmKpGain to rapidly converge on the correct attitude as indicated by the accelerometer (if present). Addresses the case of the attitude estimate becoming significantly incorrect after a crash due to high gyro rates. While the estimate will eventually converge, it does so quite slowly. The pilot may re-arm before the estimate has corrected leading to instant flips in self-leveling modes. By speeding up the convergence when disarmed we help reduce this risk.
@ctzsnooze: Thanks for this. Can you please rebase this to get rid of the conflict? @fujin: I assume that this will not interfere with #5391, since the changes are to PID filters? |
Sorry I don't know how to rebase. |
I could help and guide you if you contact me over Slack |
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.
…ints in profile Thanks DieHertz
fixed whitespace in settings.c
Note did not change OSD abbreviations, they are still LPF, and did not change gyro_lpf anywhere.
Basically copied from the DtermNotch implementation
You did a merge instead of rebase. |
I'm joining slack now - I did try to rebase, sorry. Maybe should do it all again from scratch. |
Why have you closed it? There was no need to |
Sorry I closed it because I stuffed up the merge... I have now fixed that with a new pull here: I couldn't get the slack email reply and its 1am now. Sorry of making a mess. But the new pull should be OK I fixed the one line that needed changing. Apologies. |
@ctzsnooze Just like to say thanks for this. On my ageing CC3D the motors are now cooler and the quad sounds smoother. Can't do logs to confirm but feels better. |
This PR allows the user to enable a second, independently configurable set point, PT1 type first order low-pass filter on DTerm.
This is very useful as a means of controlling because most noise in most logs arises from D, not P.
User sets dterm_lowpass_2 in CLI to the desired cut point. Double or triple the lower, standard Dterm lowpass is a good starting point. Default is 200.
The default is set to on, at twice the normal Dterm setpoint, and the default Dterm filter type is changed from biquad to PT1.
This provides greater Dterm cut than a single PT1, with twice the steepness of cut above the second setpoint. Modelling and logging shows significant reductions in higher frequency Dterm noise with only minor additional delay. Having a configurable setpoint allows the user to tune the least filtering that provides adequate noise reduction.
The biquad will provide very effective filtering but greater delay.
The following images show logs of four flights, the first panel shows that P noise is about the same on each, the second shows D noise with, from left to right, a single D PT1 at 100, and then the effect of adding a second D PT1 in series at 300, 400 and 500Hz. The second PT1 provides configurable and significant reductions in D noise from around to above its setpoint.
A spreadsheet that allows visualisation of the effect on random noise and other inputs is here https://www.dropbox.com/s/36c8ec55c3hmqoa/DualDFilters1.xlsx?dl=0