AP_AHRS: fixed use of filtered gyro with DCM active

we need to overwrite _omega with the filtered gyro value, so we get
both the low pass filter and the notch filters. Otherwise we will fly
with very high noise gyro data

this also fixes the accel_ef_blended to use the filtered accel. It is
not blended, and removing "_blended" from the API will be worthwhile
as a followup

libraries/AP_AHRS/AP_AHRS.cpp

@@ -2994,8 +2994,9 @@ uint8_t AP_AHRS::get_active_airspeed_index() const
 uint8_t AP_AHRS::get_primary_IMU_index() const
 {
     int8_t imu = -1;
-    switch (ekf_type()) {
+    switch (active_EKF_type()) {
     case EKFType::NONE:
+        imu = AP::ins().get_primary_gyro();
         break;
 #if HAL_NAVEKF2_AVAILABLE
     case EKFType::TWO:

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -85,7 +85,7 @@ AP_AHRS_DCM::update()
     }
 
     // Integrate the DCM matrix using gyro inputs
-    matrix_update(delta_t);
+    matrix_update();
 
     // Normalize the DCM matrix
     normalize();
@@ -140,41 +140,28 @@ void AP_AHRS_DCM::backup_attitude(void)
 }
 
 // update the DCM matrix using only the gyros
-void
-AP_AHRS_DCM::matrix_update(float _G_Dt)
+void AP_AHRS_DCM::matrix_update(void)
 {
+    // use only the primary gyro so our bias estimate is valid, allowing us to return the right filtered gyro
+    // for rate controllers
+    const auto &_ins = AP::ins();
+    Vector3f delta_angle;
+    float dangle_dt;
+    if (_ins.get_delta_angle(delta_angle, dangle_dt) && dangle_dt > 0) {
+        _omega = delta_angle / dangle_dt;
+        _omega += _omega_I;
+        _dcm_matrix.rotate((_omega + _omega_P + _omega_yaw_P) * dangle_dt);
+    }
+
+    // now update _omega from the filtered value from the primary IMU. We need to use
+    // the primary IMU as the notch filters will only be running on one IMU
+
     // note that we do not include the P terms in _omega. This is
     // because the spin_rate is calculated from _omega.length(),
     // and including the P terms would give positive feedback into
     // the _P_gain() calculation, which can lead to a very large P
     // value
-    _omega.zero();
-
-    // average across first two healthy gyros. This reduces noise on
-    // systems with more than one gyro. We don't use the 3rd gyro
-    // unless another is unhealthy as 3rd gyro on PH2 has a lot more
-    // noise
-    uint8_t healthy_count = 0;
-    Vector3f delta_angle;
-    const AP_InertialSensor &_ins = AP::ins();
-    for (uint8_t i=0; i<_ins.get_gyro_count(); i++) {
-        if (_ins.use_gyro(i) && healthy_count < 2) {
-            Vector3f dangle;
-            float dangle_dt;
-            if (_ins.get_delta_angle(i, dangle, dangle_dt)) {
-                healthy_count++;
-                delta_angle += dangle;
-            }
-        }
-    }
-    if (healthy_count > 1) {
-        delta_angle /= healthy_count;
-    }
-    if (_G_Dt > 0) {
-        _omega = delta_angle / _G_Dt;
-        _omega += _omega_I;
-        _dcm_matrix.rotate((_omega + _omega_P + _omega_yaw_P) * _G_Dt);
-    }
+    _omega = _ins.get_gyro() + _omega_I;
 }
 
 
@@ -687,18 +674,8 @@ AP_AHRS_DCM::drift_correction(float deltat)
         }
     }
 
-    //update _accel_ef_blended
-#if INS_MAX_INSTANCES > 1
-    if (_ins.get_accel_count() == 2 && _ins.use_accel(0) && _ins.use_accel(1)) {
-        const float imu1_weight_target = _active_accel_instance == 0 ? 1.0f : 0.0f;
-        // slew _imu1_weight over one second
-        _imu1_weight += constrain_float(imu1_weight_target-_imu1_weight, -deltat, deltat);
-        _accel_ef_blended = _accel_ef[0] * _imu1_weight + _accel_ef[1] * (1.0f - _imu1_weight);
-    } else
-#endif
-    {
-        _accel_ef_blended = _accel_ef[_ins.get_primary_accel()];
-    }
+    // set _accel_ef_blended based on filtered accel
+    _accel_ef_blended = _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

@@ -148,7 +148,7 @@ private:
     Vector3f        _accel_ef_blended;
 
     // Methods
-    void            matrix_update(float _G_Dt);
+    void            matrix_update(void);
     void            normalize(void);
     void            check_matrix(void);
     bool            renorm(Vector3f const &a, Vector3f &result);