fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 23:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2-mag_auto: always use mag 3D after takeoff

Browse Source
This commit is contained in:
bresch
2024-02-27 12:33:43 -05:00
committed by Daniel Agar
parent 086656dc7f
commit b80f15f7b5
10 changed files with 8 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.cpp
-8
View File
@@ -326,14 +326,6 @@ void Ekf::predictState(const imuSample &imu_delayed)
const float alpha = 1.0f - imu_delayed.delta_vel_dt;
_accel_lpf_NE = _accel_lpf_NE * alpha + corrected_delta_vel_ef.xy();
// calculate a yaw change about the earth frame vertical
const float spin_del_ang_D = corrected_delta_ang.dot(Vector3f(_R_to_earth.row(2)));
_yaw_delta_ef += spin_del_ang_D;
// Calculate filtered yaw rate to be used by the magnetometer fusion type selection logic
// Note fixed coefficients are used to save operations. The exact time constant is not important.
_yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / imu_delayed.delta_ang_dt;
// Calculate low pass filtered height rate
float alpha_height_rate_lpf = 0.1f * imu_delayed.delta_vel_dt; // 10 seconds time constant
_height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf;