TPA optimisations

[ctzsnooze]

Code optimisation for the TPA calculation.

Master code requires 26 cycles while throttle < TPA breakpoint, and 63 cycles while sticks are above breakpoint.

This PR reduces that to only 8 cycles, regardless of the throttle position, and returns the same result. This is achieved by doing most of the computation in pid_init.c and avoiding conditional statements.

I'm not sure that throttle can be > 1.0 at this point in the code, but kept that check in place, just in case, since if it was > 1.0 then P and D could be pushed higher than intended.

[blckmn]

AUTOMERGE: (FAIL)

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

[ledvinap]

Is it really worth touching this code? It's some obscure heuristic that probably works.
This PR does only shave few cycles (at the cost of more state variables), but code is still poorly documented and hard to read.

[ctzsnooze]

I do think that saving those cycles is important. Future work for more precise attitude control will consume more cycles, and an F411 overclocked to 120Mhz with a 4k PID loop is using 71% CPU in Angle mode already.

I measured the whole primary PID loop in Angle mode as consuming at baseline 1,657 cycles. I'm not sure where the remaining 19,000 cycles get consumed :-)

This PR saves 55 of those cycles, a non-trivial proportion of the total, and the maths is no more difficult to follow than before.

This graphic shows the measured cycleCount for the main pidController() function, over about ⅓ of a second.
The 'minimum' is 1657 cycles, with regular spikes up to about 2400 cycles. The spikes occur approximately every third PID loop. I'd love to know what causes them.

[ledvinap]

Sorry, I thought that TPA runs at RC update rate, not PID one. In that case saving is more important.

[haslinghuis]

@ledvinap moved TPA in #11779 with a patch #11969

[ctzsnooze]

@KarateBrot @ledvinap would appreciate review again... I hope I addressed the comments appropriately?
Keen to get this merged.

[github-actions]

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!