Dynamic FIFO Buffering

[andyp1per]

Split out from #27029

This normalizes the usage of the "primary" gyro in the IMU drivers and gives an opportunity to take action when it changes. We were doing this before with random calls into the AHRS, but really the IMU needs to know this information. This makes it possible to take specific required action in future PRs.

I've turned this into a demo of what this can be used for, most notably to slow down FIFO read rates on non-primary gyros.
On a CubeOrange+ with INS_GYRO_RATE=2 (4KHz):
Before:

idle          PRI=  1 sp=0x30021CF8 STACK= 296/ 504 LOAD=22.0%
SPI4          PRI=181 sp=0x3002AF58 STACK= 896/1464 LOAD=23.2%
SPI1          PRI=181 sp=0x3002B668 STACK= 824/1464 LOAD=25.2%

After:

idle          PRI=  1 sp=0x30021D00 STACK= 296/ 504 LOAD=34.7%
SPI4          PRI=181 sp=0x3002AF58 STACK= 816/1464 LOAD=22.0%
SPI1          PRI=181 sp=0x3002B668 STACK= 816/1464 LOAD=14.1%

The saving is not as great as you might expect, probably down to SPI overhead which can't be avoided but still substantial.

On my fast rate copter with 2x ICM42688 running at 16Mhz or so I get:
Before:

idle          PRI=  1 sp=0x3001F668 STACK= 296/ 504 LOAD= 4.7%
SPI1          PRI=181 sp=0x20003538 STACK= 848/1464 LOAD=17.7%
SPI4          PRI=181 sp=0x20003B50 STACK= 848/1464 LOAD=16.3%

After:

idle          PRI=  1 sp=0x3001F670 STACK= 296/ 504 LOAD=12.8%
SPI1          PRI=181 sp=0x30021838 STACK= 816/1464 LOAD=19.1%
SPI4          PRI=181 sp=0x20002720 STACK= 824/1464 LOAD= 8.3%*

Which is the difference between on the edge and comfortable

With @bugobliterator 's SPI PR #28093 I get further benefit:

idle          PRI=  1 sp=0x3001F670 STACK= 296/ 504 LOAD=15.4%
SPI1          PRI=181 sp=0x30022188 STACK= 816/1464 LOAD=15.9%
SPI4          PRI=181 sp=0x30021B70 STACK= 824/1464 LOAD= 7.6%*

[andyp1per]

Closing because I can live without changing the SPI delay and needs restructuring to fix the layering

[peterbarker]

"Not a hard no"... need to be careful

precursor for poking the primary sensor into the library, rather than fetching from AP_AHRS would be good

[andyp1per]

Flown on my 5". This really works very well indeed! Giving the fast rate loop extra CPU cycles results in even smoother flying - it was amazing!

[andyp1per]

@tridge I have changed this so that the IMU changes are only active on copter with fast rate enabled. Hopefully this goes some way to alleviate your concerns!

[andyp1per]

Test flown, EKF innovations look the same. CPU on second IMU way down.

[andyp1per]

Full fight test on two vehicles. EKF happy, CPU load great. Good to go.

[peterbarker]

Looks pretty good to me.

[tridge]

i think with the accel changes removed we should merge this, I don't love it, but we need to get to the finish line :-)

[tridge]

i think we don't need to do anything with accel in this PR, we tie the accel and gyro index in AP_AHRS::_get_primary_IMU_index() now anyway