dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: gravity fusion don't use any accel samples that are clipping

This commit is contained in:
Daniel Agar 2023-08-17 16:51:32 -04:00
parent 2c76109a94
commit 0f260fc26c
1 changed files with 3 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/ekf2/EKF/gravity_fusion.cpp
View File

@ -82,7 +82,9 @@ void Ekf::controlGravityFusion(const imuSample &imu)
_aid_src_gravity.fusion_enabled = _control_status.flags.gravity_vector;
if (_aid_src_gravity.fusion_enabled && !_aid_src_gravity.innovation_rejected) {
const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2];
if (_aid_src_gravity.fusion_enabled && !_aid_src_gravity.innovation_rejected && !accel_clipping) {
// perform fusion for each axis
_aid_src_gravity.fused = measurementUpdate(Kx, innovation_variance(0), innovation(0))
&& measurementUpdate(Ky, innovation_variance(1), innovation(1))