diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index d635363741..2572ae3f72 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -781,6 +781,18 @@ void NavEKF3::UpdateFilter(void) } } + if (primary != 0 && core[0].healthy() && !hal.util->get_soft_armed()) { + // when on the ground and disarmed force the first lane. This + // avoids us ending with with a lottery for which IMU is used + // in each flight. Otherwise the alignment of the timing of + // the lane updates with the timing of GPS updates can lead to + // a lane other than the first one being used as primary for + // some flights. As different IMUs may have quite different + // noise characteristics this leads to inconsistent + // performance + primary = 0; + } + check_log_write(); }