From 93aa6e3f7826026dd954046c24929871299afe2d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 30 Jul 2021 11:26:13 -0400 Subject: [PATCH] ekf2: baro bias publish minor cleanup - naming consistency (estimator prefix as "namespace") - only publish if baro is available and bias is changing as a small logging optimization - avoid unnecessary copying (get const reference to status directly) - trivial code style fixes --- .ci/Jenkinsfile-hardware | 1 + msg/CMakeLists.txt | 4 +-- ...s_estimate.msg => estimator_baro_bias.msg} | 0 msg/tools/uorb_rtps_message_ids.yaml | 2 +- src/modules/ekf2/EKF/ekf.h | 3 +- src/modules/ekf2/EKF/ekf_helper.cpp | 17 ++------- src/modules/ekf2/EKF2.cpp | 35 +++++++++++-------- src/modules/ekf2/EKF2.hpp | 12 ++++--- src/modules/logger/logged_topics.cpp | 2 +- 9 files changed, 38 insertions(+), 38 deletions(-) rename msg/{baro_bias_estimate.msg => estimator_baro_bias.msg} (100%) diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index b0c51d2b79..f336504548 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -902,6 +902,7 @@ void printTopics() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf_gps_drift"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener esc_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_attitude"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_baro_bias"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_event_flags"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_global_position"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovation_test_ratios"' diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index fe981ac0b9..800bbf68bb 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -44,7 +44,6 @@ set(msg_files airspeed.msg airspeed_validated.msg airspeed_wind.msg - baro_bias_estimate.msg battery_status.msg camera_capture.msg camera_status.msg @@ -59,9 +58,9 @@ set(msg_files distance_sensor.msg ekf2_timestamps.msg ekf_gps_drift.msg - event.msg esc_report.msg esc_status.msg + estimator_baro_bias.msg estimator_event_flags.msg estimator_innovations.msg estimator_optical_flow_vel.msg @@ -70,6 +69,7 @@ set(msg_files estimator_states.msg estimator_status.msg estimator_status_flags.msg + event.msg follow_target.msg generator_status.msg geofence_result.msg diff --git a/msg/baro_bias_estimate.msg b/msg/estimator_baro_bias.msg similarity index 100% rename from msg/baro_bias_estimate.msg rename to msg/estimator_baro_bias.msg diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 8a6b0d9ae0..44c4fe8f10 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -343,7 +343,7 @@ rtps: id: 159 - msg: event id: 160 - - msg: baro_bias_estimate + - msg: estimator_baro_bias id: 161 - msg: internal_combustion_engine_status id: 162 diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 5e7720acca..e929755088 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -298,7 +298,8 @@ public: // returns false when data is not available 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]); - void getBaroBiasEstimatorStatus(float &bias, float &bias_var, float &innov, float &innov_var, float &innov_test_ratio); + + const BaroBiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); } private: diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index cac4dccfe4..ed98e7dfd7 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1320,9 +1320,8 @@ void Ekf::updateBaroHgtOffset() // apply a 10 second first order low pass filter to baro offset const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias(); -; - const float offset_rate_correction = 0.1f * (unbiased_baro + _state.pos(2) - - _baro_hgt_offset); + + const float offset_rate_correction = 0.1f * (unbiased_baro + _state.pos(2) - _baro_hgt_offset); _baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f); } } @@ -1348,7 +1347,7 @@ void Ekf::updateBaroHgtBias() } if (_gps_data_ready && !_gps_hgt_intermittent - && _gps_checks_passed &&_NED_origin_initialised + && _gps_checks_passed && _NED_origin_initialised && !_baro_hgt_faulty) { // Use GPS altitude as a reference to compute the baro bias measurement const float baro_bias = (_baro_sample_delayed.hgt - _baro_hgt_offset) @@ -1358,16 +1357,6 @@ void Ekf::updateBaroHgtBias() } } -void Ekf::getBaroBiasEstimatorStatus(float &bias, float &bias_var, float &innov, float &innov_var, float &innov_test_ratio) -{ - BaroBiasEstimator::status status = _baro_b_est.getStatus(); - bias = status.bias; - bias_var = status.bias_var; - innov = status.innov; - innov_var = status.innov_var; - innov_test_ratio = status.innov_test_ratio; -} - Vector3f Ekf::getVisionVelocityInEkfFrame() const { Vector3f vel; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 3402612212..cc33604eb6 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -192,6 +192,7 @@ bool EKF2::multi_init(int imu, int mag) _ekf2_timestamps_pub.advertise(); _ekf_gps_drift_pub.advertise(); + _estimator_baro_bias_pub.advertise(); _estimator_innovation_test_ratios_pub.advertise(); _estimator_innovation_variances_pub.advertise(); _estimator_innovations_pub.advertise(); @@ -202,7 +203,6 @@ bool EKF2::multi_init(int imu, int mag) _estimator_status_flags_pub.advertise(); _estimator_visual_odometry_aligned_pub.advertised(); _yaw_est_pub.advertise(); - _baro_bias_estimate_pub.advertise(); bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag); @@ -497,6 +497,7 @@ void EKF2::Run() PublishWindEstimate(now); // publish status/logging messages + PublishBaroBias(now); PublishEkfDriftMetrics(now); PublishEventFlags(now); PublishStates(now); @@ -506,7 +507,6 @@ void EKF2::Run() PublishInnovationTestRatios(now); PublishInnovationVariances(now); PublishYawEstimatorStatus(now); - PublishBaroBiasEstimate(now); UpdateMagCalibration(now); @@ -550,19 +550,26 @@ void EKF2::PublishAttitude(const hrt_abstime ×tamp) } } -void EKF2::PublishBaroBiasEstimate(const hrt_abstime ×tamp) +void EKF2::PublishBaroBias(const hrt_abstime ×tamp) { - - baro_bias_estimate_s bbe{}; - bbe.timestamp = timestamp; - bbe.timestamp_sample = timestamp; - bbe.baro_device_id = _device_id_baro; - _ekf.getBaroBiasEstimatorStatus(bbe.bias, - bbe.bias_var, - bbe.innov, - bbe.innov_var, - bbe.innov_test_ratio); - _baro_bias_estimate_pub.publish(bbe); + if (_device_id_baro != 0) { + const BaroBiasEstimator::status &status = _ekf.getBaroBiasEstimatorStatus(); + + if (fabsf(status.bias - _last_baro_bias_published) > 0.001f) { + estimator_baro_bias_s baro_bias{}; + baro_bias.timestamp_sample = timestamp; + 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); + + _last_baro_bias_published = status.bias; + } + } } void EKF2::PublishEkfDriftMetrics(const hrt_abstime ×tamp) diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index d926ac9ce4..1901a39f75 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -41,13 +41,15 @@ #ifndef EKF2_HPP #define EKF2_HPP +#include "EKF/ekf.h" +#include "Utility/PreFlightChecker.hpp" + #include "EKF2Selector.hpp" #include #include #include -#include "EKF/ekf.h" #include #include #include @@ -62,10 +64,10 @@ #include #include #include -#include #include #include #include +#include #include #include #include @@ -92,7 +94,6 @@ #include #include -#include "Utility/PreFlightChecker.hpp" extern pthread_mutex_t ekf2_module_mutex; @@ -130,7 +131,7 @@ private: void Run() override; void PublishAttitude(const hrt_abstime ×tamp); - void PublishBaroBiasEstimate(const hrt_abstime ×tamp); + void PublishBaroBias(const hrt_abstime ×tamp); void PublishEkfDriftMetrics(const hrt_abstime ×tamp); void PublishEventFlags(const hrt_abstime ×tamp); void PublishGlobalPosition(const hrt_abstime ×tamp); @@ -218,6 +219,7 @@ private: Vector3f _last_accel_bias_published{}; Vector3f _last_gyro_bias_published{}; Vector3f _last_mag_bias_published{}; + float _last_baro_bias_published{}; float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements @@ -258,9 +260,9 @@ private: uint32_t _filter_warning_event_changes{0}; uint32_t _filter_information_event_changes{0}; - uORB::PublicationMulti _baro_bias_estimate_pub{ORB_ID(baro_bias_estimate)}; uORB::PublicationMulti _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; uORB::PublicationMulti _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)}; + uORB::PublicationMulti _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)}; uORB::PublicationMulti _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; uORB::PublicationMulti _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; uORB::PublicationMulti _estimator_innovations_pub{ORB_ID(estimator_innovations)}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 3fefdf81c2..24b5621207 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -138,7 +138,6 @@ void LoggedTopics::add_default_topics() #else static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 6; // artificially limited until PlotJuggler fixed add_topic("estimator_selector_status"); - add_topic_multi("baro_bias_estimate", 500, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_attitude", 500, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_global_position", 1000, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_local_position", 500, MAX_ESTIMATOR_INSTANCES); @@ -146,6 +145,7 @@ void LoggedTopics::add_default_topics() #endif add_topic_multi("ekf_gps_drift", 1000, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_baro_bias", 500, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_innovation_variances", 500, MAX_ESTIMATOR_INSTANCES);