diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index cb4b6f35fd..948b08bd77 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -70,7 +70,7 @@ set(msg_files estimator_aid_source_1d.msg estimator_aid_source_2d.msg estimator_aid_source_3d.msg - estimator_baro_bias.msg + estimator_bias.msg estimator_event_flags.msg estimator_gps_status.msg estimator_innovations.msg diff --git a/msg/estimator_baro_bias.msg b/msg/estimator_bias.msg similarity index 82% rename from msg/estimator_baro_bias.msg rename to msg/estimator_bias.msg index 52b807c610..b3c1539a88 100644 --- a/msg/estimator_baro_bias.msg +++ b/msg/estimator_bias.msg @@ -8,3 +8,6 @@ float32 bias_var # estimated barometric altitude bias variance (m^2) float32 innov # innovation of the last measurement fusion (m) float32 innov_var # innovation variance of the last measurement fusion (m^2) float32 innov_test_ratio # normalized innovation squared test ratio + +# TOPICS estimator_bias +# TOPICS estimator_baro_bias estimator_gnss_hgt_bias estimator_rng_hgt_bias estimator_ev_hgt_bias diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 5cc98fa85e..b92002e9e2 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -392,6 +392,9 @@ public: bool isYawEmergencyEstimateAvailable() const; const BiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); } + const BiasEstimator::status &getGpsHgtBiasEstimatorStatus() const { return _gps_hgt_b_est.getStatus(); } + const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); } + const BiasEstimator::status &getEvHgtBiasEstimatorStatus() const { return _ev_hgt_b_est.getStatus(); } const auto &aid_src_airspeed() const { return _aid_src_airspeed; } diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 3bf2fac3cf..0bb9eb2fcf 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -257,6 +257,8 @@ public: const baroSample &get_baro_sample_delayed() const { return _baro_sample_delayed; } const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; } + const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); } + const extVisionSample &get_ev_sample_delayed() const { return _ev_sample_delayed; } const bool &global_origin_valid() const { return _NED_origin_initialised; } const MapProjection &global_origin() const { return _pos_ref; } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 8ecff64938..b0be3c8a9d 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -604,6 +604,9 @@ void EKF2::Run() // publish status/logging messages PublishBaroBias(now); + PublishGnssHgtBias(now); + PublishRngHgtBias(now); + PublishEvHgtBias(now); PublishEventFlags(now); PublishGpsStatus(now); PublishInnovations(now); @@ -699,22 +702,63 @@ void EKF2::PublishBaroBias(const hrt_abstime ×tamp) const BiasEstimator::status &status = _ekf.getBaroBiasEstimatorStatus(); if (fabsf(status.bias - _last_baro_bias_published) > 0.001f) { - estimator_baro_bias_s baro_bias{}; - baro_bias.timestamp_sample = _ekf.get_baro_sample_delayed().time_us; - baro_bias.baro_device_id = _device_id_baro; - baro_bias.bias = status.bias; - baro_bias.bias_var = status.bias_var; - baro_bias.innov = status.innov; - baro_bias.innov_var = status.innov_var; - baro_bias.innov_test_ratio = status.innov_test_ratio; - baro_bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); - _estimator_baro_bias_pub.publish(baro_bias); + _estimator_baro_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.get_baro_sample_delayed().time_us, timestamp, + _device_id_baro)); _last_baro_bias_published = status.bias; } } } +void EKF2::PublishGnssHgtBias(const hrt_abstime ×tamp) +{ + const BiasEstimator::status &status = _ekf.getGpsHgtBiasEstimatorStatus(); + + if (fabsf(status.bias - _last_gnss_hgt_bias_published) > 0.001f) { + _estimator_gnss_hgt_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.get_gps_sample_delayed().time_us, timestamp)); + + _last_gnss_hgt_bias_published = status.bias; + } +} + +void EKF2::PublishRngHgtBias(const hrt_abstime ×tamp) +{ + const BiasEstimator::status &status = _ekf.getRngHgtBiasEstimatorStatus(); + + if (fabsf(status.bias - _last_rng_hgt_bias_published) > 0.001f) { + _estimator_rng_hgt_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.get_rng_sample_delayed().time_us, timestamp)); + + _last_rng_hgt_bias_published = status.bias; + } +} + +void EKF2::PublishEvHgtBias(const hrt_abstime ×tamp) +{ + const BiasEstimator::status &status = _ekf.getEvHgtBiasEstimatorStatus(); + + if (fabsf(status.bias - _last_ev_hgt_bias_published) > 0.001f) { + _estimator_ev_hgt_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.get_ev_sample_delayed().time_us, timestamp)); + + _last_ev_hgt_bias_published = status.bias; + } +} + +estimator_bias_s EKF2::fillEstimatorBiasMsg(const BiasEstimator::status &status, uint64_t timestamp_sample_us, + uint64_t timestamp, uint32_t device_id) +{ + estimator_bias_s bias{}; + bias.timestamp_sample = timestamp_sample_us; + bias.baro_device_id = device_id; + bias.bias = status.bias; + bias.bias_var = status.bias_var; + bias.innov = status.innov; + bias.innov_var = status.innov_var; + bias.innov_test_ratio = status.innov_test_ratio; + bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + + return bias; +} + void EKF2::PublishEventFlags(const hrt_abstime ×tamp) { // information events diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 099ad25e7e..283b7df7ef 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -67,7 +67,7 @@ #include #include #include -#include +#include #include #include #include @@ -137,6 +137,11 @@ private: void PublishAidSourceStatus(const hrt_abstime ×tamp); void PublishAttitude(const hrt_abstime ×tamp); void PublishBaroBias(const hrt_abstime ×tamp); + void PublishGnssHgtBias(const hrt_abstime ×tamp); + void PublishRngHgtBias(const hrt_abstime ×tamp); + void PublishEvHgtBias(const hrt_abstime ×tamp); + estimator_bias_s fillEstimatorBiasMsg(const BiasEstimator::status &status, uint64_t timestamp_sample_us, + uint64_t timestamp, uint32_t device_id = 0); void PublishEventFlags(const hrt_abstime ×tamp); void PublishGlobalPosition(const hrt_abstime ×tamp); void PublishGpsStatus(const hrt_abstime ×tamp); @@ -269,6 +274,9 @@ private: hrt_abstime _status_aux_vel_pub_last{0}; float _last_baro_bias_published{}; + float _last_gnss_hgt_bias_published{}; + float _last_rng_hgt_bias_published{}; + float _last_ev_hgt_bias_published{}; float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements hrt_abstime _airspeed_validated_timestamp_last{0}; @@ -312,7 +320,10 @@ private: uint32_t _filter_information_event_changes{0}; uORB::PublicationMulti _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; - uORB::PublicationMulti _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)}; + uORB::PublicationMulti _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)}; + uORB::PublicationMulti _estimator_gnss_hgt_bias_pub{ORB_ID(estimator_gnss_hgt_bias)}; + uORB::PublicationMulti _estimator_rng_hgt_bias_pub{ORB_ID(estimator_rng_hgt_bias)}; + uORB::PublicationMulti _estimator_ev_hgt_bias_pub{ORB_ID(estimator_ev_hgt_bias)}; uORB::PublicationMultiData _estimator_event_flags_pub{ORB_ID(estimator_event_flags)}; uORB::PublicationMulti _estimator_gps_status_pub{ORB_ID(estimator_gps_status)}; uORB::PublicationMulti _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index ee61a61bb5..6ba4b2a10d 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -144,6 +144,9 @@ void LoggedTopics::add_default_topics() // always add the first instance add_topic("estimator_baro_bias", 500); + add_topic("estimator_gnss_hgt_bias", 500); + add_topic("estimator_rng_hgt_bias", 500); + add_topic("estimator_ev_hgt_bias", 500); add_topic("estimator_event_flags", 0); add_topic("estimator_gps_status", 1000); add_topic("estimator_innovation_test_ratios", 500); @@ -158,6 +161,9 @@ void LoggedTopics::add_default_topics() add_topic("yaw_estimator_status", 1000); add_optional_topic_multi("estimator_baro_bias", 500, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_gnss_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_rng_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_ev_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_gps_status", 1000, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES); @@ -241,6 +247,9 @@ void LoggedTopics::add_default_topics() // EKF replay add_topic("estimator_baro_bias"); + add_topic("estimator_gnss_hgt_bias"); + add_topic("estimator_rng_hgt_bias"); + add_topic("estimator_ev_hgt_bias"); add_topic("estimator_event_flags"); add_topic("estimator_gps_status"); add_topic("estimator_innovation_test_ratios");