Add Notch Filter to Rate Controller Feed Forward

[MattKear]

This is very much WIP and still full of hacky implementation/debugging. Opening PR purely to socialise the idea.

Credit to Ryan Beall as this is his idea.

The purpose of this filter is to prevent the flight controller from exciting structural resonances. This video covers the motivation behind why this should be of benefit: https://youtu.be/b_8v8scghh8?t=935

I am hoping this will be of use to multi-rotors that have long flexible landing gear and tend to hit a resonance excited by the yaw rate controller when sat on ground and slowly advancing the throttle to take off. I am also hoping this will be useful to VTOL planes that excite a vibration mode in the wings and/or tail when in the hover.

This is just a basic implementation to begin with whilst I am still experimenting and testing.

I have added a simple ground resonance model into copter SITL.

Thus far, based on SITL testing this implementation does not appear to be doing much to prevent ground resonance. This could either be due to my bad SITL model, the way I have implemented in AP, or it just may not be an effective method to prevent this coupling. Currently working on IRL testing to understand what is going on.

Stuff that I am still planning to do:

• [ ] Make the params updatable at run time
• [ ] Add a Lua script example to be able to switch off the notch when in flight as an example for the ground resonance use case.
• [ ] Define out for 1MB boards

Comments and ideas welcome.

[andyp1per]

Hey @Gone4Dirt I think @lthall had planned to do this in the PID itself if I understood correctly.

[lthall]

Yep, the plan was to implement the notch in the PID object on the measured input. That way we can set the notch to help filter or tune each axis separately.

For the ground resonance I planed to have a gain back off parameter for each axis that is activated when we are landed. This would let us back yaw off strongly, say 50% and roll and pitch just a little, say 25%.

So we are all thinking along the same lines!!

[amilcarlucas]

@andyp1per isn´t something like this from you already merged ?