AP_AHRS: removed use of "blended" accel values

calculate a single EF accel based on primary IMU

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;
-            }
+            _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) {
+            Vector3f accel = _ins.get_accel(primary_accel);
             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()];
+            _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;
-            }
+            _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) {
+            // use the primary IMU for accel earth frame
+            Vector3f accel = _ins.get_accel(primary_accel);
             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()];
+            _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];
+        _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
 {

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;

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;
     };
 

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

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);

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));

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 {