From c62fc336cb56d82ce94f46496eca688cf31770ed Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 3 Sep 2016 17:12:39 +1000 Subject: [PATCH] AP_AHRS: use current EKF2 IMU core in gyro estimate The EKF2 implementation uses one IMU per EKF2 core. When reporting the gyro estimate, accel_ef estimate and gyro bias estimate we need to use the values associated with the current IMU index being used by the current EKF2 core. Otherwise we will have an inconsistency between the gyro estimate and attitude estimate This affects all multi-IMU systems using EKF2 --- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 46 +++++++++++++++++++--------- libraries/AP_AHRS/AP_AHRS_NavEKF.h | 12 +++----- 2 files changed, 36 insertions(+), 22 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index b78d70e174..4604d0d915 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -217,39 +217,37 @@ void AP_AHRS_NavEKF::update_EKF2(void) update_trig(); // keep _gyro_bias for get_gyro_drift() + _gyro_bias.zero(); EKF2.getGyroBias(-1,_gyro_bias); _gyro_bias = -_gyro_bias; // calculate corrected gryo estimate for get_gyro() _gyro_estimate.zero(); - uint8_t healthy_count = 0; - for (uint8_t i=0; i<_ins.get_gyro_count(); i++) { - if (_ins.get_gyro_health(i) && healthy_count < 2 && _ins.use_gyro(i)) { - _gyro_estimate += _ins.get_gyro(i); - healthy_count++; - } - } - if (healthy_count > 1) { - _gyro_estimate /= healthy_count; + + // the gyro bias applies only to the IMU associated with the primary EKF2 + // core + int8_t primary_imu = EKF2.getPrimaryCoreIMUIndex(); + if (primary_imu == -1) { + _gyro_estimate = _ins.get_gyro(); + } else { + _gyro_estimate = _ins.get_gyro(primary_imu); } _gyro_estimate += _gyro_bias; - float abias; + float abias = 0; EKF2.getAccelZBias(-1,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 + // This EKF is currently using primary_imu, and abias applies to only that IMU 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.z -= abias; } if (_ins.get_accel_health(i)) { _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()]; } } } @@ -1397,5 +1395,23 @@ bool AP_AHRS_NavEKF::have_ekf_logging(void) const return false; } +// get earth-frame accel vector for primary IMU +const Vector3f &AP_AHRS_NavEKF::get_accel_ef() const +{ + int8_t imu = -1; + switch (ekf_type()) { + case 2: + // let EKF2 choose primary IMU + imu = EKF2.getPrimaryCoreIMUIndex(); + break; + default: + break; + } + if (imu == -1) { + imu = _ins.get_primary_accel(); + } + return get_accel_ef(imu); +} + #endif // AP_AHRS_NAVEKF_AVAILABLE diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 439873850f..5d9b79812b 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -63,11 +63,11 @@ public: NavEKF &_EKF1, NavEKF2 &_EKF2, Flags flags = FLAG_NONE); // return the smoothed gyro vector corrected for drift - const Vector3f &get_gyro(void) const; - const Matrix3f &get_rotation_body_to_ned(void) const; + const Vector3f &get_gyro(void) const override; + const Matrix3f &get_rotation_body_to_ned(void) const override; // return the current drift correction integrator value - const Vector3f &get_gyro_drift(void) const; + const Vector3f &get_gyro_drift(void) const override; // reset the current gyro drift estimate // should be called if gyro offsets are recalculated @@ -123,10 +123,8 @@ public: // EKF has a better ground speed vector estimate Vector2f groundspeed_vector(void); - const Vector3f &get_accel_ef(uint8_t i) const; - const Vector3f &get_accel_ef() const { - return get_accel_ef(_ins.get_primary_accel()); - }; + const Vector3f &get_accel_ef(uint8_t i) const override; + const Vector3f &get_accel_ef() const override; // blended accelerometer values in the earth frame in m/s/s const Vector3f &get_accel_ef_blended(void) const;