From 63140014c1b286b9b8d6949ed759a9b1bafcc5a0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 3 May 2022 20:39:27 +1000 Subject: [PATCH] AP_AHRS: fixed bug in blended EKF bias The bug fix in #20431 was incomplete, and ended up with the wrong blended accel bias. This fixes it --- libraries/AP_AHRS/AP_AHRS.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 4bfb122993..972a26e73d 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -587,7 +587,7 @@ void AP_AHRS::update_EKF3(void) _accel_ef_ekf[i] = _dcm_matrix * accel; } } - _accel_ef_ekf_blended = _accel_ef_ekf[_ins.get_primary_accel()]; + _accel_ef_ekf_blended = _accel_ef_ekf[primary_imu>=0?primary_imu:_ins.get_primary_accel()]; nav_filter_status filt_state; EKF3.getFilterStatus(-1,filt_state); update_notify_from_filter_status(filt_state);