Second PT1 on DTerm #5427

ctzsnooze · 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.

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

DieHertz · Mar 9, 2018

DieHertz requested changes Mar 9, 2018

View changes

DieHertz · Mar 9, 2018

src/main/flight/pid.c

@@ -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];


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

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

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

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

OK will change to to dtermLowpass and dtermLowpass2

DieHertz · Mar 9, 2018

src/main/interface/settings.c

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


Should be dterm_lowpass2_type

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'

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

fujin · Mar 9, 2018

fujin reviewed Mar 9, 2018

View changes

fujin · Mar 9, 2018

src/main/flight/pid.c

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


remove 'extra',

will we ever allow others than pt1?

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?

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.

Hmm it has to be different from the other names for the same thing

fujin · Mar 9, 2018

fujin reviewed Mar 9, 2018

View changes

fujin · Mar 9, 2018

src/main/flight/pid.h

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


I'd been standardizing names and suggest the following:

dterm_lowpass_hz
dterm_lowpass2_hz

yes no problem

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.

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

fujin · Mar 9, 2018

fujin reviewed Mar 9, 2018

View changes

fujin · Mar 9, 2018

src/main/flight/pid.c

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


whitespace/indentation here

fixed (I think, pls check)

fujin · Mar 9, 2018

fujin reviewed Mar 9, 2018

View changes

fujin · Mar 9, 2018

src/main/flight/pid.c

+    //extra Dterm PT1 filter
+	static pt1Filter_t ExtraPt1FilterD;
+	if (pidProfile->dterm_lpf_2_hz == 0 || pidProfile->dterm_lpf_2_hz > pidFrequencyNyquist) {
+		dtermLpf2ApplyFn = nullFilterApply;


better to set this regardless and then do the following block in the if(){} instead of if/else.

fujin · Mar 9, 2018

fujin reviewed Mar 9, 2018

View changes

fujin · Mar 9, 2018

src/main/flight/pid.c

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


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

OK.

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

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

fujin · Mar 9, 2018

fujin reviewed Mar 9, 2018

View changes

fujin · Mar 9, 2018

src/main/flight/pid.c

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


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

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

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

DieHertz · Mar 9, 2018

DieHertz requested changes Mar 9, 2018

View changes

DieHertz · Mar 9, 2018

src/main/flight/pid.c

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


There's still an underscore before 2

DieHertz · Mar 9, 2018

src/main/flight/pid.h

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


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

DieHertz · 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 · 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.

DieHertz · Mar 9, 2018

DieHertz reviewed Mar 9, 2018

View changes

DieHertz · Mar 9, 2018

DieHertz reviewed Mar 9, 2018

View changes

DieHertz · Mar 9, 2018

src/main/interface/settings.c

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


Looks like there's a redundant indent :)

ctzsnooze · 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 · Mar 9, 2018

ctzsnooze dismissed DieHertz’s stale review via `1755544` Mar 9, 2018

ctzsnooze · 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.

DieHertz · Mar 9, 2018

DieHertz reviewed Mar 9, 2018

View changes

Good job, will have a look soon

DieHertz · Mar 9, 2018

DieHertz reviewed Mar 9, 2018

View changes

Just noticed the static state, was it added by your changes or present before?

DieHertz · Mar 9, 2018

src/main/flight/pid.c

+	} else {
+		dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply;
+		for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
+			dtermLowpass2[axis] = &dtermLowpass2State;


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

DieHertz · Mar 9, 2018

src/main/flight/pid.c

+		}
+	}
+
+    static dtermLowpass_t dtermLowpassUnion;


Same here, this effectively makes state shared for all axes

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.

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.

Yaw is just one axis, so it's not shared by definition.
In your code roll&pitch get shared Dterm state for lowpass.

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

@ctzsnooze : the problem is here: https://github.com/betaflight/betaflight/pull/5427/files#r173423911

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

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

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

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 · Mar 9, 2018

I can help you once I get home ;-)

mikeller · 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?

mikeller · Mar 15, 2018

mikeller reviewed Mar 15, 2018

View changes

fujin · Mar 15, 2018

@mikeller yes, should not conflict with #5391

ctzsnooze · Mar 15, 2018

Sorry I don't know how to rebase.

DieHertz · Mar 15, 2018

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

ctzsnooze · Mar 15, 2018

ctzsnooze dismissed mikeller’s stale review via `b849b24` Mar 15, 2018

DieHertz · Mar 15, 2018

You did a merge instead of rebase.

