Add ff to angle controller, make feedforward respect gyro setpoint in all modes

[ChrisRosser]

This is a simple addition of an FF term into the angle controller. This PR also changes FF in level modes to respect the unfiltered level mode setpoint rather than acro stick inputs.

Before, stick inputs in angle mode would be passed with acro rate scaling (!) into feedforward instead of the correct setpoint from the angle controller. This causes bad behaviour #12034

This PR correctly adds stick input FF into the angle mode controller. Giving back the sharp response to stick inputs we like in angle mode but without all the bad behaviour associated with the previous implementation.

The benefits of this split approach to FF in angle mode are numerous.

More accurate stick tracking in the angle controller due to proper FF More accurate gyro setpoint tracking in angle mode Better response to sudden perturbations (crashes) in angle mode as FF will push the gyro pid loop to better follow the angle controller setpoint. World peace.

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[blckmn]

AUTOMERGE: (FAIL)

• github identifies PR as mergeable -> FAIL
• assigned to a milestone -> PASS
• cooling off period lapsed -> PASS
• commit count less or equal to three -> PASS
• Don't merge label NOT found -> PASS
• at least one RN: label found -> PASS
• Tested label found -> FAIL
• assigned to an approver -> FAIL
• approver count at least three -> FAIL

[Quick-Flash]

More accurate stick tracking in the angle controller due to proper FF More accurate gyro setpoint tracking in angle mode Better response to sudden perturbations (crashes) in angle mode as FF will push the gyro pid loop to better follow the angle controller setpoint. World peace.

So I do have an issue with this statement.

1. "More accurate stick tracking in the angle controller due to proper FF" This is not necessarily true. The angle PID controller would be a PFF controller (really a P + Setpoint Kick controller) and may actually cause more overshoot and not make setpoint tracking more accurate. Adding Dterm as well would likely solve that problem.
2. "Better response to sudden perturbations (crashes) in angle mode" No this is wrong. FF only applies when the sticks are moved, in a crash where sticks are held constant this will do nothing and the angle PID controller will act like it was before this code change.

Code issues yet to be solved with this:

1. Extra Overshoot from the FF term
2. Fixing the FF code to make this work properly, this will also help with #12035

Solutions:

1. Make this a PDFF controller (look at EmuFlight if you need help here).
2. Fix the FF code.
3. Add an additional (and more true) FF to the controller that can directly pass a portion of the angle pids pidsum into the rate pids pidsum. This will make the angle pid controller more responsive (look at EmuFlight if you need help here).

Current problems with BF angle mode:

1. Angle mode is non-linear, AKA higher pitch angles will make roll changes more sensitive. (Take a look at EmuFlight if you need help here)
2. BF angle mode uses only P term.

This is a real improvement to the BF angle code. BF would be served well by looking at what EmuFlight has done in this regard as the EmuFlight angle controller works very well.

[ChrisRosser]

Some test results for this code.

Without PR:

With PR:

Testing shows the gyro setpoint tracking is improved and the delay between angle rate and heading is reduced. Although we could of course consider further improvements in future as @Quick-Flash suggests this is a suitable fix for now.

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[sugaarK]

ok..... so I tried this only my 2s hd whoop with logging... the stock FF gain is WAY to high not usable at all.. we need to test this on more little birds as they the primary users of level Mode

[ChrisRosser]

ok..... so I tried this only my 2s hd whoop with logging... the stock FF gain is WAY to high not usable at all.. we need to test this on more little birds as they the primary users of level Mode

Sorry, hadn't got around to setting the defaults sensibly. That's done now. A gain of 50 should be good.

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[sugaarK]

ok..... so I tried this only my 2s hd whoop with logging... the stock FF gain is WAY to high not usable at all.. we need to test this on more little birds as they the primary users of level Mode

Sorry, hadn't got around to setting the defaults sensibly. That's done now. A gain of 50 should be good.

