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

ekf2: remove aid src status fusion_enabled flag

Browse Source
This commit is contained in:
Daniel Agar
2023-09-26 10:30:16 -04:00
parent 99197919d7
commit 14a967e2ca
27 changed files with 105 additions and 137 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/gravity_fusion.cpp
+1 -3
View File
@@ -80,11 +80,9 @@ void Ekf::controlGravityFusion(const imuSample &imu)
float innovation_gate = 1.f;
setEstimatorAidStatusTestRatio(_aid_src_gravity, innovation_gate);
_aid_src_gravity.fusion_enabled = _control_status.flags.gravity_vector;
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) {
if (_control_status.flags.gravity_vector && !_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))