diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 8a68081311..eda1183ee0 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -200,29 +200,22 @@ void AP_AHRS_NavEKF::update_EKF2(void) } _gyro_estimate += _gyro_bias; - float abias1, abias2; - EKF2.getAccelZBias(abias1, abias2); + float abias; + EKF2.getAccelZBias(abias); + // This EKF uses the primary IMU + // Eventually we will run a separate instance of the EKF for each IMU and do the selection and blending of EKF outputs upstream // update _accel_ef_ekf for (uint8_t i=0; i<_ins.get_accel_count(); i++) { Vector3f accel = _ins.get_accel(i); - if (i==0) { - accel.z -= abias1; - } else if (i==1) { - accel.z -= abias2; + if (i==_ins.get_primary_accel()) { + accel.z -= abias; } if (_ins.get_accel_health(i)) { _accel_ef_ekf[i] = _dcm_matrix * accel; } } - - if(_ins.use_accel(0) && _ins.use_accel(1)) { - float IMU1_weighting; - EKF2.getIMU1Weighting(IMU1_weighting); - _accel_ef_ekf_blended = _accel_ef_ekf[0] * IMU1_weighting + _accel_ef_ekf[1] * (1.0f-IMU1_weighting); - } else { - _accel_ef_ekf_blended = _accel_ef_ekf[_ins.get_primary_accel()]; - } + _accel_ef_ekf_blended = _accel_ef_ekf[_ins.get_primary_accel()]; } } }