diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index d9af5e271f..4645b8f75f 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -751,6 +751,18 @@ void NavEKF2::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(); }