New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Second PT1 on DTerm #5427

Closed
wants to merge 38 commits into
base: master
from

Conversation

@ctzsnooze
Contributor

ctzsnooze commented Mar 9, 2018

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.

amount of p in each trace

amount of d in each trace

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

larryho5 and others added some commits Mar 7, 2018

ctzsnooze
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.
Merge pull request #5407 from larryho5/betaflight_3_3_0_R3
- 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];

This comment has been minimized.

@DieHertz

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? :)

This comment has been minimized.

@fujin

fujin Mar 9, 2018

Member

lowpass/lowpass2 is also the naming I went with in the dual-stage PR.

This comment has been minimized.

@fujin

fujin Mar 9, 2018

Member

makes me wonder if we should be using FAST_RAM in gyroSensor for similar storage as well...

This comment has been minimized.

@DieHertz

DieHertz Mar 9, 2018

Member

Don't dive into that, I'm working on it as a part of F7 optimization branch

This comment has been minimized.

@ctzsnooze

ctzsnooze Mar 9, 2018

Contributor

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

This comment has been minimized.

@DieHertz

DieHertz Mar 9, 2018

Member

Should be dterm_lowpass2_type

This comment has been minimized.

@ctzsnooze

ctzsnooze Mar 9, 2018

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'

This comment has been minimized.

@DieHertz

DieHertz Mar 9, 2018

Member

No underscore before 2, please, to conform with gyro lowpass and notch

This comment has been minimized.

@ctzsnooze

ctzsnooze Mar 9, 2018

Contributor

done

@@ -216,6 +219,18 @@ void pidInitFilters(const pidProfile_t *pidProfile)
} else {
dtermNotchFilterApplyFn = nullFilterApply;
}
//extra Dterm PT1 filter
static pt1Filter_t ExtraPt1FilterD;

This comment has been minimized.

@fujin

fujin Mar 9, 2018

Member

remove 'extra',

will we ever allow others than pt1?

This comment has been minimized.

@ctzsnooze

ctzsnooze Mar 9, 2018

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?

This comment has been minimized.

@fujin

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.

This comment has been minimized.

@ctzsnooze

ctzsnooze Mar 9, 2018

Contributor

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

This comment has been minimized.

@fujin

fujin Mar 9, 2018

Member

I'd been standardizing names and suggest the following:

dterm_lowpass_hz
dterm_lowpass2_hz

This comment has been minimized.

@ctzsnooze

ctzsnooze Mar 9, 2018

Contributor

yes no problem

This comment has been minimized.

@ctzsnooze

ctzsnooze Mar 9, 2018

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.

This comment has been minimized.

@DieHertz

DieHertz Mar 9, 2018

Member

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

This comment has been minimized.

@fujin

fujin Mar 9, 2018

Member

whitespace/indentation here

This comment has been minimized.

@ctzsnooze

ctzsnooze Mar 9, 2018

Contributor

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;

This comment has been minimized.

@fujin

fujin Mar 9, 2018

Member

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

This comment has been minimized.

@fujin

fujin Mar 9, 2018

Member

let's default to 0 (off) for now.

This comment has been minimized.

@ctzsnooze

ctzsnooze Mar 9, 2018

Contributor

OK.

But if feedback is very positive during release candidate stage, the advantage of providing a suitable default is considerable for new users.

This comment has been minimized.

@fujin

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).

This comment has been minimized.

@ctzsnooze

ctzsnooze Mar 9, 2018

Contributor

no problem

.dterm_notch_hz = 260,
.dterm_notch_cutoff = 160,
.dterm_filter_type = FILTER_BIQUAD,
.dterm_filter_type = FILTER_PT1,

This comment has been minimized.

@fujin

fujin Mar 9, 2018

Member

changing defaults is generally not recommended at the same time as implementation

This comment has been minimized.

@DieHertz

DieHertz Mar 9, 2018

Member

I second that, let's collect more report first that for most people PT1+PT1 are better than Biquad

This comment has been minimized.

@ctzsnooze

ctzsnooze Mar 9, 2018

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.

@ledvinap

This comment has been minimized.

Contributor

ledvinap commented Mar 9, 2018

@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 ...

@fujin

This comment has been minimized.

Member

fujin commented Mar 9, 2018

Looks really promising! I'm excited to test this both in combination with and instead of dual-stage gyro LPF's. Great work.

ctzsnooze
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
@ctzsnooze

This comment has been minimized.

Contributor

ctzsnooze commented Mar 9, 2018

@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

This comment has been minimized.

@DieHertz

DieHertz Mar 9, 2018

Member

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

This comment has been minimized.

@DieHertz

DieHertz Mar 9, 2018

Member

I suggested renaming it in CLI as well to make it uniform with gyro_lowpass_hz

@DieHertz

This comment has been minimized.

Member

DieHertz commented Mar 9, 2018

Thanks, there remains the point about underscore, and I think making lowpass naming conventions for gyro and Dterm uniform is beneficial.

@ctzsnooze

This comment has been minimized.

Contributor

