Skip to content
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

Implement throttle based dynamic gyro and dterm filters. #6943

Merged
merged 2 commits into from Oct 27, 2018

Conversation

Projects
None yet
9 participants
@kmitchel
Copy link
Contributor

kmitchel commented Oct 16, 2018

This PR dynamically adjusts the gyro and d-term lowpass filters by passing the current throttle value thru a configurable cubic function. This PR currently incorporates #6933. I still need to make it dual gyro safe. Any help/guidance improving this PR would be appreciated.

@ctzsnooze

This comment has been minimized.

Copy link
Contributor

ctzsnooze commented Oct 17, 2018

There are two fundamental changes to filter configuration provided by this PR.

1. Changes to dTerm lowpass filtering options, including changed defaults.

The option of using the biquad filter type on the second dTerm lowpass filter, which previously was available only as an option only on the first of the two D lowpass filters, provides several small improvements in D filtering. By default we propose enabling both dTerm lowpass filters in the steeper cut biquad form, with higher default set points than with the PT1's used previously.

The reason for this change is to enhance the amount of D being passed by the filters at 50-100Hz, the typical prop wash frequency range, while strongly restricting noise transmission above 180-200Hz. The overall delay position with these defaults is about the same as before, but D is slightly more effective against propwash than before, and the stronger attenuation above 180-200Hz seems to result in cooler motors.

2. Providing an optional upward shift of the first gyro and dterm cutoff frequencies as throttle increases.

This allows setting of a relatively low initial lowpass value to apply while arming and up to hover point, for that filter frequency to increase as throttle increases to a maximum value.

The advantages are:

  • a relatively low initial lowpass value removes higher harmonics from the noise spectrum at low throttle, allowing the dynamic notch filter to drop quite low and more effectively control motor noise. Otherwise, without significant lowpass filtering at idle, the dynamic notch filter can end up going too high.

  • a relatively high lowpass value will reduce filter delay at high throttle, and allows enough noise through at the high end for the dynamic notch to properly track and better eliminate the high motor generated noise peak. Even if the dynamic gyro filter max is set as high as 400 or 500, motor temperatures often are not as bad as expected, while filter delay is markedly less.

  • dTerm seems to be the more effective of the PID components that deal with prop wash. As mentioned above, dual biquad filters let more D through at 40-100Hz than dual PT1's at half the cutoff values. The lowest of the two dTerm biquads is responsible for most of the D delay. This code optionally allows the first biquad to go higher with increasing throttle, reducing D delay, and improving prop wash. Motor temps should be closely checked when moving the defaults upwards. The least delay is with both D biquad filters as high as possible. The defaults are reasonably conservative in this regard.

Notes:

  • the dynamic notch FFT should be set to track the after_static gyro data. The defaults are intended to 'shape' the incoming noise to best allow the FFT to track the noise spectrum for the motor peak. If the first gyro lowpass is made dynamic, the name after_static is, perhaps, misleading. Perhaps 'after_lowpass' might be a better name. This would ensure the new default position of the FFT would apply on clean installs and on copy/paste updates.

  • these dynamic filters are intentionally not driven by FFT data. We learned from plasmatree logs that most quads have a characteristic noise curve as rpm increases, and we found a cubic function could be constructed that emulated this curve very effectively from throttle input alone. This allowed dynamic pre-filtering of the gyro noise in such a way that the FFT could much better track and remove the motor peak across the full range of rpm. The FFT change itself is delayed first by needing the motors to change RPM, and second by delays in calculating and smoothing the FFT output. In contrast, a value derived immediately from throttle has essentially no delay. This means that when the lowpass filters move dynamically upwards, that change happens well before the motors spin up, reducing filter delay very quickly. This also encourages the FFT to move up more quickly, and overall the filtering delay now can be reduced very quickly on throttling up. The combination of these effects is what improves prop wash handling on throttling up.

Settings:

The dynamic lowpass stays at a constant value until throttle exceeds the user configurable 'idle' value. Typically the idle value would be set at the throttle position at which the quad hovers, to keep filtering constant at the standard low-pass value from arming until take-off. The defaults are 20% throttle and can be independently adjusted upwards or downwards. If set to 50% say, there will be no increase in lowpass filtering until 50% throttle is reached. This provides some configurability. We need feedback on whether any user really needs to change these values, and whether the same threshold could be the same for both gyro and D, rather than having separate independent values.

The max gyro value can be set around 500 for 3" or smaller quads. The default is OK for slower 5" quads, but high-revving 5" quads (e.g. 6S) may need 400 or higher. These values assume the presence of a second fixed PT1 at 300 to maintain very high frequency noise control. With biquad low passes on D it is surprising how little gyro filtering is required on many quads. The best way to determine an appropriate MAX value is to log with debug FFT-FREQ, and check that if there is a clean high frequency motor peak, that the FFT is tracking it cleanly (not dropping low). As a simple guide, check motor temperature after high throttle flying.

