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

ekf: calculate the delta time between consecutive baro measurments

Browse Source 
(used for calculating filtered baro offset when primary height source
is not baro)

Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman 2017-02-28 10:49:29 +01:00 committed by ChristophTobler
parent a0ab5cf0d7
commit 36bffd2571
2 changed files with 9 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
EKF/control.cpp
View File

@ -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)

1
EKF/ekf.h
View File

@ -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