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

ekf2: always update IMU filters

Browse Source
This commit is contained in:
bresch 2025-04-08 16:38:58 +02:00 committed by Mathieu Bresciani
parent 6a105bcbdb
commit a40377e544
1 changed files with 31 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
src/modules/ekf2/EKF/ekf.cpp
View File

@ -137,14 +137,6 @@ void Ekf::reset()
bool Ekf::update()
{
if (!_filter_initialised) {
_filter_initialised = initialiseFilter();
if (!_filter_initialised) {
return false;
}
}
// Only run the filter if IMU data in the buffer has been updated
if (_imu_updated) {
_imu_updated = false;
@ -153,11 +145,38 @@ bool Ekf::update()
// TODO: explicitly pop at desired time horizon
const imuSample imu_sample_delayed = _imu_buffer.get_oldest();
// protect against zero data
if (imu_sample_delayed.delta_vel_dt < 1e-4f || imu_sample_delayed.delta_ang_dt < 1e-4f) {
return false;
}
// calculate an average filter update time
// filter and limit input between -50% and +100% of nominal value
float input = 0.5f * (imu_sample_delayed.delta_vel_dt + imu_sample_delayed.delta_ang_dt);
float filter_update_s = 1e-6f * _params.filter_update_interval_us;
_dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * math::constrain(input, 0.5f * filter_update_s, 2.f * filter_update_s);
// limit input between -50% and +100% of nominal value
const float filter_update_s = 1e-6f * _params.filter_update_interval_us;
const float input = math::constrain(0.5f * (imu_sample_delayed.delta_vel_dt + imu_sample_delayed.delta_ang_dt),
0.5f * filter_update_s,
2.f * filter_update_s);
if (_is_first_imu_sample) {
_accel_lpf.reset(imu_sample_delayed.delta_vel / imu_sample_delayed.delta_vel_dt);
_gyro_lpf.reset(imu_sample_delayed.delta_ang / imu_sample_delayed.delta_ang_dt);
_dt_ekf_avg = input;
_is_first_imu_sample = false;
} else {
_accel_lpf.update(imu_sample_delayed.delta_vel / imu_sample_delayed.delta_vel_dt);
_gyro_lpf.update(imu_sample_delayed.delta_ang / imu_sample_delayed.delta_ang_dt);
_dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input;
}
if (!_filter_initialised) {
_filter_initialised = initialiseFilter();
if (!_filter_initialised) {
return false;
}
}
updateIMUBiasInhibit(imu_sample_delayed);
@ -179,24 +198,6 @@ bool Ekf::update()
bool Ekf::initialiseFilter()
{
// Filter accel for tilt initialization
const imuSample &imu_init = _imu_buffer.get_newest();
// protect against zero data
if (imu_init.delta_vel_dt < 1e-4f || imu_init.delta_ang_dt < 1e-4f) {
return false;
}
if (_is_first_imu_sample) {
_accel_lpf.reset(imu_init.delta_vel / imu_init.delta_vel_dt);
_gyro_lpf.reset(imu_init.delta_ang / imu_init.delta_ang_dt);
_is_first_imu_sample = false;
} else {
_accel_lpf.update(imu_init.delta_vel / imu_init.delta_vel_dt);
_gyro_lpf.update(imu_init.delta_ang / imu_init.delta_ang_dt);
}
if (!initialiseTilt()) {
return false;
}