diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index fba2a1af63..bdccc31a90 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/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; diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index ef16622ba3..be7391a3f9 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/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) { diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 305be3fb18..1e113bb161 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/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;