fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +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-05-23 16:58:49 +10:00
parent d80e71a499
commit 52229da089
1 changed files with 16 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
EKF/optflow_fusion.cpp
View File

@ -402,9 +402,23 @@ void Ekf::fuseOptFlow()
}
}
// if either axis fails, we fail the sensor
if (optflow_test_ratio[0] > 1.0f || optflow_test_ratio[1] > 1.0f) {
// record the innovation test pass/fail
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;
}
for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {