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

AP_NavEKF: Output terrain relative PosD when relying on Optical Flow

Browse Source
This commit is contained in:
priseborough 2014-11-29 20:26:35 +11:00 committed by Andrew Tridgell
parent 0156d846f1
commit d701cfcb75
1 changed files with 8 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -3576,7 +3576,14 @@ void NavEKF::getVelNED(Vector3f &vel) const
// return false if no position is available
bool NavEKF::getPosNED(Vector3f &pos) const
{
pos = state.position;
pos.x = state.position.x;
pos.y = state.position.y;
// If relying on optical flow, then output ground relative position so that the vehicle does terain following
if (_fusionModeGPS == 3) {
pos.z = state.position.z - flowStates[1];
} else {
pos.z = state.position.z;
}
return true;
}