Improve magnetometer consistency check

[MaEtUgR]

Describe problem solved by the proposed pull request
Fixes #12272

Describe your preferred solution
Instead of comparing each vector component of the 3D filed vectors of each additional megnetometer against the primary one we calculate the angle between the vectors. This allows to check if the sensor orientation is set correctly and the magnetometers mounts are not completely misaligned. It should result in less false positives because of small magnetometer amplitude differences between internal and external compass since we had a lot of false positives preventing people to fly with perfectly fine setups.

There's also some refactoring going on:

• There were early returns one of which lead to not unsubscribing from the sensors topic and leaking file descriptors.
• matrix namespace can be used globally in the cpp source to safe repetitive code
• we should shift towards consistent camelCase function names in our C++ classes, that improves readability against variables

Test data / coverage
Not yet tested. Tests to come.
EDIT: I did bench tests and it works great 🎉 :

• I strapped a GPS module (with external mag) to a pixhawk 4 with foam in between to avoid mag interference because of the pxihawk 4 metal case.
• I did the compass calibration.
• I armed with all the circuit breakers because it's only a board. It worked every time with this fixed setup.
• I took the GPS module and external compass off and tilted it a bit. From 30° (default) on in any direction it denies to arm and shows you what the inconsistency error in degree is such that the user gets a feel if it's 180° or 31°.
  
  
• I put the mag back where the calibration was done and it works fine again

Describe possible alternatives
closes #12327

Additional context
Add any other related context or media.

[dagar]

There were early returns one of which lead to not unsubscribing from the sensors topic and leaking file descriptors.

All of that orb usage can be updated to stack allocated uORB::Subscription, which cleans itself up via RAII and doesn't even use file descriptors internally. I can do it in a followup PR.

[MaEtUgR]

@dagar Great, @bkueng mentioned that to me when I quickly asked him 👍
I started testing this pr and will follow up with the results.

[MaEtUgR]

I successfully benchtested this PR, comments in description and improved the output for the user in case the check fails.

[dagar]

The updated output is quite nice, but still feels one step away from being actionable for a user. Could we just keep all the angle errors (mag_inconsistency_angle[MAG_COUNT_MAX]) and report which mags exceed the threshold? That way we'd have more detail in the log (sensor_preflight) and the user could decide to disable a particular mag if necessary (we'd have to be careful with the reported numbering).

Preflight Fail: Compass X %.0f° inconsistent with primary (Y)"

[MaEtUgR]

I force-pushed to switch the printf value from double to int since it anyways doesn't have a decimal place printed: https://github.com/PX4/Firmware/compare/0216cdd624c2394aed31ed4b03cb5c5cbbbf8d54..a4a6593370fe39536bb19190382d7430e3fb50f1

[MaEtUgR]

The updated output is quite nice, but still feels one step away from being actionable for a user.

@dagar It's the best that was possible with the current architecture. The number of the mag would certainly also be useful but the information is not passed on to the PreflightCheck and I think the voted_sensors check doesn't even know what mag ID it is that gets checked, just the subscription instance and if it's primary or not. Can we improve it further in a next revision?

[dagar]

Can we improve it further in a next revision?

Yes of course, I don't mean to impede progress, just brainstorming ideas. This is a nice improvement.

[LorenzMeier]

@PX4/testflights Please test and please make sure to also test with a magnet if the check still triggers.

[priseborough]

A bias vector in the direction of the earth field will be undetected by this method and result in a false negative if the vehicle remains in a constant orientation from boot to flight so this change does reduce the level of protection. One way to close this vulnerability is to require the user to perform a 90 degree yaw rotation as part of the preflight check.

This method does eliminates false positives caused by different t absolute scale factors between magnetometers.

My testing has shown that calibrating a pixracer board when powered via USB results in an incorrect calibration for the internal magnetometer so this is something to be aware of during testing. I have never had the legacy check fail on a correctly calibrated vehicle other than when magnetic interference caused a different readings between internal and external sensors.

Testing of this PR should include rotating either the internal or external mag with a magnet or by setting offsets to check that the check triggers when there is an error, not just testing to show it doesn't trigger.