The min gyro setting determines gyro filter delay at low throttle. The lower the setting, the better the filtering. Values around 100Hz allow the FFT to track motor noise at low rpm that can cause jello, but may be lower than needed on some quads. Higher values will let more high frequency noise through, causing the FFT to go high and not eliminate the hover motor noise peak. This may worsen jello but may improve off-throttle propwash a little.

The Dterm settings are a little conservative. For 'clean' quads, higher values (e.g. min 100, max 250, fixed 150) can work well and will improve prop wash. Take care when lifting D filtering since not enough D filtering can make motors very hot. Inadequate minimum D filtering can cause motor grinding, frame and prop flex, and climbing vertically on zero throttle.

@RipperDrone

This comment has been minimized.

Copy link

RipperDrone commented Oct 17, 2018

Could the throttle threshold value (20%) to trigger lowpass filtering be derived from the hover limit which is named Throttle_MID in BFC?

@ctzsnooze

This comment has been minimized.

Copy link
Contributor

ctzsnooze commented Oct 18, 2018

Throttle_mid is used by TPA as the point above which P and D will start to be attenuated, assuming some TPA is set. For users of TPA this is often the point at which P and D noise at high throttle becomes a problem; often its not hover point. Some other users start from a low value. Most likely we should keep the two independent for the time being.
I'd appreciate your feedback on whether, after testing, it's actually needed to have separate idle points for gyro and D, and even whether idle point needs to be user-configurable.

@ctzsnooze

This comment has been minimized.

Copy link
Contributor

ctzsnooze commented Oct 18, 2018

The green line on this graph shows the nature of the curve we are using to shift the lowpass filters, with a 20% idle point. Other lines show what it would be like if idle point was set to 0, and if we only had a linear 'curve'. The curve very closely matches typical throttle vs rpm relationships on a range of quads.
screen shot 2018-10-18 at 12 31 53
This is a similar curve with idle at 15% overlaid on a motor trace to show good agreement between the two:
dynfiltoverlaid

@ctzsnooze ctzsnooze requested review from DieHertz and mikeller Oct 18, 2018

@ctzsnooze

This comment has been minimized.

Copy link
Contributor

ctzsnooze commented Oct 18, 2018

throttle blip

Log showing how lowpass filter moves immediately with throttle, ahead of motors, to minimise delay/propwash, whereas the FFT based notch filter stays low until the motors rev up, and then some calculation delay. Notch filter needs to track actual motor peak, but lowpass doesn't, so its good to bring it up in advance of the motors.

@RipperDrone

This comment has been minimized.

Copy link

RipperDrone commented Oct 18, 2018

Throttle_mid is used by TPA as the point above which P and D will start to be attenuated, assuming some TPA is set. For users of TPA this is often the point at which P and D noise at high throttle becomes a problem; often its not hover point. Some other users start from a low value. Most likely we should keep the two independent for the time being.

r u sure? There is a different setting for TPA threshold, defaults to 1650, isn't it? Whereas THROTTLE_MID is used to set the expo curve for throttle properly. Usually it's set to 50%, but on light builds it can be as low as 20%. Therefore, it seemed quite natural to me to also use this very variable for your purpose of triggering lowpass filtering beyond a 'hovering around' throttle threshold... anyway, your call - and maybe I got s/t wrong on my part...

@spatzengr

This comment has been minimized.

Copy link

spatzengr commented Oct 18, 2018

A short review of the benefits of the Dynamic LPFs PR: https://youtu.be/W1aN_uQ7_W8

@kmitchel kmitchel force-pushed the kmitchel:dynamic_lowpass branch 10 times, most recently from e481108 to 02faa30 Oct 18, 2018

@mikeller mikeller added this to the 4.0 milestone Oct 23, 2018


