Repository navigation
Description
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
- start copter SITL
- param set EK3_AFFINITY 4 (EnableCompassAffinity)
- param set COMPASS_LEARN 2 (EKF-Learning, saved learned offsets at disarm)
- param set COMPASS_USE3 0 (ignore third compass)
- reboot SITL
- arm and takeoff in Guided or Loiter (any mode is fine)
- param set SIM_MAG1_OFS_X 200 (sets compass1's actual offsets)
- param set SIM_MAG2_OFS_X -150 (sets compass2's actual offsets)
- fly around to allow cores to learn offsets
- param set SIM_MAG1_FAIL 1 (to fail compass1, look for "EKF3 IMU0 switching to compass 1" message)
- 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)
- disarm force (to stop the learning immediately before it has time to re-learn correct offsets)
- param show COMPASS_OFS_X
- 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