ctzsnooze commented Mar 9, 2018

Thanks for all your feedback Fujin and DieHertz! Definitely more readable.
I think I've implemented nearly all your suggestions, uploaded now, please check and see if anything else would help.
@DieHertz: For now I've left the 'value' type variable names in the form '_lpf_hz' to match current CLI nomenclature. Changing them all would be a big change, people updating would get non-matching fields from CLI dumps. Perhaps that could be separate pull request.
@fujin: there is still a bit of whitespace problem in some places, I'll fix that on the next push.

{ "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) },

This comment has been minimized.

@DieHertz

DieHertz Mar 9, 2018

Member

Looks like there's a redundant indent :)

@ctzsnooze

This comment has been minimized.

Contributor

ctzsnooze commented Mar 9, 2018

@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

@ctzsnooze

This comment has been minimized.

Contributor

ctzsnooze commented Mar 9, 2018

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.

ctzsnooze
fix whitespace
fixed whitespace in settings.c
@DieHertz

Good job, will have a look soon

@DieHertz

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;

This comment has been minimized.

@DieHertz

DieHertz Mar 9, 2018

Member

This falls short of using the same state for all axes, just like @fujin's PR before

}
}
static dtermLowpass_t dtermLowpassUnion;

This comment has been minimized.

@DieHertz

DieHertz Mar 9, 2018

Member

Same here, this effectively makes state shared for all axes

This comment has been minimized.

@ctzsnooze

ctzsnooze Mar 9, 2018

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.

This comment has been minimized.

@ctzsnooze

ctzsnooze Mar 9, 2018

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.

This comment has been minimized.

@DieHertz

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.

This comment has been minimized.

@DieHertz

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

This comment has been minimized.

This comment has been minimized.

@ctzsnooze

ctzsnooze Mar 9, 2018

Contributor

OK sorry I have to wait for actual code suggestion, too dumb to know how to do that.

This comment has been minimized.

@ctzsnooze

ctzsnooze Mar 9, 2018

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

This comment has been minimized.

@ledvinap

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

This comment has been minimized.

@ctzsnooze

ctzsnooze Mar 9, 2018

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);
    }        
}
@DieHertz

This comment has been minimized.

Member

DieHertz commented Mar 9, 2018

I can help you once I get home ;-)

ctzsnooze
change lpf to lowpass where appropriate elsewhere
Note did not change OSD abbreviations, they are still LPF, and did not
change gyro_lpf anywhere.

@mikeller mikeller added this to the Betaflight v3.4 milestone Mar 14, 2018

IMU attitude estimate accelerated convergence after disarming (#5425)
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.
@mikeller

This comment has been minimized.

Member

mikeller commented Mar 15, 2018

@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?

@fujin

This comment has been minimized.

Member

fujin commented Mar 15, 2018

@mikeller yes, should not conflict with #5391

@ctzsnooze

This comment has been minimized.

Contributor

ctzsnooze commented Mar 15, 2018

Sorry I don't know how to rebase.

@DieHertz

This comment has been minimized.

Member

DieHertz commented Mar 15, 2018

I could help and guide you if you contact me over Slack

ctzsnooze added some commits Mar 7, 2018

ctzsnooze
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.
ctzsnooze
fix whitespace
fixed whitespace in settings.c
ctzsnooze
change lpf to lowpass where appropriate elsewhere
Note did not change OSD abbreviations, they are still LPF, and did not
change gyro_lpf anywhere.
ctzsnooze
second attempt at a simple PT1 implementation
Basically copied from the DtermNotch implementation
@DieHertz

This comment has been minimized.

Member

DieHertz commented Mar 15, 2018

You did a merge instead of rebase.

@ctzsnooze

This comment has been minimized.

Contributor

ctzsnooze commented Mar 15, 2018

I'm joining slack now - I did try to rebase, sorry. Maybe should do it all again from scratch.

@ctzsnooze ctzsnooze closed this Mar 15, 2018

@ctzsnooze ctzsnooze deleted the ctzsnooze:Second-Dterm-PT1 branch Mar 15, 2018

@DieHertz

This comment has been minimized.

Member

DieHertz commented Mar 15, 2018

Why have you closed it? There was no need to

@ctzsnooze ctzsnooze restored the ctzsnooze:Second-Dterm-PT1 branch Mar 15, 2018

@ctzsnooze ctzsnooze deleted the ctzsnooze:Second-Dterm-PT1 branch Mar 15, 2018

@ctzsnooze ctzsnooze restored the ctzsnooze:Second-Dterm-PT1 branch Mar 15, 2018

@ctzsnooze ctzsnooze referenced this pull request Mar 15, 2018

Merged

Second dterm pt1 #5458

@ctzsnooze

This comment has been minimized.

Contributor

ctzsnooze commented Mar 15, 2018

Sorry I closed it because I stuffed up the merge... I have now fixed that with a new pull here:

#5458

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.

@atomiclama

This comment has been minimized.

Contributor

atomiclama commented Mar 15, 2018

@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.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment