diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 6be1a59cea..4c6e06d573 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -411,6 +411,7 @@ void AP_AHRS::copy_estimates_from_backend_estimates(const AP_AHRS_Backend::Estim _accel_ef_ekf[i] = results.accel_ef[i]; } _accel_ef_ekf_blended = results.accel_ef_blended; + _accel_bias = results.accel_bias; update_cd_values(); update_trig(); @@ -494,7 +495,7 @@ void AP_AHRS::update_EKF2(void) } // get z accel bias estimate from active EKF (this is usually for the primary IMU) - float abias = 0; + float &abias = _accel_bias.z; EKF2.getAccelZBias(-1,abias); // This EKF is currently using primary_imu, and abias applies to only that IMU @@ -571,7 +572,7 @@ void AP_AHRS::update_EKF3(void) } // get 3-axis accel bias festimates for active EKF (this is usually for the primary IMU) - Vector3f abias; + Vector3f &abias = _accel_bias; EKF3.getAccelBias(-1,abias); // This EKF uses the primary IMU diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 1b0510bb56..266618e8fd 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -552,6 +552,13 @@ public: return AP::ins().get_accel(); } + // return primary accel bias. This should be subtracted from + // get_accel() vector to get best current body frame accel + // estimate + const Vector3f &get_accel_bias(void) const { + return _accel_bias; + } + /* * AHRS is used as a transport for vehicle-takeoff-expected and * vehicle-landing-expected: @@ -713,6 +720,8 @@ private: Vector3f _gyro_estimate; Vector3f _accel_ef_ekf[INS_MAX_INSTANCES]; Vector3f _accel_ef_ekf_blended; + Vector3f _accel_bias; + const uint16_t startup_delay_ms = 1000; uint32_t start_time_ms; uint8_t _ekf_flags; // bitmask from Flags enumeration diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h index cd26b9f63f..c9264c7038 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.h +++ b/libraries/AP_AHRS/AP_AHRS_Backend.h @@ -61,6 +61,7 @@ public: Vector3f gyro_drift; Vector3f accel_ef[INS_MAX_INSTANCES]; // must be INS_MAX_INSTANCES Vector3f accel_ef_blended; + Vector3f accel_bias; }; // init sets up INS board orientation