From fd6c9d476de52bc09a7c0314fc9c4aadcf78ca03 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 3 Nov 2021 06:29:31 +1100 Subject: [PATCH] AP_NavEKF3: fixed switch to non-zero primary on disarm if EK3_PRIMARY is not zero then we were not switching to it when disarmed --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 53060851a7..250e99e459 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -965,7 +965,7 @@ void NavEKF3::UpdateFilter(void) } const uint8_t user_primary = uint8_t(_primary_core) < num_cores? _primary_core : 0; - if (primary != 0 && core[user_primary].healthy() && !armed) { + if (primary != user_primary && core[user_primary].healthy() && !armed) { // when on the ground and disarmed force the selected primary // core. This avoids us ending with with a lottery for which // IMU is used in each flight. Otherwise the alignment of the