From ea80c5027e9882576578bda113f3b398a3570c7b Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 18 Jan 2022 16:15:51 +0100 Subject: [PATCH] ekf2: split yaw estimator state getter into several functions --- src/modules/ekf2/EKF/EKFGSF_yaw.cpp | 11 ----------- src/modules/ekf2/EKF/EKFGSF_yaw.h | 6 +++--- src/modules/ekf2/EKF/ekf_helper.cpp | 6 +++--- 3 files changed, 6 insertions(+), 17 deletions(-) diff --git a/src/modules/ekf2/EKF/EKFGSF_yaw.cpp b/src/modules/ekf2/EKF/EKFGSF_yaw.cpp index ab6786f115..c02fa64763 100644 --- a/src/modules/ekf2/EKF/EKFGSF_yaw.cpp +++ b/src/modules/ekf2/EKF/EKFGSF_yaw.cpp @@ -533,17 +533,6 @@ Matrix3f EKFGSF_yaw::ahrsPredictRotMat(const Matrix3f &R, const Vector3f &g) return ret; } -bool EKFGSF_yaw::getYawData(float *yaw, float *yaw_variance) const -{ - if (_ekf_gsf_vel_fuse_started) { - *yaw = _gsf_yaw; - *yaw_variance = _gsf_yaw_variance; - return true; - } - - return false; -} - void EKFGSF_yaw::setVelocity(const Vector2f &velocity, float accuracy) { _vel_NE = velocity; diff --git a/src/modules/ekf2/EKF/EKFGSF_yaw.h b/src/modules/ekf2/EKF/EKFGSF_yaw.h index ebc28a77ed..55dc4f8cca 100644 --- a/src/modules/ekf2/EKF/EKFGSF_yaw.h +++ b/src/modules/ekf2/EKF/EKFGSF_yaw.h @@ -48,9 +48,9 @@ public: float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const; - // get yaw estimate and the corresponding variance - // return false if no yaw estimate available - bool getYawData(float *yaw, float *yaw_variance) const; + bool isActive() const { return _ekf_gsf_vel_fuse_started; } + float getYaw() const { return _gsf_yaw; } + float getYawVar() const { return _gsf_yaw_variance; } private: diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index df74794247..ce2514dd26 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1729,18 +1729,18 @@ bool Ekf::resetYawToEKFGSF() { // don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity // data and the yaw estimate has converged - float new_yaw, new_yaw_variance; - - if (!_yawEstimator.getYawData(&new_yaw, &new_yaw_variance)) { + if (!_yawEstimator.isActive()) { return false; } + const float new_yaw_variance = _yawEstimator.getYawVar(); const bool has_converged = new_yaw_variance < sq(_params.EKFGSF_yaw_err_max); if (!has_converged) { return false; } + const float new_yaw = _yawEstimator.getYaw(); resetQuatStateYaw(new_yaw, new_yaw_variance, true); // reset velocity and position states to GPS - if yaw is fixed then the filter should start to operate correctly