aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: Update protection for out of focus flow data

Browse Source
This commit is contained in:
priseborough 2016-10-07 08:00:39 +11:00 committed by Randy Mackay
parent 635826c056
commit a75a383ef2
1 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
View File

@ -43,8 +43,10 @@ void NavEKF2_core::SelectFlowFusion()
gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000);
// Perform tilt check
bool tiltOK = (prevTnb.c.z > frontend->DCM33FlowMin);
// Constrain measurements to zero if we are on the ground
if (frontend->_fusionModeGPS == 3 && !takeOffDetected) {
// Constrain measurements to zero if takeoff is not detected and the height above ground
// is insuffient to achieve acceptable focus. This allows the vehicle to be picked up
// and carried to test optical flow operation
if (!takeOffDetected && ((terrainState - stateStruct.position.z) < 0.5f)) {
ofDataDelayed.flowRadXYcomp.zero();
ofDataDelayed.flowRadXY.zero();
flowDataValid = true;