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<INS_MAX_INSTANCES; i++) { - _accel_ef_ekf[i] = results.accel_ef[i]; - } - _accel_ef_ekf_blended = results.accel_ef_blended; + _accel_ef = results.accel_ef; _accel_bias = results.accel_bias; update_cd_values(); @@ -507,9 +503,10 @@ void AP_AHRS::update_EKF2(void) update_trig(); // Use the primary EKF to select the primary gyro - const int8_t primary_imu = EKF2.getPrimaryCoreIMUIndex(); - const AP_InertialSensor &_ins = AP::ins(); + const int8_t primary_imu = EKF2.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 @@ -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<INS_MAX_INSTANCES; i++) { - const Vector3f &accel = _ins.get_accel(i); - _accel_ef_ekf[i] = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel; - } - _accel_ef_ekf_blended = _accel_ef_ekf[0]; - + const Vector3f &accel = _ins.get_accel(); + _accel_ef = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel; } #if HAL_NAVEKF3_AVAILABLE @@ -709,27 +678,12 @@ void AP_AHRS::update_external(void) _gyro_estimate = AP::externalAHRS().get_gyro(); - for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { - Vector3f accel = AP::externalAHRS().get_accel(); - _accel_ef_ekf[i] = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel; - } - _accel_ef_ekf_blended = _accel_ef_ekf[0]; + Vector3f accel = AP::externalAHRS().get_accel(); + _accel_ef = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel; } } #endif // HAL_EXTERNAL_AHRS_ENABLED -// accelerometer values in the earth frame in m/s/s -const Vector3f &AP_AHRS::get_accel_ef(uint8_t i) const -{ - return _accel_ef_ekf[i]; -} - -// blended accelerometer values in the earth frame in m/s/s -const Vector3f &AP_AHRS::get_accel_ef_blended(void) const -{ - return _accel_ef_ekf_blended; -} - void AP_AHRS::reset() { // support locked access functions to AHRS data @@ -3025,12 +2979,6 @@ uint8_t AP_AHRS::get_primary_IMU_index() const return imu; } -// get earth-frame accel vector for primary IMU -const Vector3f &AP_AHRS::get_accel_ef() const -{ - return get_accel_ef(get_primary_accel_index()); -} - // return the index of the primary core or -1 if no primary core selected int8_t AP_AHRS::get_primary_core_index() const { diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index ccac69552b..93f49fe019 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -193,15 +193,13 @@ public: // return ground speed estimate in meters/second. Used by ground vehicles. float groundspeed(void); - const Vector3f &get_accel_ef(uint8_t i) const; - const Vector3f &get_accel_ef() const; + const Vector3f &get_accel_ef() const { + return _accel_ef; + } // Retrieves the corrected NED delta velocity in use by the inertial navigation void getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const; - // blended accelerometer values in the earth frame in m/s/s - const Vector3f &get_accel_ef_blended() const; - // set the EKF's origin location in 10e7 degrees. This should only // be called when the EKF has no absolute position reference (i.e. GPS) // from which to decide the origin on its own @@ -710,8 +708,7 @@ private: Vector3f _gyro_drift; Vector3f _gyro_estimate; - Vector3f _accel_ef_ekf[INS_MAX_INSTANCES]; - Vector3f _accel_ef_ekf_blended; + Vector3f _accel_ef; Vector3f _accel_bias; const uint16_t startup_delay_ms = 1000; diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h index 0e42385b41..ad7b3e13cc 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.h +++ b/libraries/AP_AHRS/AP_AHRS_Backend.h @@ -55,8 +55,7 @@ public: Matrix3f dcm_matrix; Vector3f gyro_estimate; Vector3f gyro_drift; - Vector3f accel_ef[INS_MAX_INSTANCES]; // must be INS_MAX_INSTANCES - Vector3f accel_ef_blended; + Vector3f accel_ef; Vector3f accel_bias; }; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index a4d69b36ec..9b148645a4 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -122,10 +122,7 @@ void AP_AHRS_DCM::get_results(AP_AHRS_Backend::Estimates &results) results.dcm_matrix = _body_dcm_matrix; results.gyro_estimate = _omega; results.gyro_drift = _omega_I; - - const uint8_t to_copy = MIN(sizeof(results.accel_ef), sizeof(_accel_ef)); - memcpy(results.accel_ef, _accel_ef, to_copy); - results.accel_ef_blended = _accel_ef_blended; + results.accel_ef = _accel_ef; } /* @@ -397,7 +394,7 @@ AP_AHRS_DCM::_P_gain(float spin_rate) float AP_AHRS_DCM::_yaw_gain(void) const { - const float VdotEFmag = _accel_ef[_active_accel_instance].xy().length(); + const float VdotEFmag = _accel_ef.xy().length(); if (VdotEFmag <= 4.0f) { return 0.2f*(4.5f - VdotEFmag); @@ -654,9 +651,8 @@ AP_AHRS_DCM::drift_correction(float deltat) const AP_InertialSensor &_ins = AP::ins(); // rotate accelerometer values into the earth frame - uint8_t healthy_count = 0; for (uint8_t i=0; i<_ins.get_accel_count(); i++) { - if (_ins.use_accel(i) && healthy_count < 2) { + if (_ins.use_accel(i)) { /* by using get_delta_velocity() instead of get_accel() the accel value is sampled over the right time delta for @@ -666,16 +662,15 @@ AP_AHRS_DCM::drift_correction(float deltat) float delta_velocity_dt; _ins.get_delta_velocity(i, delta_velocity, delta_velocity_dt); if (delta_velocity_dt > 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 {