Compass Ordering Improvement #12629

bugobliterator · 2019-10-23T07:25:58Z

This PR is targeted towards making compasses show up in a consistent order.

AP_Compass:

Enables registration to frontend based on device id, instead of auto incrementing compass_count. This ensure whatever order of compass was reached in first boot is retained in following boots.
Registration process involves, first looking if device id was already detected and recorded in previous boot, if so, return the instance id.
If above fails, we look for free instance, i.e. the one that has never been linked with in previous boots.
If all fails, we replace the first unregistered compass. This is most likely going to happen in case the compass was replaced by user with a different one.
Compass_UAVCAN driver is modified to reorder the detected devices, so that the compass with larger node id is sent for registration first. This ensures two things incase we are overflowing number of CAN Instances: We give older compasses priority, they can replace the newer one if connected again alongside, also makes it consistent that same uavcan compass appears as one of the instances.

AP_Logger, GCS_MAVLink, ArduCopter

We use new method instance_available to select whether to record/send/use the respective values for compass instance.

AP_NavEKF2, AP_NavEKF3

We scroll through list of available instances when needed to select different Compass, the changes are made to accommodate behaviour change in Compass library, where devices may appear in an order with missing instance in between.

OXINARF

This is a feature we've wanted for some time, but I have to say I'm a bit afraid with this change to non-sequential instance numbers - we have it in other libraries and we have had issues with it.

In my opinion, the correct way to do it would be to separate the parameters from the _state struct (this state should actually be in the AP_Compass_Backend class). Put the parameters in a struct array and then just give a reference to the backend when it registers a compass.

A last comment: I think we should have a pre-arm check (maybe not failing) that warns the user if not all previous compasses are present. Previously you would (most of times) get an uncalibrated compass and couldn't arm, but now the user will be oblivious to the problem.
Also this kind of breaks the expected dev ids feature because if it isn't there, it simply won't exist, but there is no warning.

libraries/AP_Compass/AP_Compass.cpp

libraries/AP_Compass/AP_Compass.h

OXINARF · 2019-10-23T14:39:27Z

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

                uint8_t tempIndex = magSelectIndex + i;
                // loop back to the start index if we have exceeded the bounds