ended up with a gain of 15 and 75 angle expo on roll and pitch... 50 is still too high for a whoop

[sugaarK]

in the end I started with 0 and went up in gain 5 at a time

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[ctzsnooze]

Here is a table of CLI values relevant to angle_mode, with suggested re-naming for consistency.

The intent is that when the user types 'get level_' they will find all these values

Current name	Suggested name	Notes
angle_level_strength	level_strength	only angle mode
level_limit	level_angle_limit	only angle mode	angle in degrees, want to find with 'angle' text search
level_gyro_feedforward_adjust	level_gyro_feedforward	only angle mode, adjust seems unnecessary
angle_level_feedforward	level_feedforward	only angle mode
pitch_level_expo	level_expo_pitch	angle and horizon
roll_level_expo	level_expo_roll	angle and horizon
level_race_mode	level_race_mode	only angle mode

[Quick-Flash]

Here is a table of CLI values relevant to angle_mode, with suggested re-naming for consistency.

The intent is that when the user types 'get level_' they will find all these values Current name Suggested name Notes angle_level_strength level_strength only angle mode level_limit level_angle_limit only angle mode level_gyro_feedforward_adjust level_gyro_feedforward only angle mode, adjust seems unnecessary angle_level_feedforward level_feedforward only angle mode pitch_level_expo level_expo_pitch angle and horizon roll_level_expo level_expo_roll angle and horizon level_race_mode level_race_mode only angle mode

Really the angle level strength should just be a Pterm and not called strength, its a stupid name.

[Quick-Flash]

ok..... so I tried this only my 2s hd whoop with logging... the stock FF gain is WAY to high not usable at all.. we need to test this on more little birds as they the primary users of level Mode

Sorry, hadn't got around to setting the defaults sensibly. That's done now. A gain of 50 should be good.

ended up with a gain of 15 and 75 angle expo on roll and pitch... 50 is still too high for a whoop

Part of what will make this tricky, is that this FF simply creates a greater rate setpoint, which means that this is dependent on how your acro controller is tuned. Not sure it has to do with whoops directly. I also did mention that a D term would be quite useful and it would likely allow for a much higher FF before it became to much.

[ChrisRosser]

Here is a table of CLI values relevant to angle_mode, with suggested re-naming for consistency.

The intent is that when the user types 'get level_' they will find all these values

Current name Suggested name Notes angle_level_strength level_strength only angle mode level_limit level_angle_limit only angle mode level_gyro_feedforward_adjust level_gyro_feedforward only angle mode, adjust seems unnecessary angle_level_feedforward level_feedforward only angle mode pitch_level_expo level_expo_pitch angle and horizon roll_level_expo level_expo_roll angle and horizon level_race_mode level_race_mode only angle mode

What is this level mode everyone keeps talking about?! 😉 We have ANGLE HORIZON and ACRO modes. LEVEL mode doesn't exist so why would anyone search for it??!! If it's a legacy term let's kill it or if it has a specific meaning let's be consistent.

[ChrisRosser]

ok..... so I tried this only my 2s hd whoop with logging... the stock FF gain is WAY to high not usable at all.. we need to test this on more little birds as they the primary users of level Mode

Sorry, hadn't got around to setting the defaults sensibly. That's done now. A gain of 50 should be good.

ended up with a gain of 15 and 75 angle expo on roll and pitch... 50 is still too high for a whoop

Part of what will make this tricky, is that this FF simply creates a greater rate setpoint, which means that this is dependent on how your acro controller is tuned. Not sure it has to do with whoops directly. I also did mention that a D term would be quite useful and it would likely allow for a much higher FF before it became to much.

I agree it might be interesting to try D term. We are having issues with additive noise right now with extra derivative terms in the angle controller (FF) if we add D to the angle controller we really will have a noise issue and will need to add a ton of filtering which might eliminate any benefit.

[github-actions[bot]]

Do you want to test this code? Here you have an automated build: Assets WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!