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

PX4 · Aug 28, 2023 · 0f260fc · 0f260fc
1 parent 2c76109
commit 0f260fc
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/src/modules/ekf2/EKF/gravity_fusion.cpp b/src/modules/ekf2/EKF/gravity_fusion.cpp
@@ -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))