From 45aebfdf7402ae42d9d50eefe85e8cfb31b5da7f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 2 Apr 2022 18:13:51 +1100 Subject: [PATCH] AP_AHRS: subtract accel bias from correct ins accel instance In the case that you have INS_USE indicating IMUs should be used, but EK3_IMU_MASK leaving some IMUs unused, we subtract the bias from the wrong INS data --- 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 ab414a545e..4bfb122993 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -580,7 +580,7 @@ void AP_AHRS::update_EKF3(void) // update _accel_ef_ekf for (uint8_t i=0; i<_ins.get_accel_count(); i++) { Vector3f accel = _ins.get_accel(i); - if (i==_ins.get_primary_accel()) { + if (i == primary_imu) { accel -= abias; } if (_ins.get_accel_health(i)) {