Filtering: re-work harmonic notch filter freq clamping and disable #25442

tridge · 2023-11-02T10:06:53Z

This reworks the harmonic notch setup. Major changes include:

move all clamping decisions down to one place in HarmonicNotchFilter::set_center_frequency
by default disable notch completely when freq is under 50% of min freq
added INS_HNTCH_OPTS bit to use min freq when RPM source stops (for heli governors)
make the double and triple notch clearer, note this changes the spread for harmonics, need to think if that is correct
removed code that initialised frequencies to the min freq, instead rely on update calls
allow harmonic notches to run in quadplanes in fwd flight

libraries/AP_Vehicle/AP_Vehicle.cpp

libraries/Filter/NotchFilter.cpp

andyp1per

I'm really not on board with this at all I am afraid. The structure is no easier to understand and we are making quite large sweeping changes to some very basic functionality. I don't think its necessary to make a change this large in order to address the bug in question and we have some quite subtle things going on. I don't think we should be handling options down in the filter class. If we do want to make such a large change I think the refactoring needs to be separate to the bug-fixing.

libraries/AP_ESC_Telem/AP_ESC_Telem.cpp

tridge · 2023-11-03T02:15:23Z

The structure is no easier to understand and we are making quite large sweeping changes to some very basic functionality.

it is not just that it is easier to understand, it fixes a number important limitations:

