AP_Motors: quadratic programming optimal mixer (fixed size matrixes)

[IamPete1]

The key difference between this and the new mixer is how the constraints are applied. The current mixer constrains all motors by the maximum or minimum output. This new approach constrains each motor individually.

This is a demo and explanation, that turned out longer than I planned:
https://youtu.be/oaU7gt31oZk

The new approach should be better with high numbers of motors and for motor failure. It also allows new possibility such as imposing slew limiting at a mixer level. It also makes it much easier to trade priorities between roll, pitch, yaw and throttle.

See #20775

This version uses fixed matrix sizes setup for the max of 12 motors, this has the advantage of not having to allocate dynamically. However it does mean that the were solving more than we need to if there are less than 12 motors. Spent lots of time optimising the maths, this now uses a sparse constraints matrix.

Not yet tested on real hardware, should be much faster than v1, hopefully it will run. Still to be tested are different optimisation levels and inlining functions.

[IamPete1]

Moved to float and done some division to multiplication optimizations. This now runs on H7, "only" at +20% load in the PM log.

Would be nice to be able to make this a little more general again. It is currently a quadratic optimization with linear constraints. If we move to a quadratic optimization with quadratic constraints this could replace all the mixers and be a general solution to tilting motors. Would be nice to expand from 4 DoF to 6DoF too...

However, that generalization will negate some of the optimizations I have been able to do for this case, for example treating the constraints as a spare matrix.

As currently implemented this is also still using fixed size matrices, so your always solving the worst case size. Huge speedups possible for vehicles with fewer motors if we can work out how pick matrix size at runtime without being slower or costing lots of flash.

Also need to test how many iterations the solver is converging in, should be less than 10. Need to make sure if it does use the full 15 iterations it does not cause overruns.

[IamPete1]

A few more optimizations. Now "only" very roughly 20x slower than normal mixer, CubeOrange gets a bump of about 20% load. Its getting to the point where one might consider a test flight. Still needs a few more things, limit flags and some logging would be a good idea.

I think this is about the limit of the "manual" optimizations, its already getting quite hard to follow. The next step would be auto generated symbolic optimizations.

[rmackay9]

@IamPete1, I agree re the limits of the "manual" optimisations. The current mixer is really tricky but on the other hand I don't know if the new code is easier or harder to understand and modify.

[IamPete1]

#20961 new implementation is twice as fast as this one.