From db138d4480c556fc90a8ac54c2038c63a1fc019e 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 a8ba2a4ce3..de35f40c38 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -330,6 +330,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(); } private: @@ -1040,9 +1043,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(); }; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 86cbbc6574..f398a1fe51 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1334,6 +1334,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();