From 182ea434450c0e2496e0509eaddcac526caee9c6 Mon Sep 17 00:00:00 2001 From: Roman Date: Thu, 27 Apr 2017 17:19:31 +0200 Subject: [PATCH] fixed baro offset calculation - do not subtract the height sensor offset variable when computing the baro offset from the local origin. The baro height offset is calculated when baro is not fused and so the height sensor offset used in that case is associated to another sensor and has nothing to do with the baro. Signed-off-by: Roman --- EKF/control.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index c4907d720d..8b292ca516 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -766,7 +766,7 @@ void Ekf::controlHeightFusion() local_time_step = math::constrain(local_time_step, 0.0f, 1.0f); // apply a 10 second first order low pass filter to baro offset - float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt - _hgt_sensor_offset + _state.pos( + float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt + _state.pos( 2) - _baro_hgt_offset); _baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f); }