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

ekf2-gravity: start based on accel LPF instead of peak hold

Browse Source 
This prevent rapid switching in presence of noise and the innovation
filter is good at rejecting spikes
This commit is contained in:
bresch
2025-01-23 15:21:41 +01:00
committed by Mathieu Bresciani
parent 6ec6f1b297
commit ee30b70a3c
1 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp
+3 -2
View File
@@ -54,8 +54,9 @@ void Ekf::controlGravityFusion(const imuSample &imu)
const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f;
const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f;
const bool accel_lpf_norm_good = (_accel_magnitude_filt > lower_accel_limit)
&& (_accel_magnitude_filt < upper_accel_limit);
const float accel_lpf_norm_sq = _accel_lpf.getState().norm_squared();
const bool accel_lpf_norm_good = (accel_lpf_norm_sq > sq(lower_accel_limit))
&& (accel_lpf_norm_sq < sq(upper_accel_limit));
// fuse gravity observation if our overall acceleration isn't too big
_control_status.flags.gravity_vector = (_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector))