Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 20:48:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Fix solution validity reporting bug

Browse Source 
Fixes bug that causes relative position status to report as false when using optical flow and using range finder as the primary height sensor.
This commit is contained in:
priseborough 2017-04-28 11:04:12 +10:00 committed by Francisco Ferreira
parent 62c123bb08
commit 082f429da3
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
View File

@ -37,8 +37,8 @@ void NavEKF2_core::SelectFlowFusion()
// Perform Data Checks
// Check if the optical flow data is still valid
flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000);
// check is the terrain offset estimate is still valid
gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000);
// check is the terrain offset estimate is still valid - if we are using range finder as the main height reference, the ground is assumed to be at 0
gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000) || (activeHgtSource == HGT_SOURCE_RNG);
// Perform tilt check
bool tiltOK = (prevTnb.c.z > frontend->DCM33FlowMin);
// Constrain measurements to zero if takeoff is not detected and the height above ground