fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 08:38:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf: always publish baro and gnss bias even if zero (#25385)

Browse Source 
* ekf: always publish baro and gnss bias even if zero

* [SQUASH] ekf2: simplify hgt bias publish

---------

Co-authored-by: Daniel Agar <daniel@agar.ca>
This commit is contained in:
Jacob Dahl
2025-08-08 11:18:50 -08:00
committed by GitHub
co-authored by Daniel Agar
parent eef01b1b9a
commit 078e4c911f
2 changed files with 7 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp
+5
View File
@@ -45,6 +45,11 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
auto &aid_src = _aid_src_baro_hgt; auto &aid_src = _aid_src_baro_hgt;
HeightBiasEstimator &bias_est = _baro_b_est; HeightBiasEstimator &bias_est = _baro_b_est;
if (_params.ekf2_baro_ctrl == 0) {
stopBaroHgtFusion();
return;
}
bias_est.predict(_dt_ekf_avg); bias_est.predict(_dt_ekf_avg);
baroSample baro_sample; baroSample baro_sample;
src/modules/ekf2/EKF2.cpp
+2 -2
View File
@@ -1041,7 +1041,7 @@ void EKF2::PublishBaroBias(const hrt_abstime &timestamp)
if (_ekf.aid_src_baro_hgt().timestamp_sample != 0) { if (_ekf.aid_src_baro_hgt().timestamp_sample != 0) {
const BiasEstimator::status &status = _ekf.getBaroBiasEstimatorStatus(); const BiasEstimator::status &status = _ekf.getBaroBiasEstimatorStatus();
if (fabsf(status.bias - _last_baro_bias_published) > 0.001f) { if (fabsf(status.bias - _last_baro_bias_published) > 1e-6f) {
_estimator_baro_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.aid_src_baro_hgt().timestamp_sample, timestamp, _estimator_baro_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.aid_src_baro_hgt().timestamp_sample, timestamp,
_device_id_baro)); _device_id_baro));
@@ -1057,7 +1057,7 @@ void EKF2::PublishGnssHgtBias(const hrt_abstime &timestamp)
if (_ekf.get_gps_sample_delayed().time_us != 0) { if (_ekf.get_gps_sample_delayed().time_us != 0) {
const BiasEstimator::status &status = _ekf.getGpsHgtBiasEstimatorStatus(); const BiasEstimator::status &status = _ekf.getGpsHgtBiasEstimatorStatus();
if (fabsf(status.bias - _last_gnss_hgt_bias_published) > 0.001f) { if (fabsf(status.bias - _last_gnss_hgt_bias_published) > 1e-6f) {
_estimator_gnss_hgt_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.get_gps_sample_delayed().time_us, timestamp)); _estimator_gnss_hgt_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.get_gps_sample_delayed().time_us, timestamp));
_last_gnss_hgt_bias_published = status.bias; _last_gnss_hgt_bias_published = status.bias;