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

ekf2: EKF control don't allow invalid flow gyro to propagate

Browse Source
This commit is contained in:
Daniel Agar
2021-10-14 16:31:22 -04:00
parent e8a064af02
commit 56823b5ac9
1 changed files with 7 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+7
View File
@@ -396,6 +396,13 @@ void Ekf::controlOpticalFlowFusion()
_flow_sample_delayed.dt = delta_time_min;
}
// don't allow invalid flow gyro_xyz to propagate
if (!is_body_rate_comp_available) {
if (!PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(0)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(1)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(2))) {
_flow_sample_delayed.gyro_xyz.zero();
}
}
// when on the ground with poor flow quality,
// assume zero ground relative velocity and LOS rate
_flow_compensated_XY_rad.setZero();