From 3889b79342de5703609eac8b34d8c83934d9bae3 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 25 Mar 2022 11:30:32 -0400 Subject: [PATCH] ekf2: yaw estimator add yaw_composite_valid boolean --- msg/yaw_estimator_status.msg | 1 + src/modules/ekf2/EKF/ekf.h | 6 +++--- src/modules/ekf2/EKF2.cpp | 1 + 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/msg/yaw_estimator_status.msg b/msg/yaw_estimator_status.msg index 4f6ab58956..36091e26e7 100644 --- a/msg/yaw_estimator_status.msg +++ b/msg/yaw_estimator_status.msg @@ -3,6 +3,7 @@ uint64 timestamp_sample # the timestamp of the raw data (microseconds) float32 yaw_composite # composite yaw from GSF (rad) float32 yaw_variance # composite yaw variance from GSF (rad^2) +bool yaw_composite_valid float32[5] yaw # yaw estimate for each model in the filter bank (rad) float32[5] innov_vn # North velocity innovation for each model in the filter bank (m/s) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index ed8cbf0430..df86bdadf1 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -338,6 +338,9 @@ public: bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]); + // Returns true if the output of the yaw emergency estimator can be used for a reset + bool isYawEmergencyEstimateAvailable() const; + const BaroBiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); } const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; } @@ -1062,9 +1065,6 @@ private: // Returns true if the reset was successful bool resetYawToEKFGSF(); - // Returns true if the output of the yaw emergency estimator can be used for a reset - bool isYawEmergencyEstimateAvailable() const; - void resetGpsDriftCheckFilters(); bool resetEstimatorAidStatusFlags(estimator_aid_source_1d_s &status) const diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 8d8108f646..7b58a8019f 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1384,6 +1384,7 @@ void EKF2::PublishYawEstimatorStatus(const hrt_abstime ×tamp) yaw_est_test_data.innov_vn, yaw_est_test_data.innov_ve, yaw_est_test_data.weight)) { + yaw_est_test_data.yaw_composite_valid = _ekf.isYawEmergencyEstimateAvailable(); yaw_est_test_data.timestamp_sample = _ekf.get_imu_sample_delayed().time_us; yaw_est_test_data.timestamp = _replay_mode ? timestamp : hrt_absolute_time();