ekf2: split yaw estimator state getter into several functions

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;

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:
 

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