diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 5fc3416418..8eaa95ad2f 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -528,6 +528,12 @@ public: return false; } + // return the innovations for the specified instance + // An out of range instance (eg -1) returns data for the primary instance + virtual bool get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const { + return false; + } + // get_variances - provides the innovations normalised using the innovation variance where a value of 0 // indicates perfect consistency between the measurement and the EKF solution and a value of of 1 is the maximum // inconsistency that will be accepted by the filter diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index adfda33eee..af46f008d4 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -1643,6 +1643,38 @@ bool AP_AHRS_NavEKF::get_location(struct Location &loc) const } } +// return the innovations for the primariy EKF +// boolean false is returned if innovations are not available +bool AP_AHRS_NavEKF::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const +{ + switch (ekf_type()) { + case EKF_TYPE_NONE: + // We are not using an EKF so no data + return false; + + case EKF_TYPE2: + default: + // use EKF to get innovations + EKF2.getInnovations(-1, velInnov, posInnov, magInnov, tasInnov, yawInnov); + return true; + + case EKF_TYPE3: + // use EKF to get innovations + EKF3.getInnovations(-1, velInnov, posInnov, magInnov, tasInnov, yawInnov); + return true; + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + case EKF_TYPE_SITL: + velInnov.zero(); + posInnov.zero(); + magInnov.zero(); + tasInnov = 0.0f; + yawInnov = 0.0f; + return true; +#endif + } +} + // get_variances - provides the innovations normalised using the innovation variance where a value of 0 // indicates prefect consistency between the measurement and the EKF solution and a value of of 1 is the maximum // inconsistency that will be accpeted by the filter diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 52225702f3..283ac388b8 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -230,6 +230,10 @@ public: // position), false on failure bool get_location(struct Location &loc) const; + // return the innovations for the specified instance + // An out of range instance (eg -1) returns data for the primary instance + bool get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const override; + // get_variances - provides the innovations normalised using the innovation variance where a value of 0 // indicates perfect consistency between the measurement and the EKF solution and a value of of 1 is the maximum // inconsistency that will be accepted by the filter