ctzsnooze · Mar 15, 2018

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

DieHertz · Mar 15, 2018

Why have you closed it? There was no need to

ctzsnooze · 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 · 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.

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

d3014ff

Remove underscore in lowpass_2, add hz to setpoint for lowpass
Thanks DieHertz

c3f192e

completed replacing lpf with lowpass, added _hz to all lowpass set po…
…ints in profile Thanks DieHertz

1755544

ctzsnooze dismissed DieHertz’s stale review via 1755544 Mar 9, 2018

fix whitespace
fixed whitespace in settings.c

decc92f

whitespace attempt #57

72676fb

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

d7fc099

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

mikeller added the Rebase Required label Mar 14, 2018

Uniformly reduce UART buffer sizes to 128 bytes (#5432)

6056ee1

ctzsnooze dismissed mikeller’s stale review via b849b24 Mar 15, 2018

ctzsnooze closed this Mar 15, 2018

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

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

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

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

ctzsnooze referenced this pull request Mar 15, 2018
Merged
Second dterm pt1 #5458

betaflight/betaflight

Join GitHub today

Second PT1 on DTerm #5427

Conversation

ctzsnooze commented Mar 9, 2018

larryho5 and others added some commits Mar 7, 2018

DieHertz requested changes Mar 9, 2018 View changes

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

fujin reviewed Mar 9, 2018 View changes

This comment has been minimized.

This comment has been minimized.

ctzsnooze Mar 9, 2018 • edited

This comment has been minimized.

This comment has been minimized.

fujin reviewed Mar 9, 2018 View changes

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

fujin reviewed Mar 9, 2018 View changes

This comment has been minimized.

This comment has been minimized.

fujin reviewed Mar 9, 2018 View changes

This comment has been minimized.

fujin reviewed Mar 9, 2018 View changes

This comment has been minimized.

This comment has been minimized.

ctzsnooze Mar 9, 2018 • edited

This comment has been minimized.

This comment has been minimized.

fujin reviewed Mar 9, 2018 View changes

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

ctzsnooze Mar 9, 2018 • edited

This comment has been minimized.

ledvinap commented Mar 9, 2018

This comment has been minimized.

fujin commented Mar 9, 2018

This comment has been minimized.

ctzsnooze commented Mar 9, 2018

DieHertz requested changes Mar 9, 2018 View changes

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

DieHertz commented Mar 9, 2018

This comment has been minimized.

ctzsnooze commented Mar 9, 2018

DieHertz reviewed Mar 9, 2018 View changes

DieHertz reviewed Mar 9, 2018 View changes

This comment has been minimized.

This comment has been minimized.

ctzsnooze commented Mar 9, 2018

ctzsnooze dismissed DieHertz’s stale review via 1755544 Mar 9, 2018

This comment has been minimized.

ctzsnooze commented Mar 9, 2018

DieHertz reviewed Mar 9, 2018 View changes

DieHertz reviewed Mar 9, 2018 View changes

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

ctzsnooze Mar 9, 2018 • edited

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

DieHertz Mar 9, 2018 • edited

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

ctzsnooze Mar 9, 2018 • edited

This comment has been minimized.

This comment has been minimized.

DieHertz requested changes Mar 9, 2018

View changes

fujin reviewed Mar 9, 2018

View changes

ctzsnooze Mar 9, 2018 •

edited

fujin reviewed Mar 9, 2018

View changes

fujin reviewed Mar 9, 2018

View changes

fujin reviewed Mar 9, 2018

View changes

fujin reviewed Mar 9, 2018

View changes

ctzsnooze Mar 9, 2018 •

edited

fujin reviewed Mar 9, 2018

View changes

ctzsnooze Mar 9, 2018 •

edited

DieHertz requested changes Mar 9, 2018

View changes

DieHertz reviewed Mar 9, 2018

View changes

DieHertz reviewed Mar 9, 2018

View changes

ctzsnooze dismissed DieHertz’s stale review via `1755544` Mar 9, 2018

DieHertz reviewed Mar 9, 2018

View changes

DieHertz reviewed Mar 9, 2018

View changes

ctzsnooze Mar 9, 2018 •

edited

DieHertz Mar 9, 2018 •

edited

ctzsnooze Mar 9, 2018 •

edited

ctzsnooze Mar 9, 2018 •

edited

mikeller reviewed Mar 15, 2018

View changes

ctzsnooze dismissed mikeller’s stale review via `b849b24` Mar 15, 2018