-                if (tempIndex >= maxCount) {
-                    tempIndex -= maxCount;
+                if (tempIndex >= COMPASS_MAX_INSTANCES) {
+                    tempIndex -= COMPASS_MAX_INSTANCES;
+                }


I know this is pre-existing, but since we are changing it... This could just be: uint8_t tempIndex = (magSelectIndex + i) % COMPASS_MAX_INSTANCES;

OXINARF · 2019-10-23T14:41:57Z

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -222,14 +223,17 @@ void NavEKF2_core::readMagData()
        // If the magnetometer has timed out (been rejected too long) we find another magnetometer to use if available
        // Don't do this if we are on the ground because there can be magnetic interference and we need to know if there is a problem
        // before taking off. Don't do this within the first 30 seconds from startup because the yaw error could be due to large yaw gyro bias affsets
-        if (magTimeout && (maxCount > 1) && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000) {
+        if (magTimeout && _ahrs->get_compass()->any_detected() && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000) {


Again, not the same. The current code checks if there is more than one compass, this code just checks for any compass.
It isn't as bad because it just won't find any other compass in the loop below, but it will be doing an unnecessary loop.

OXINARF · 2019-10-23T14:50:34Z

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -1584,7 +1584,7 @@ void GCS_MAVLINK::send_raw_imu()
    const Vector3f &accel = ins.get_accel(0);
    const Vector3f &gyro = ins.get_gyro(0);
    Vector3f mag;
-    if (compass.get_count() >= 1) {
+    if (compass.instance_available(0)) {


This means that if we don't have compass 0, but have compass 1, then it won't be sent. In my opinion, that's a bad regression.

we send compass 1 and more on SCALED_IMU message, we need to keep the order, otherwise it will lead to confusion while things like log-analysis

OXINARF · 2019-10-23T14:53:00Z

ArduCopter/compassmot.cpp

-    for (uint8_t i=0; i<compass.get_count(); i++) {
-        if (!compass.healthy(i)) {
+    for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
+        if (!compass.healthy(i) || !compass.instance_available(i)) {


Why is the second check needed? How could it be healthy if it's not there?

It's just part of the replacement strategy for compass.get_count. Its a bit paranoid here, I will remove this.

OXINARF · 2019-10-23T15:08:10Z

libraries/AP_Compass/AP_Compass.cpp

@@ -578,7 +578,7 @@ void Compass::init()

 //  Register a new compass instance
 //
-int8_t Compass::register_compass(uint32_t dev_id)
+int8_t Compass::register_compass(int32_t dev_id)


This change is in the wrong commit (AP_Periph: use any_detected method for compass instances available or not)

tridge · 2019-10-24T07:17:15Z

I think this is a big improvement over what we have now. One thing that worries me a bit is that if a user permanently removes a compass and then does a recal they will still end up with a gap. Ideally a gap should only happen if a compass has changed since the last calibration.
To fix that we'd need to re-order drivers when we do a cal. That would imply we'd need to change the instance variables in the drivers. Alternatively we could move the instance number into the backend class so it can be manipulated by common code.
The tricky part is that users currently can use COMPASS_USE, USE2 and USE3 to disable an individual compass, and that is often done before calibration as they don't want that compass for cal.

tridge · 2019-10-26T06:30:56Z

ArduCopter/compassmot.cpp

@@ -42,7 +42,7 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan)

    // check compass health
    compass.read();
-    for (uint8_t i=0; i<compass.get_count(); i++) {
+    for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {


this will cause compassmot to always fail if we don't have 3 compasses (I tested this by changing SITL to have 2 compasses, and found compassmot did indeed fail). We need a check for whether there is a driver for this instance in this loop

I'd actually prefer a FORALL_COMPASS_AVAILABLE(i) macro, much like we have FOREACH_I2C_EXTERNAL(i), it would make for easier to read code, and quite a bit smaller patch (as no indent changes)

tridge · 2019-10-26T06:37:16Z

Tools/AP_Periph/can.cpp

@@ -978,7 +978,7 @@ void AP_Periph_FW::can_mag_update(void)
 #ifdef HAL_PERIPH_ENABLE_MAG
    compass.read();
 #if 1
-    if (compass.get_count() == 0) {
+    if (compass.num_detected() > 0) {


this has reversed the test - it is meant to run the init() if no sensors have been found

libraries/AP_Compass/AP_Compass.cpp

tridge · 2019-10-26T06:44:54Z

libraries/AP_Compass/AP_Compass.h

+    ///
+    /// @returns true if compass backend successfully registered
+    ///
+    bool instance_available(uint8_t i) const { return _state[i].registered; }


given how this is used I'd like it to be a cpp function and it also check if i >= COMPASS_MAX_INSTANCES, to prevent index beyond end of array when we limit the number of compasses below 3 on some builds

tridge · 2019-10-26T06:49:04Z

libraries/AP_Compass/AP_Compass_HIL.cpp

@@ -50,7 +50,7 @@ AP_Compass_HIL::init(void)
 {
    // register two compass instances
    for (uint8_t i=0; i<HIL_NUM_COMPASSES; i++) {
-        _compass_instance[i] = register_compass();
+        _compass_instance[i] = register_compass(i + 1);


should use make_bus_id() with type SITL here

tridge · 2019-10-26T06:50:04Z

libraries/AP_Compass/AP_Compass_HMC5843.cpp

    _bus->set_device_type(DEVTYPE_HMC5883);
+    _compass_instance = register_compass(_bus->get_bus_id());


can we roll the set_dev_id() into the register_compass() call so we have less duplication?

libraries/AP_Compass/AP_Compass_LSM303D.cpp

tridge · 2019-10-26T06:57:33Z

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

-    uint8_t maxCount = _ahrs->get_compass()->get_count();
-    if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() && magSelectIndex >= maxCount-1 && inFlight)) {
+    if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() && 
+                                (magNumTimesChanged >= (_ahrs->get_compass()->num_detected() - 1)) && 


I don't understand the equivalence of this code. magNumTimesChanged is always increasing, and could quickly get beyond num_detected. I think this will kill the EKF after a few compass changes

tridge · 2019-10-26T06:59:15Z

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

-    uint8_t maxCount = _ahrs->get_compass()->get_count();
-    if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() && magSelectIndex >= maxCount-1 && inFlight)) {
+    if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() &&
+                                (magNumTimesChanged >= (_ahrs->get_compass()->num_detected() - 1)) &&


see EKF2 comment above

tridge · 2019-10-28T23:19:31Z

discussed on dev call, we think it should pack the compasses to be contiguous on a re-cal at least

tridge

There are a lot of subtleties with this. The part I find hardest to review is dual indexing scheme. We now have indexing by instance ID and indexing by priority. The PR changes get_field() to use the priority indexing, but most other interfaces still use instance indexing. I'm pretty sure this breaks assumptions in the compass learn code which assumes that get_field(), get_offsets(), get_diagonals() etc all refer to a corresponding compass. I think it may also break things in the calibration code, especially the auto-orientation.
It also breaks logging of MAG messages, which assume that get_field(), get_offsets() and others all refer to the same indexing scheme. That would in turn break external log based analysis (eg. it would break the new magfit_WMM that I wrote recently).
To make progress on this we need to work out what we think the 'correct' API is for each exposed interface, and ensure that it is difficult to mis-use that API. We also need to work out if the pain of the dual indexing is worth the gains.
Then we need to work out how to test this. I think we'd need new SIM_ parameters to set the bus IDs of SITL compasses, so that we can setup scenarios in SITL where compasses are replaced or change probe order and then work out what the correct behaviour is).
For example, I think with what is proposed now that if a user has one UAVCAN compass and one internal SPI compass, then if they replaced the UAVCAN compass then it would change from having the UAVCAN compass as highest priority to it beving lowest priority. I don't think that is what the user would want, but it is so hard to know.

tridge · 2019-12-01T23:13:22Z

libraries/AP_Compass/AP_Compass.cpp

@@ -207,12 +208,6 @@ const AP_Param::GroupInfo Compass::var_info[] = {
    // @User: Advanced
    AP_GROUPINFO("MOT2",    11, Compass, _state[1].motor_compensation, 0),

-    // @Param: PRIMARY


this breaks existing ordering for users who have set COMPASS_PRIMARY already. Also, when removing a parameter we should leave a comment to reserve the slot to prevent it being re-used

This is fixed now

tridge · 2019-12-01T23:16:14Z

ArduCopter/compassmot.cpp

-                                               motor_compensation[compass.get_primary()].x,
-                                               motor_compensation[compass.get_primary()].y,
-                                               motor_compensation[compass.get_primary()].z);
+                                               interference_pct[0],


this seems to assume the first instance is the primary? Don't we need to lookup via a mapping between the PRI var and instance?

The primary compass is always given slot 0, as far as anything above frontend is concerned.

tridge · 2019-12-01T23:18:00Z

libraries/AP_Arming/AP_Arming.cpp

@@ -384,7 +384,7 @@ bool AP_Arming::compass_checks(bool report)

        // avoid Compass::use_for_yaw(void) as it implicitly calls healthy() which can
        // incorrectly skip the remaining checks, pass the primary instance directly
-        if (!_compass.use_for_yaw(_compass.get_primary())) {
+        if (!_compass.use_for_yaw(0)) {


the API becomes rather unclear. Is the argument to use_for_yaw() the instance number or the order number?

All API calls to Compass Frontend take in Priority as Index, any conversions if needed are taken care of by the compass class.

tridge · 2019-12-02T00:48:54Z

minor thing on naming. I think I suggested the PRI name, but I now think 1ST, 2ND and 3RD would be clearer, especially for people who don't have english as first language

bugobliterator · 2020-02-17T07:56:43Z

Overall I really like the intention and the new MP screen. I think it is a big improvement from a user point of view to be able to more directly decide the priority order of the compasses and also enable the 4th, 5th, 6th, etc compasses if they exist.

Please note that 4th 5th 6th and actually 7th and 8th compasses are only there as detected compasses, they do not function. Only there so that if there are more than 3 compasses on board, user may change the priority. Only three compasses ordered in PRIO_* are used and matter.

The extra indirection is frightening though from a maintainability point of view. It seems extremely likely to me that this PR will introduce some bugs most likely in how the EKF handles the compasses. I really wonder how much testing has been done to ensure the EKF is always getting the right compass including situations where it's changing which compass it is using.

The interface to EKF is not changed, the minor change that is introduced is that primary compass instance is always 0, as far as anything external to Compass backend is concerned the order of compass and there priority is same, which actually makes understanding quite easy.

I also wonder what happens in cases where the user modifies the compass order in flight or overwrites some of the parameter values (especially the new PRIO params).

All changes made to order params require reboot. Any changes that are seen post boot are not used in any fashion inside the driver, all params are read and consumed only at init time. So whatever changes made to PRIO params will not be consumed mid flight.

libraries/AP_Compass/AP_Compass_Calibration.cpp

…hotplugging

rmackay9 · 2020-02-17T23:27:23Z

libraries/AP_Compass/AP_Compass.cpp

-    // @Param: PRI_DEVID
-    // @DisplayName: Primary Compass device id
-    // @Description: Primary Compass device id, set automatically if 0. Reboot required after change.
+    // @Param: PRIO1_ID


We should put a "@RebootRequired" on these parameters I think.. and maybe a few others?

rmackay9 · 2020-02-17T23:29:08Z

We discussed this on the dev call and we don't think there are any blockers to this going in. We didn't immediately merge it because I wasn't sure if the int() change had been made but I see now that it has gone in.

It's possible that some more comments should be added to help future developers although we don't have specific advice on what comments should be added.

I think we should get it passing travis and then merge.

LGTM now, thanks!

bugobliterator · 2020-02-18T07:12:13Z

It's possible that some more comments should be added to help future developers although we don't have specific advice on what comments should be added.

Great, I will fix the CI issue. Thanks for your review and followup.

rmackay9 · 2020-02-19T05:00:00Z

Merged, thanks!

proficnc · 2020-02-19T06:37:46Z

Thankyou Sid and Randy!

This is one of those background tasks that is quite transparent to most people but will make a massive difference to the safety and reliability of all of our systems!

Well done!

tridge · 2020-02-19T07:30:11Z

@bugobliterator thanks for all your effort on this!

Pedals2Paddles · 2020-02-19T16:34:26Z

Do the compass 1/2/3 parameters follow the priority settings now? So COMPASS_SCALE2 is the one set for priority 2 rather than just whatever the second detected compass happens to be today? Or is this only the order in which the EKF uses them, and everything else is the same?

rmackay9 · 2020-02-20T01:55:40Z

@Pedals2Paddles, I think the parameters are still in the original order. so COMPASS_DEVID2 will tell you which compass is using COMPASS_SCALE2.

@bugobliterator will know for sure

bugobliterator requested a review from tridge October 23, 2019 07:26

bugobliterator self-assigned this Oct 23, 2019

bugobliterator added Driver Library labels Oct 23, 2019

bugobliterator force-pushed the pr-compass-ordering branch 3 times, most recently from 2425cac to 2afe64c Compare October 23, 2019 08:49

bugobliterator requested a review from rmackay9 October 23, 2019 08:51

OXINARF requested changes Oct 23, 2019

View reviewed changes

bugobliterator force-pushed the pr-compass-ordering branch 3 times, most recently from 1ffcb57 to 876567a Compare October 24, 2019 06:37

tridge requested changes Oct 26, 2019

View reviewed changes

tridge added the DevCallTopic label Oct 28, 2019

CraigElder added DevCallTopic and removed DevCallTopic labels Oct 28, 2019

CraigElder removed the DevCallTopic label Nov 5, 2019

bugobliterator force-pushed the pr-compass-ordering branch 8 times, most recently from bee426f to 31962c8 Compare November 22, 2019 02:02

CraigElder added the DevCallTopic label Nov 26, 2019

tridge requested changes Dec 2, 2019

View reviewed changes

bugobliterator force-pushed the pr-compass-ordering branch 2 times, most recently from 16835b5 to 0c131e3 Compare February 15, 2020 07:31

bugobliterator force-pushed the pr-compass-ordering branch from 0c131e3 to 767e7c4 Compare February 17, 2020 08:31

rmackay9 reviewed Feb 17, 2020

View reviewed changes

libraries/AP_Compass/AP_Compass_Calibration.cpp Outdated Show resolved Hide resolved

rmackay9 reviewed Feb 17, 2020

View reviewed changes

libraries/AP_Compass/AP_Compass_Calibration.cpp Outdated Show resolved Hide resolved

rmackay9 reviewed Feb 17, 2020

View reviewed changes

libraries/AP_Compass/AP_Compass_Calibration.cpp Outdated Show resolved Hide resolved

rmackay9 reviewed Feb 17, 2020

View reviewed changes

libraries/AP_Compass/AP_Compass_Calibration.cpp Outdated Show resolved Hide resolved

rmackay9 reviewed Feb 17, 2020

View reviewed changes

libraries/AP_Compass/AP_Compass_Calibration.cpp Outdated Show resolved Hide resolved

bugobliterator added 3 commits February 17, 2020 16:28

AP_Compass: modify compass driver to support consistent ordering and …

7d97ed8

…hotplugging

SITL: add parameter MAG_DEVID in SITL

4f439b3

AP_Param: add method to find key of top level group variable by pointer

8aba8a5

bugobliterator force-pushed the pr-compass-ordering branch from 767e7c4 to cc81e0d Compare February 17, 2020 11:02

CraigElder removed the DevCallTopic label Feb 17, 2020

rmackay9 reviewed Feb 17, 2020

View reviewed changes

bugobliterator added 2 commits February 18, 2020 18:59

AP_Common: add method that can help to put contraint on accessor type

54f2535

AP_Compass: run style script on Compass.cpp

d7fda5d

bugobliterator force-pushed the pr-compass-ordering branch from cc81e0d to d7fda5d Compare February 18, 2020 13:30

rmackay9 merged commit f00a39a into ArduPilot:master Feb 19, 2020

tatsuy mentioned this pull request Apr 24, 2020

Compass ordering issue #14185

Closed

rmackay9 added this to PRs in Copter 4.0 backports May 11, 2020

tridge moved this from PRs to 4.0.4-rc1 in Copter 4.0 backports May 22, 2020

		_bus->set_device_type(DEVTYPE_HMC5883);
		_compass_instance = register_compass(_bus->get_bus_id());

Compass Ordering Improvement #12629

Compass Ordering Improvement #12629

Conversation

bugobliterator commented Oct 23, 2019 • edited

OXINARF left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

tridge commented Oct 24, 2019

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

tridge commented Oct 28, 2019

tridge left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

tridge commented Dec 2, 2019

bugobliterator commented Feb 17, 2020 • edited

Choose a reason for hiding this comment

rmackay9 commented Feb 17, 2020

bugobliterator commented Feb 18, 2020

rmackay9 commented Feb 19, 2020

proficnc commented Feb 19, 2020

tridge commented Feb 19, 2020

Pedals2Paddles commented Feb 19, 2020

rmackay9 commented Feb 20, 2020

bugobliterator commented Oct 23, 2019 •

edited

bugobliterator commented Feb 17, 2020 •

edited