From 36bffd2571f4fa44ad9caccef6df7acb0645bb62 Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 28 Feb 2017 10:49:29 +0100 Subject: [PATCH] ekf: calculate the delta time between consecutive baro measurments (used for calculating filtered baro offset when primary height source is not baro) Signed-off-by: Roman --- EKF/control.cpp | 8 ++++++++ EKF/ekf.h | 1 + 2 files changed, 9 insertions(+) diff --git a/EKF/control.cpp b/EKF/control.cpp index 1204ec3edd..dc1bcd3a82 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -82,8 +82,16 @@ void Ekf::controlFusionModes() // check for arrival of new sensor data at the fusion time horizon _gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed); _mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed); + + _delta_time_baro_us = _baro_sample_delayed.time_us; _baro_data_ready = _baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed); + // if we have a new baro sample save the delta time between this sample and the last sample which is + // used below for baro offset calculations + if (_baro_data_ready) { + _delta_time_baro_us = _baro_sample_delayed.time_us - _delta_time_baro_us; + } + // calculate 2,2 element of rotation matrix from sensor frame to earth frame _R_rng_to_earth_2_2 = _R_to_earth(2, 0) * _sin_tilt_rng + _R_to_earth(2, 2) * _cos_tilt_rng; _range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed) diff --git a/EKF/ekf.h b/EKF/ekf.h index 37b4c6ed52..10d721523b 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -261,6 +261,7 @@ private: float _imu_collection_time_adj{0.0f}; // the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec) uint64_t _time_acc_bias_check{0}; // last time the accel bias check passed (usec) + uint64_t _delta_time_baro_us{0}; // delta time between two consecutive delayed baro samples from the buffer (usec) Vector3f _earth_rate_NED; // earth rotation vector (NED) in rad/s