Skip to content

AP_NavEKF3: affinity may result in storing offsets to the wrong compass #35133

Open

Description

@rmackay9

Bug Report

While working on PR #35128 my AI noticed that there is an edge case where the EKF3 with affinity enabled (see EK3_AFFINITY) may store the learned mag field to the wrong compass's offsets.

With compass affinity enabled (EK3_AFFINITY bit for compass), a compass failure and recovery in flight can leave the EKF's learned magnetic field (body_magfield) holding values learned for a different compass than the one it's now using. If COMPASS_LEARN is 2 (EKF-Learning), those values are then used to compute offsets that are saved to the wrong compass's COMPASSn_OFS_X/Y/Z parameters at disarm.

How to reproduce

  1. start copter SITL
  2. param set EK3_AFFINITY 4 (EnableCompassAffinity)
  3. param set COMPASS_LEARN 2 (EKF-Learning, saved learned offsets at disarm)
  4. param set COMPASS_USE3 0 (ignore third compass)
  5. reboot SITL
  6. arm and takeoff in Guided or Loiter (any mode is fine)
  7. param set SIM_MAG1_OFS_X 200 (sets compass1's actual offsets)
  8. param set SIM_MAG2_OFS_X -150 (sets compass2's actual offsets)
  9. fly around to allow cores to learn offsets
  10. param set SIM_MAG1_FAIL 1 (to fail compass1, look for "EKF3 IMU0 switching to compass 1" message)
  11. param set SIM_MAG1_FAIL 0 (to recover compass1, you should not see "switching to compass 0" which is a sign the zeroing of the mag field hasn't happened as it should)
  12. disarm force (to stop the learning immediately before it has time to re-learn correct offsets)
  13. param show COMPASS_OFS_X
  14. param show COMPASS_OFS2_X

If everything works correctly (e.g. no bug):

  • COMPASS_OFS_X should be close to 200 (same as SIM_MAG1_OFS_X)
  • COMPASS_OFS2_X should be close to -150 (same as SIM_MAG2_OFS_X)

Instead, we see:

  • COMPASS_OFS_X is far from 200 and closer to the -150 (the second compass's offset). This happened because the EKF core pinned to compass index 0 learned the second compass's field while compass index 0 was failed. After compass index 0 recovered, that field was used to calculate and save offsets for compass index 0.
  • No EKF3 IMU0 switching to compass 0 message appears when compass index 0 recovers, which is consistent with update_mag_selection() changing magSelectIndex without going through tryChangeCompass().

Possible fix

In update_mag_selection(), when the affinity branch changes magSelectIndex, treat it like any other switch, for example by calling tryChangeCompass(core_index). That zeroes the learned field and resets the related state. That would be a behaviour change to the affinity logic, so a maintainer who knows that design should decide.

Version
master (aka 4.8.0-dev)

Platform
[x] All
[ ] AntennaTracker
[ ] Copter
[ ] Plane
[ ] Rover
[ ] Submarine

As per usual, Claude helped me greatly in the creation and verification of this issue

Activity

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

      Milestone

      No milestone

      Relationships

      None yet

      Development

      No branches or pull requests

      Issue actions