diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 3c8de0ae87..8477e3d36e 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -270,10 +270,11 @@ bool Ekf::update() } else { // calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference - float dt = math::constrain(1e-6f*(float)(_baro_sample_delayed.time_us - last_baro_time_us),0.0f,1.0f); + float local_time_step = 1e-6f*(float)(_baro_sample_delayed.time_us - last_baro_time_us); + local_time_step = math::constrain(local_time_step,0.0f,1.0f); last_baro_time_us = _baro_sample_delayed.time_us; float raw_offset_error = (_baro_sample_delayed.hgt - _hgt_sensor_offset) + _state.pos(2) - _baro_hgt_offset; - _baro_hgt_offset += dt * math::constrain(0.1f * raw_offset_error, -0.1f, 0.1f); + _baro_hgt_offset += local_time_step * math::constrain((0.1f * raw_offset_error), -0.1f, 0.1f); } }