diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index b37f4dcc51..402639bfcd 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -528,7 +528,7 @@ public: // 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 // boolean false is returned if variances are not available - virtual bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const { + virtual bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const { return false; } diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index f5bc43bdde..64f398e4d4 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -2051,7 +2051,7 @@ bool AP_AHRS_NavEKF::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vec // 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 // boolean false is returned if variances are not available -bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const +bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const { switch (ekf_type()) { case EKFType::NONE: @@ -2059,17 +2059,21 @@ bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar, return false; #if HAL_NAVEKF2_AVAILABLE - case EKFType::TWO: + case EKFType::TWO: { // use EKF to get variance - EKF2.getVariances(-1,velVar, posVar, hgtVar, magVar, tasVar, offset); + Vector2f offset; + EKF2.getVariances(-1, velVar, posVar, hgtVar, magVar, tasVar, offset); return true; + } #endif #if HAL_NAVEKF3_AVAILABLE - case EKFType::THREE: + case EKFType::THREE: { // use EKF to get variance - EKF3.getVariances(-1,velVar, posVar, hgtVar, magVar, tasVar, offset); + Vector2f offset; + EKF3.getVariances(-1, velVar, posVar, hgtVar, magVar, tasVar, offset); return true; + } #endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL @@ -2079,7 +2083,6 @@ bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar, hgtVar = 0; magVar.zero(); tasVar = 0; - offset.zero(); return true; #endif } diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 1957e96271..de5c1a23f1 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -264,7 +264,7 @@ public: // 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 // boolean false is returned if variances are not available - bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const override; + bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const override; // returns the expected NED magnetic field bool get_mag_field_NED(Vector3f& ret) const;