AP_AHRS: add get_vel_innovations_and_variances_for_source

libraries/AP_AHRS/AP_AHRS.h

@@ -530,6 +530,12 @@ public:
         return false;
     }
 
+    // get a source's velocity innovations.  source should be from 0 to 7 (see AP_NavEKF_Source::SourceXY)
+    // returns true on success and results are placed in innovations and variances arguments
+    virtual bool get_vel_innovations_and_variances_for_source(uint8_t source, Vector3f &innovations, Vector3f &variances) const WARN_IF_UNUSED {
+        return false;
+    }
+
     // get the selected ekf type, for allocation decisions
     int8_t get_ekf_type(void) const {
         return _ekf_type;

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -2129,6 +2129,37 @@ bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar,
     return false;
 }
 
+// get a source's velocity innovations.  source should be from 0 to 7 (see AP_NavEKF_Source::SourceXY)
+// returns true on success and results are placed in innovations and variances arguments
+bool AP_AHRS_NavEKF::get_vel_innovations_and_variances_for_source(uint8_t source, Vector3f &innovations, Vector3f &variances) const
+{
+    switch (ekf_type()) {
+    case EKFType::NONE:
+        // We are not using an EKF so no data
+        return false;
+
+#if HAL_NAVEKF2_AVAILABLE
+    case EKFType::TWO:
+        // EKF2 does not support source level variances
+        return false;
+#endif
+
+#if HAL_NAVEKF3_AVAILABLE
+    case EKFType::THREE:
+        // use EKF to get variance
+        return EKF3.getVelInnovationsAndVariancesForSource(-1, (AP_NavEKF_Source::SourceXY)source, innovations, variances);
+#endif
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKFType::SITL:
+        // SITL does not support source level variances
+        return false;
+#endif
+    }
+
+    return false;
+}
+
 void AP_AHRS_NavEKF::setTakeoffExpected(bool val)
 {
     switch (takeoffExpectedState) {

libraries/AP_AHRS/AP_AHRS_NavEKF.h

@@ -266,6 +266,10 @@ public:
     // boolean false is returned if variances are not available
     bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const override;
 
+    // get a source's velocity innovations
+    // returns true on success and results are placed in innovations and variances arguments
+    bool get_vel_innovations_and_variances_for_source(uint8_t source, Vector3f &innovations, Vector3f &variances) const override WARN_IF_UNUSED;
+
     // returns the expected NED magnetic field
     bool get_mag_field_NED(Vector3f& ret) const;