Taking off near local magnetic anomalies can still result in a rotated field seen by the external and internal mag depending on their relative locations and result in a check fail even for sensors that are correctly calibrated. A way to resolve a test fail caused by this is to ask the user to pick up and put down the vehicle. If the error clears when the vehicle is picked up, then it is unlikely to be a calibration error, so we could add logic that if the check passes for longer than TBD seconds (long enough to pick up the vehicle) then it latches to passed.

[MaEtUgR]

A bias vector in the direction of the earth field will be undetected by this method

I'm aware of this, The goal was to bring down the false positives that I've seen people really struggling with while keeping a useful safety check to see if the values make sense or the setup is completely screwed up e.g. wrong sensor rotation.

My testing has shown that calibrating a pixracer board when powered via USB results in an incorrect calibration

I've seen small differences in calibration that intermittently pass and fail the 0.15 threshold on all components when calibrating with the USB cable plugged. We have to face the fact that people including me will also in future calibrate using a USB cable. The accuracy will be reduced by that and your ideas about separating scale and offset calibration plus learning the offsets over time are in my opinion very valuable and we should look into them.

I have never had the legacy check fail on a correctly calibrated vehicle other than when magnetic interference caused a different readings between internal and external sensors.

I had this happening: I have a wing and a plane with PX4 and had this problem with both. I calibrated them multiple times with and without USB cable and the check sometimes passed sometimes failed randomly even when I didn't change anything about the setup and just went flying on a different day. The unpleasant surprise of having everything set up on the field just like the day before holding the plane in the hand ready to launch, yaw direction looking good just to have the autopilot randomly failing preflight check lead me to increase the threshold to basically disable the check. Multiple times when this happened I plotted the magnetic field vectors and inspected them. Since the vehicle was properly calibrated the angle between the vectors was always within 15° but the amplitude changed for some interference reason. I thought for a long time I'm the only one having this problem until I realized a lot of users experience the same issue.

I asked myself what the point of assuming an exact match of internal and external magnetometer data is if you have an external compass because the internal one is buried in components that generate potentially dynamic interference.

Testing of this PR should include rotating either the internal or external mag with a magnet or by setting offsets to check that the check triggers when there is an error, not just testing to show it doesn't trigger.

I did test rotating the external mag and checking the result. The test reliably failed and passed in all the cases I came up with rotating the external mag around different axes.

EDIT: I don't understand why CI fails... Did I break the uORB tree in some way?

[dannyfpv]

Tested on Pixhawk 4 v5

Procedure

• Loaded PR12334, placed close to the GPS, proceeded to arm but vehicle did not arm. (Image below shows the reponse)

• Removed mag from GPS and armed, vehicle armed as expected and flew without issue,
  Log without Mag: https://review.px4.io/plot_app?log=ca9e3e2b-0197-4523-baa3-df26fd689222

• Placed mag by the GPS, and proceeded to calibrate, then armed and flew with mag attached with no issue.
  Log with mag during calibration: https://review.px4.io/plot_app?log=91ac4920-ee45-4c17-b935-14736f2bb9dc

• Removed mag form GPS and QGC send the alert on the image below.

[MaEtUgR]

I rebased cleanly to properly run CI again.

[MaEtUgR]

I had to fix the uorb_graph exception rule because the naming changed. Now CI should pass.

[MaEtUgR]

Rebased without conflicts to make CI happy. It was the appveyor version check failing because of shallow clone, see #12555.

[dagar]

This is what I see.

Link to nxp_fmuk66-v3_default.px4 - http://ci.px4.io:8080/job/PX4_misc/job/Firmware-compile/job/PR-12334/10/artifact/build/nxp_fmuk66-v3_default/nxp_fmuk66-v3_default.px4

[MaEtUgR]

I removed the merge commit, rebased, fixed the naming to camel case for the line that recently changed with #12586. Now CI is finally happy. Anyone approving?

[dagar]

@MaEtUgR small side note - those merge commits (from clicking "Update branch" on github) disappear when we "Rebase and merge" or "Squash and merge".