Hi,
My name is Francisco and I have been doing some debugging on a problem i have been having with my magnetometers and think I have found a bug in the EKF2 (and EKF3) file AP_NavEKF2_Measurements.cpp, function readMagData. On the condition not to accept new data faster than 14Hz (around line 220) I found that the comparison of times on the (_ahrs->get_compass()->last_update_usec() - lastMagUpdate_us) is always ok when dealing with the primary compass, bue something odd happens when changing to a secondary, and this might happen because when changing compasses the _ahrs->get_compass()->last_update_usec() term is still pointing at the first compass and the second to the secondary. I changed last_update_usec() to last_update_usec(magSelectIndex) and found things to work out.
Also, i found that if you start with the last magnetometer, if something fails, you won’t be coming back to the other magnetometer, since they’re index is smaller and whenever you reenter the readMagData function you get caught on the allMagSensorsFailed: (if allMagSensorsFailed || (magTimeOut && assume_zero_sideslip() && magSelectIndex >= maxCount-1 && inFlight))
I was thinking of creating a variable containing the number of magnetometers that have already timedOut instead of the index to evaluate this condition, and would appreciate on both problems I have found.