From f80ba734c8469d925758d2596513efccd1bed861 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 27 Jul 2022 10:21:01 +1000 Subject: [PATCH] AP_AHRS: removed use of "blended" accel values calculate a single EF accel based on primary IMU --- libraries/AP_AHRS/AP_AHRS.cpp | 98 +++++++-------------------- libraries/AP_AHRS/AP_AHRS.h | 11 ++- libraries/AP_AHRS/AP_AHRS_Backend.h | 3 +- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 17 ++--- libraries/AP_AHRS/AP_AHRS_DCM.h | 3 +- libraries/AP_AHRS/AP_AHRS_Logging.cpp | 2 +- libraries/AP_AHRS/AP_AHRS_View.h | 4 +- 7 files changed, 38 insertions(+), 100 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 1daaeb1db5..f96d4a82cb 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -438,11 +438,7 @@ void AP_AHRS::copy_estimates_from_backend_estimates(const AP_AHRS_Backend::Estim _gyro_estimate = results.gyro_estimate; _gyro_drift = results.gyro_drift; - // copy earth-frame accelerometer estimates: - for (uint8_t i=0; i=0?primary_imu:_ins.get_primary_gyro(); + const uint8_t primary_accel = primary_imu>=0?primary_imu:_ins.get_primary_accel(); // get gyro bias for primary EKF and change sign to give gyro drift // Note sign convention used by EKF is bias = measurement - truth @@ -517,30 +514,18 @@ void AP_AHRS::update_EKF2(void) EKF2.getGyroBias(_gyro_drift); _gyro_drift = -_gyro_drift; - // calculate corrected gyro estimate for get_gyro() - if (primary_imu == -1 || !_ins.get_gyro_health(primary_imu)) { - // the primary IMU is undefined so use an uncorrected default value from the INS library - _gyro_estimate = _ins.get_gyro(); - } else { - // use the same IMU as the primary EKF and correct for gyro drift - _gyro_estimate = _ins.get_gyro(primary_imu) + _gyro_drift; - } + // use the same IMU as the primary EKF and correct for gyro drift + _gyro_estimate = _ins.get_gyro(primary_gyro) + _gyro_drift; // get z accel bias estimate from active EKF (this is usually for the primary IMU) float &abias = _accel_bias.z; EKF2.getAccelZBias(abias); // 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 == primary_imu) { - accel.z -= abias; - } - if (_ins.get_accel_health(i)) { - _accel_ef_ekf[i] = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel; - } - } - _accel_ef_ekf_blended = _accel_ef_ekf[primary_imu>=0?primary_imu:_ins.get_primary_accel()]; + Vector3f accel = _ins.get_accel(primary_accel); + accel.z -= abias; + _accel_ef = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel; + nav_filter_status filt_state; EKF2.getFilterStatus(filt_state); update_notify_from_filter_status(filt_state); @@ -587,6 +572,8 @@ void AP_AHRS::update_EKF3(void) // Use the primary EKF to select the primary gyro const int8_t primary_imu = EKF3.getPrimaryCoreIMUIndex(); + const uint8_t primary_gyro = primary_imu>=0?primary_imu:_ins.get_primary_gyro(); + const uint8_t primary_accel = primary_imu>=0?primary_imu:_ins.get_primary_accel(); // get gyro bias for primary EKF and change sign to give gyro drift // Note sign convention used by EKF is bias = measurement - truth @@ -594,32 +581,18 @@ void AP_AHRS::update_EKF3(void) EKF3.getGyroBias(-1,_gyro_drift); _gyro_drift = -_gyro_drift; - // calculate corrected gyro estimate for get_gyro() - if (primary_imu == -1 || !_ins.get_gyro_health(primary_imu)) { - // the primary IMU is undefined so use an uncorrected default value from the INS library - _gyro_estimate = _ins.get_gyro(); - } else { - // use the same IMU as the primary EKF and correct for gyro drift - _gyro_estimate = _ins.get_gyro(primary_imu) + _gyro_drift; - } + // use the same IMU as the primary EKF and correct for gyro drift + _gyro_estimate = _ins.get_gyro(primary_gyro) + _gyro_drift; // get 3-axis accel bias festimates for active EKF (this is usually for the primary IMU) Vector3f &abias = _accel_bias; EKF3.getAccelBias(-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 - for (uint8_t i=0; i<_ins.get_accel_count(); i++) { - Vector3f accel = _ins.get_accel(i); - if (i == primary_imu) { - accel -= abias; - } - if (_ins.get_accel_health(i)) { - _accel_ef_ekf[i] = _dcm_matrix * accel; - } - } - _accel_ef_ekf_blended = _accel_ef_ekf[primary_imu>=0?primary_imu:_ins.get_primary_accel()]; + // use the primary IMU for accel earth frame + Vector3f accel = _ins.get_accel(primary_accel); + accel -= abias; + _accel_ef = _dcm_matrix * accel; + nav_filter_status filt_state; EKF3.getFilterStatus(filt_state); update_notify_from_filter_status(filt_state); @@ -654,12 +627,8 @@ void AP_AHRS::update_SITL(void) _gyro_estimate = _ins.get_gyro(); - for (uint8_t i=0; i 0) { - _accel_ef[i] = _dcm_matrix * (delta_velocity / delta_velocity_dt); + Vector3f accel_ef = _dcm_matrix * (delta_velocity / delta_velocity_dt); // integrate the accel vector in the earth frame between GPS readings - _ra_sum[i] += _accel_ef[i] * deltat; + _ra_sum[i] += accel_ef * deltat; } - healthy_count++; } } // set _accel_ef_blended based on filtered accel - _accel_ef_blended = _dcm_matrix * _ins.get_accel(); + _accel_ef = _dcm_matrix * _ins.get_accel(); // keep a sum of the deltat values, so we know how much time // we have integrated over diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index f10c0435f8..7e9ef95184 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -144,8 +144,7 @@ private: static constexpr float _ki_yaw = 0.01f; // accelerometer values in the earth frame in m/s/s - Vector3f _accel_ef[INS_MAX_INSTANCES]; - Vector3f _accel_ef_blended; + Vector3f _accel_ef; // Methods void matrix_update(void); diff --git a/libraries/AP_AHRS/AP_AHRS_Logging.cpp b/libraries/AP_AHRS/AP_AHRS_Logging.cpp index 641f6701aa..3c5316a991 100644 --- a/libraries/AP_AHRS/AP_AHRS_Logging.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Logging.cpp @@ -158,7 +158,7 @@ void AP_AHRS_View::Write_Rate(const AP_Motors &motors, const AC_AttitudeControl yaw : degrees(get_gyro().z), yaw_out : motors.get_yaw()+motors.get_yaw_ff(), control_accel : (float)accel_target.z, - accel : (float)(-(get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f), + accel : (float)(-(get_accel_ef().z + GRAVITY_MSS) * 100.0f), accel_out : motors.get_throttle() }; AP::logger().WriteBlock(&pkt_rate, sizeof(pkt_rate)); diff --git a/libraries/AP_AHRS/AP_AHRS_View.h b/libraries/AP_AHRS/AP_AHRS_View.h index c1e1b3879d..80abb2c628 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.h +++ b/libraries/AP_AHRS/AP_AHRS_View.h @@ -141,8 +141,8 @@ public: return ahrs.groundspeed(); } - const Vector3f &get_accel_ef_blended(void) const { - return ahrs.get_accel_ef_blended(); + const Vector3f &get_accel_ef(void) const { + return ahrs.get_accel_ef(); } uint32_t getLastPosNorthEastReset(Vector2f &pos) WARN_IF_UNUSED {