fixed wing notch filtering works for fwd motor (doesn't work at all in master)
quadplane notch filtering works when in fwd flight (doesn't work at all in master)

the ability to completely disable notches when an RPM is zero is critical for fixed wing, and is at the heart of the bug that led to this work. The logic to treat zero as INS_HNTCH_FREQ was repeated in many places in the code, and it is just the wrong this to do in most aircraft. This centralises that decision and then gives the user the ability to configure either for the existing (use min freq) behaviour or to completely disable the notch. That is a huge improvement for fixed wing, and it enables us to remove the checks in AP_Vehicle.cpp that prevented the use of notches for fixed wing.

tridge · 2023-11-03T09:24:04Z

the DynamicRpmNotches flapping test has me worried. It failed on this PR:

it passes locally, so likely just flapping again, but I think we should try and get it fixed

andyp1per · 2023-11-03T17:28:32Z

The structure is no easier to understand and we are making quite large sweeping changes to some very basic functionality.

it is not just that it is easier to understand, it fixes a number important limitations:
* fixed wing notch filtering works for fwd motor (doesn't work at all in master)

* quadplane notch filtering works when in fwd flight (doesn't work at all in master)
the ability to completely disable notches when an RPM is zero is critical for fixed wing, and is at the heart of the bug that led to this work. The logic to treat zero as INS_HNTCH_FREQ was repeated in many places in the code, and it is just the wrong this to do in most aircraft. This centralises that decision and then gives the user the ability to configure either for the existing (use min freq) behaviour or to completely disable the notch. That is a huge improvement for fixed wing, and it enables us to remove the checks in AP_Vehicle.cpp that prevented the use of notches for fixed wing.

Let's sit down and discuss because I do not agree and you are railroading in changes that require more discussion and thought. I have spent some time discussing with @lthall and I think there are other ways to do this that are certainly less invasive and less likely to introduce problems.

andyp1per · 2023-11-03T17:32:11Z

@lthall is going to write up an issue to explain the problem we are trying to solve

lthall

Before starting work on this we should accept #25355 as it fixes the original bug and the bug introduced in #25352. That gives us a minimum change fix to the existing code that can be back ported. Then we can develop this as a feature enhancement.

I have started a feature request as a place to develop the specification for this algorithm here: #25461

libraries/Filter/NotchFilter.cpp

tridge · 2024-01-29T06:18:42Z

The attenuation formula from #25461 does not seem to be correct. This PR implements it and also adds a test that graphs attenuation and phase lag with and without the adjustment

In the above graph the attenuation and lag is plotted with and without the adjustment. This graph shows:

source frequency of 35Hz
notch min freq of 50Hz
bandwidth of 25Hz
attenuation of 30dB

The purple and green lines are with the TreatLowAsMin option, which is what we do now. We see that the maximum attenuation is at 50Hz as expected.
The blue and yellow lines are without the TreatLowAsMin, which means it is using the attenuation formula to allow the tracking frequency to drop below the notch min freq. As expected we see that the maximum attenuation is at the source frequency of 35Hz. The problem is that for frequencies above 35Hz we see more phase lag than we get with the old method, which means the attenuation forumula is not achieving the "no more phase lag than at min freq" target

tridge · 2024-01-29T22:55:46Z

after some discussion with @lthall we decided to use a linear interpolation and keep the notch at the min frequency. This ensures phase lag is no worse than current code but still has the property of no glitches as the notch fades to zero.

the above graph is for:

source frequency of 35Hz
notch min freq of 50Hz
bandwidth of 25Hz
attenuation of 30dB

you can see that the source freq of 35Hz, which is below the 50Hz min freq, results in a notch at 50Hz, while reducing the attenuation

tridge · 2024-01-30T01:25:20Z

@lthall @andyp1per after the discussion on the call today this graph illustrates the result:

This is the familiy of attenuation graphs for the following settings:

INS_HNTCH_FREQ = 50
INS_HNTCH_HMNCS = 15
INS_HNTCH_BW = 25
INS_HNTCH_ATT = 30

the different graphs are for changes in the RPM source frequency. You can see the harmonics folding down into a single notch then completely disabled, which is I think exactly what we want

libraries/Filter/HarmonicNotchFilter.cpp

andyp1per

Just a few things remaining. I think the logging is basically ok but a couple of tidy-ups. There are a couple of comments that haven't been addressed and we need to resolve the FFT issue.

do all frequenct clamping in one place in set_center_frequency(). Allow for zero frequency to disable the notch. Add an option to treat inactive RPM source as min frequency

moved the claiming down into HarmonicNotchFilter

and pass params object down into HarmonicNotchFilter

fill in a parameters structure

this also fixes the uint8_t storage of a number than can be greater than 256. Max total notches in a single HarmonicNotchFilter is currently 12*16*3 for 12 ESCs, with INS_HNTCH_HMNCS=0xFFFF and triple notch

max is more than 256

use sqrt() adjustment from Leonard

this prevents duplication of the logic for the priority of the double notch vs the triple notch option

log up to 5 sources in new FCN message, or FCNS for single source

andyp1per · 2024-03-14T17:50:22Z

@tridge I setup a post-filter fft notch to test this, but I am not getting any logging for the second notch which makes me think it isn't working. Not sure whether its your PR or something else, maybe you can see what is going wrong. Here is the log:

log1.zip

tridge · 2024-03-15T00:56:49Z

I setup a post-filter fft notch to test this, but I am not getting any logging for the second notch which makes me think it isn't working.

I'm seeing logging for both notches in your log.
Here is the first notch, a multi-source ESC notch:

here is your 2nd notch, a single source FFT notch:

maybe you don't know that FTN is used for multi-source and FTNS is used for single source?
here it is with 1st and 2nd harmonic shown:

tridge added the WIP label Nov 2, 2023

tridge requested review from andyp1per, IamPete1 and lthall November 2, 2023 10:06

Hwurzburg added the WikiNeeded needs wiki update label Nov 2, 2023

IamPete1 reviewed Nov 2, 2023

View reviewed changes

libraries/AP_Vehicle/AP_Vehicle.cpp Show resolved Hide resolved

IamPete1 reviewed Nov 2, 2023

View reviewed changes

libraries/Filter/NotchFilter.cpp Outdated Show resolved Hide resolved

andyp1per requested changes Nov 2, 2023

View reviewed changes

libraries/AP_ESC_Telem/AP_ESC_Telem.cpp Outdated Show resolved Hide resolved

tridge force-pushed the pr-notch-rework branch from 66e1273 to 71a8431 Compare November 2, 2023 20:08

tridge force-pushed the pr-notch-rework branch 3 times, most recently from cf84b98 to 1c5d873 Compare November 3, 2023 22:22

tridge mentioned this pull request Nov 4, 2023

AP_InertialSensor: init all notch center frequencies #25355

Merged

lthall reviewed Nov 4, 2023

View reviewed changes

libraries/Filter/NotchFilter.cpp Outdated Show resolved Hide resolved

tridge force-pushed the pr-notch-rework branch from 3a71d41 to 80e591a Compare November 15, 2023 05:00

tridge force-pushed the pr-notch-rework branch from 80e591a to 1e8bd30 Compare January 21, 2024 20:47

tridge force-pushed the pr-notch-rework branch from 1e8bd30 to 26e264b Compare January 29, 2024 06:13

tridge force-pushed the pr-notch-rework branch from 26e264b to 297e736 Compare January 29, 2024 22:53

tridge force-pushed the pr-notch-rework branch from 297e736 to b1f164f Compare January 29, 2024 23:11

tridge added the DevCallTopic label Jan 29, 2024

CraigElder removed the DevCallTopic label Jan 29, 2024

tridge force-pushed the pr-notch-rework branch from b1f164f to 61b92a3 Compare January 30, 2024 00:47

tridge force-pushed the pr-notch-rework branch from 61b92a3 to 1b03ca2 Compare January 30, 2024 01:26

andyp1per reviewed Mar 11, 2024

View reviewed changes

libraries/Filter/HarmonicNotchFilter.cpp Outdated Show resolved Hide resolved

andyp1per reviewed Mar 11, 2024

View reviewed changes

libraries/Filter/HarmonicNotchFilter.cpp Outdated Show resolved Hide resolved

andyp1per requested changes Mar 11, 2024

View reviewed changes

tridge added 15 commits March 12, 2024 07:23

Filter: rework harmonic notch

22f2b3f

do all frequenct clamping in one place in set_center_frequency(). Allow for zero frequency to disable the notch. Add an option to treat inactive RPM source as min frequency

AP_Vehicle: removed the clamping of notch filters at vehicle level

d24c2be

moved the claiming down into HarmonicNotchFilter

AP_InertialSensor: removed zero checks and clamping on notch filters

210b2a1

and pass params object down into HarmonicNotchFilter

Filter: fixed test suite

1559dcd

fill in a parameters structure

Filter: fixed filter expansion for other than motors based RPM sources

ffc6590

this also fixes the uint8_t storage of a number than can be greater than 256. Max total notches in a single HarmonicNotchFilter is currently 12*16*3 for 12 ESCs, with INS_HNTCH_HMNCS=0xFFFF and triple notch

AP_InertialSensor: fixed num_filters to be uint16_t

5706eab

max is more than 256

Filter: adjust attenuation properly at low frequencies

4b3780e

use sqrt() adjustment from Leonard

Filter: added test for attenuation adjustment

210ea26

AP_InertialSensor: use num_composite_notches()

f2c9f5a

this prevents duplication of the logic for the priority of the double notch vs the triple notch option

AP_Logger: added VER.FV for filter version

f38f154

AP_InertialSensor: added comments

b4a493f

Filter: added logging of notch centers and 1st harmonic

088e1d3

log up to 5 sources in new FCN message, or FCNS for single source

AP_InertialSensor: call logging function for notch centers and harmonics

d6a9627

AP_InertialSensor: adjust logging descriptions

07cd9f8

Filter: log up to 6 harmonics

4a921e1

tridge force-pushed the pr-notch-rework branch from 417b4ea to 4a921e1 Compare March 11, 2024 20:23

Filter: use a define for nyquist cutoff

f45fd83

tridge added DevCallEU and removed DevCallEU labels Mar 13, 2024

andyp1per approved these changes Mar 15, 2024

View reviewed changes

tridge merged commit 148c176 into ArduPilot:master Mar 15, 2024
91 checks passed

andyp1per mentioned this pull request May 2, 2024

AP_InertialSensor: ensure that notches get updated while converging #26958

Merged

Hwurzburg mentioned this pull request Jul 11, 2024

Filter: add "source" to option 5 #27535

Merged

andyp1per mentioned this pull request Sep 7, 2024

Fix Static Notches #28034

Merged

IamPete1 mentioned this pull request Sep 7, 2024

FilterReview: add support for new version of AP notch logging ArduPilot/WebTools#184

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Filtering: re-work harmonic notch filter freq clamping and disable #25442

Filtering: re-work harmonic notch filter freq clamping and disable #25442

tridge commented Nov 2, 2023 •

edited

Loading

andyp1per left a comment

tridge commented Nov 3, 2023

tridge commented Nov 3, 2023 •

edited

Loading

andyp1per commented Nov 3, 2023

andyp1per commented Nov 3, 2023

lthall left a comment

tridge commented Jan 29, 2024 •

edited

Loading

tridge commented Jan 29, 2024 •

edited

Loading

tridge commented Jan 30, 2024

andyp1per left a comment

andyp1per commented Mar 14, 2024

tridge commented Mar 15, 2024

Filtering: re-work harmonic notch filter freq clamping and disable #25442

Filtering: re-work harmonic notch filter freq clamping and disable #25442

Conversation

tridge commented Nov 2, 2023 • edited Loading

andyp1per left a comment

Choose a reason for hiding this comment

tridge commented Nov 3, 2023

tridge commented Nov 3, 2023 • edited Loading

andyp1per commented Nov 3, 2023

andyp1per commented Nov 3, 2023

lthall left a comment

Choose a reason for hiding this comment

tridge commented Jan 29, 2024 • edited Loading

tridge commented Jan 29, 2024 • edited Loading

tridge commented Jan 30, 2024

andyp1per left a comment

Choose a reason for hiding this comment

andyp1per commented Mar 14, 2024

tridge commented Mar 15, 2024

tridge commented Nov 2, 2023 •

edited

Loading

tridge commented Nov 3, 2023 •

edited

Loading

tridge commented Jan 29, 2024 •

edited

Loading

tridge commented Jan 29, 2024 •

edited

Loading