Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Don't correct flow sensor for zero position offset

This commit is contained in:
priseborough 2016-10-15 09:17:48 +11:00 committed by Andrew Tridgell
parent b40016db62
commit 9a0ce1d5eb
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

@ -307,8 +307,10 @@ void NavEKF2_core::FuseOptFlow()
// the corrected value is the predicted range from the sensor focal point to the
// centre of the image on the ground assuming flat terrain
Vector3f posOffsetBody = ofDataDelayed.body_offset - accelPosOffset;
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
range -= posOffsetEarth.z / prevTnb.c.z;
if (!posOffsetBody.is_zero()) {
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
range -= posOffsetEarth.z / prevTnb.c.z;
}
// calculate relative velocity in sensor frame including the relative motion due to rotation
relVelSensor = prevTnb*stateStruct.velocity + ofDataDelayed.bodyRadXYZ % posOffsetBody;