AHRS: fixed initial attitude for non-zero AHRS_ORIENTATION

[tridge]

we need to get the orientation right before any data is read

[peterbarker]

LGTM

[rmackay9]

this is included in 4.6.0-beta6

[dallinbriggs]

@tridge
I would like to open this issue back up. I can confirm that the issue persists in this commit, at least with a Cube Orange Plus.
Test:

• AHRS_orientation = 8
• Cube orange booted while inverted and held still for the entire boot process

Results:

• The vehicle's orientation could be seen flipping over after boot.

Here is a log of it happening after boot. I have also confirmed that cherry picking this commit back to 4.5.7 also gives the same results.
inverted_bootup.zip

[rmackay9]

I've reviewed @dallinbriggs's log and also reproduced it on my own CubeOrange. I've created an issue to capture the problem here #30064

This is what I see with 4.6.0-beta6 and 4.7.0-dev using Copter:

Strangely 4.5.7 has very similar behaviour but it cannot be captured in the logs, perhaps because logging starts later?

Dallin's log shows slightly different behaviour with the roll-over to the correct attitude taking much longer. Maybe this is because DCM is the active attitude estimator in Plane?

EDIT: yes, the issue appears to be worse on Plane. here is the output from my test with Plane-4.7.0 and it looks like Dallin's