#ifdef USE_DYN_LPF
if (pidProfile->dyn_lpf_dterm_idle > 0 && pidProfile->dyn_lpf_dterm_max_hz > 0) {
if (pidProfile->dterm_lowpass_hz > 0 ){

This comment has been minimized.

@mikeller

mikeller Oct 23, 2018

Member

Missing space before {.

if (pidProfile->dyn_lpf_dterm_idle > 0 && pidProfile->dyn_lpf_dterm_max_hz > 0) {
if (pidProfile->dterm_lowpass_hz > 0 ){
dynLpfMin = pidProfile->dterm_lowpass_hz;
if (pidProfile->dterm_filter_type == FILTER_PT1) {

This comment has been minimized.

@mikeller

mikeller Oct 23, 2018

Member

Should use a switch statement.

@@ -125,12 +125,12 @@ void resetPidProfile(pidProfile_t *pidProfile)
.pidSumLimit = PIDSUM_LIMIT,
.pidSumLimitYaw = PIDSUM_LIMIT_YAW,
.yaw_lowpass_hz = 0,
.dterm_lowpass_hz = 100, // dual PT1 filtering ON by default
.dterm_lowpass2_hz = 200, // second Dterm LPF ON by default
.dterm_lowpass_hz = 150, // dual PT1 filtering ON by default

This comment has been minimized.

@mikeller

mikeller Oct 23, 2018

Member

Are these defaults desirable for targets that do not have USE_DYN_LPF? If not they should be made conditional.

@@ -214,3 +216,6 @@ float pidLevel(int axis, const pidProfile_t *pidProfile,
const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint);
float calcHorizonLevelStrength(void);
#endif
#ifdef USE_DYN_LPF

This comment has been minimized.

@mikeller

mikeller Oct 23, 2018

Member

Header files normally don't use conditionals - there is little benefit to this.

Show resolved Hide resolved src/main/sensors/gyro.c Outdated

@kmitchel kmitchel force-pushed the kmitchel:dynamic_lowpass branch from 02faa30 to 0cb490d Oct 24, 2018

@McGiverGim

This comment has been minimized.

Copy link
Member

McGiverGim commented Oct 24, 2018

I see, as commented by mikeller that you have changed the "fixed" filter value. If I'm not wrong, then if an user deactivates the dynamic lpf filters (putting a zero the idle and max values I think), then the "default" filters will be higher in value than usual (less filtering). Is that true? Is that intended?

If dynamic lpf filters will be an enabled default feature, I don't see it too bad, but if not, this can be dangerous.

@kmitchel

This comment has been minimized.

Copy link
Contributor Author

kmitchel commented Oct 24, 2018

@ctzsnooze correct me if I'm wrong. With the cascading dterm biquads, there is an improved tolerance for gyro noise, so we can have the increased gyro lpf. The "baseline" settings should be acceptable for most quads... shouldn't matter if the dynamic stuff is on or not. Going forward I personally would like to see this feature enabled going into RC, so that it can be exposed to a larger test pool. I would expect setting to be refined.

Show resolved Hide resolved src/main/flight/pid.c
Show resolved Hide resolved src/main/sensors/gyro.h Outdated
Show resolved Hide resolved src/main/target/common_pre.h Outdated
Show resolved Hide resolved src/main/flight/pid.c Outdated
Show resolved Hide resolved src/main/flight/pid.c Outdated
Show resolved Hide resolved src/main/flight/pid.c Outdated
Show resolved Hide resolved src/main/flight/pid.c Outdated
Show resolved Hide resolved src/main/flight/pid.c Outdated
Show resolved Hide resolved src/main/flight/pid.c Outdated
Show resolved Hide resolved src/main/flight/pid.c Outdated
Show resolved Hide resolved src/main/sensors/gyro.c Outdated
Show resolved Hide resolved src/main/sensors/gyro.c Outdated
Show resolved Hide resolved src/main/sensors/gyro.c Outdated
Show resolved Hide resolved src/main/sensors/gyro.c Outdated
Show resolved Hide resolved src/main/flight/pid.c Outdated
Show resolved Hide resolved src/main/sensors/gyro.c Outdated

@kmitchel kmitchel force-pushed the kmitchel:dynamic_lowpass branch from 0cb490d to d41dd4b Oct 26, 2018

@kmitchel kmitchel force-pushed the kmitchel:dynamic_lowpass branch from d41dd4b to ff8d1a8 Oct 26, 2018

@kmitchel

This comment has been minimized.

Copy link
Contributor Author

kmitchel commented Oct 26, 2018

@jirif Ty for review,

@mikeller mikeller merged commit 8609346 into betaflight:master Oct 27, 2018

@jcutcher01

This comment has been minimized.

Copy link

jcutcher01 commented Oct 28, 2018

Man this is great stuff guys. This dynamic filtering coupled with the dynamic notch is perfect.

@spatzengr

This comment has been minimized.

Copy link

spatzengr commented Oct 29, 2018

@jcutcher01 , dynamic LPFs is actually tied to throttle to reduce latency of them moving up immediatly to reduce prop wash.

Best debug to see things in action is DEBUG_MODE=FFT_FREQ allows you to track the LPFs and Dynamic Notch:
debug[0] = Dyn Notch center on Roll
debug[1] = Gyro Lowpass1 cutoff on Roll
debug[2] = filtered noise on Roll (BUT before the Dynamic Notch filtering)
debug[3] = raw unfiltered noise on Roll.
(see Gyro trace for fully filtered; of course)

Also for a brief on it: https://youtu.be/nWPgmZXNGJA

@jcutcher01

This comment has been minimized.

Copy link

jcutcher01 commented Oct 29, 2018

Thanks. I am not into BB logging and debugging so I am waiting for your awesome video to show it and cover the topic.

@controlflow

This comment has been minimized.

Copy link

controlflow commented Oct 29, 2018

Sorry me my noob question: is the source throttle value is purely RC throttle channel based or is it based on some kind of base throttle value after air mode logic being applied? I wonder what happens with lowpass on propwash at zero throttle, when air mode is actively bumping up the baseline throttle value...

@kmitchel kmitchel deleted the kmitchel:dynamic_lowpass branch Dec 28, 2018

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.