AHRS: add pre-arm check that attitudes are consistent

[rmackay9]

This PR adds and changes two pre-arm checks to resolve this issue which stems from a crash of a vehicle running Copter-3.6.6 where the backup EKF flipped over while the vehicle was disarmed (probably because the user ran a compass calibration but did not reboot the board because the pre-arm compass checks were disabled).

• makes the compass calibration pre-arm checks mandatory (i.e. they cannot be turned off)
• adds a new attitude check that all AHRSs (DCM, EKF2, EKF3) are close to the attitude from whichever of these is primary. The threshold is set to 5deg roll/pitch, 15deg yaw. This new check is part of the INS pre-arm checks.

This has been tested in SITL by adding some debug and then setting these parameters to throw off some of the attitude estimates.

To make the roll/pitch check fail I adjusted the accelerometer offsets as follows:

• param set INS_ACCOFS_X 1

To make the yaw check fail I set the compass offsets as follows:

• param set COMPASS_OFS_X 200
• param set COMPASS_OFS_Y 200

Picture of some SITL testing

[khancyr]

LGTM.
I just have a little concern about what append if we don't have DCM ? Will the check failed ?

[rmackay9]

I've done some additional tests of the attitude check by manually creating pairs of quaternions (from eulers) and then comparing whether they pass or fail the tests. Below are the results.

To explain this screenshot:

• x1,y1,z1 are the euler angles (in degrees) used to create the 1st quaternion
• x2,y2,z2 are the euler angles for the 2nd quaternion
• dx, dy, dz shows the angular difference (in degrees) of the comparison
• rpOK, yOK say whether the test passed ("1") or failed ("0") the roll/pitch threshold (10deg) and yaw threshold (20deg)
  
  The results look OK to me although you can see in test 22 the yaw error (of 15deg) ends up appearing as 7.5deg in roll-pitch, 7.5deg in yaw. Still, I think this is OK.

[peterbarker]

On Wed, 20 Feb 2019, Randy Mackay wrote: yes, indeed.. i think because it's so rarely called and only when disarmed it's probably not worth the optimisation. txs though

Mmmm... not entirely true IIRC, we do go through this when we try to arm. But this isn't tight-looping code, I'll certainly grant you that :-)

[rmackay9]

I've made a minor fix to make DCM return true if it's the only AHRS. This was causing some example sketches to fail.

I've also done some more testing in which I've made it display the roll, pitch and yaw difference between the main EKF and a hard-coded fake EKF with roll/pitch/yaw of 0/0/0 and I'm convinced that this check will not incorrectly every say the attitudes match when they don't.

[rmackay9]

Merged, thanks!