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

EKF: capture optical flow innovation test failures

Browse Source
This commit is contained in:
Paul Riseborough
2016-06-01 17:13:00 +10:00
parent d80e71a499
commit 52229da089
1 changed files with 16 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/optflow_fusion.cpp
+16 -2
View File
@@ -402,9 +402,23 @@ void Ekf::fuseOptFlow()
} }
} }
// if either axis fails, we fail the sensor // record the innovation test pass/fail
if (optflow_test_ratio[0] > 1.0f || optflow_test_ratio[1] > 1.0f) { bool flow_fail = false;
for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
if (optflow_test_ratio[obs_index] > 1.0f) {
flow_fail = true;
_sensor_fault_status.value |= (1 << (obs_index + 9));
} else {
_sensor_fault_status.value &= ~(1 << (obs_index + 9));
}
}
// if either axis fails we abort the fusion
if (flow_fail) {
return; return;
} }
for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) { for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {