diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 762369887a..9a65042bc0 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -347,6 +347,8 @@ public: const BaroBiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); } + const auto &aid_src_airspeed() const { return _aid_src_airspeed; } + const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; } const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 349b71c5aa..0737095e30 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -638,6 +638,9 @@ void EKF2::Run() void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) { + // airspeed + PublishAidSourceStatus(_ekf.aid_src_airspeed(), _status_airspeed_pub_last, _estimator_aid_src_airspeed_pub); + // baro height PublishAidSourceStatus(_ekf.aid_src_baro_hgt(), _status_baro_hgt_pub_last, _estimator_aid_src_baro_hgt_pub); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index e279ff0b56..bc72f98a73 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -254,6 +254,8 @@ private: hrt_abstime _last_sensor_bias_published{0}; hrt_abstime _last_gps_status_published{0}; + hrt_abstime _status_airspeed_pub_last{0}; + hrt_abstime _status_baro_hgt_pub_last{0}; hrt_abstime _status_rng_hgt_pub_last{0}; @@ -320,6 +322,7 @@ private: uORB::PublicationMulti _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)}; uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; + uORB::PublicationMulti _estimator_aid_src_airspeed_pub{ORB_ID(estimator_aid_src_airspeed)}; uORB::PublicationMulti _estimator_aid_src_baro_hgt_pub{ORB_ID(estimator_aid_src_baro_hgt)}; uORB::PublicationMulti _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)};