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

AP_NavEKF3: Remove unncessary local position height reporting offset 

The offset generated by the EK3_OGN_HGT_MASK parameter bit 2 option is applied to the baro or range finder sensor so it does not have to be applied to the local position height.
This commit is contained in:
Paul Riseborough 2024-05-21 20:56:48 +10:00 committed by Randy Mackay
parent 3070177396
commit d13aefe004
1 changed files with 1 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -260,17 +260,7 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const
// Return true if the estimate is valid // Return true if the estimate is valid
bool NavEKF3_core::getPosD_local(float &posD) const bool NavEKF3_core::getPosD_local(float &posD) const
{ {
// The EKF always has a height estimate regardless of mode of operation
// Correct for the IMU offset (EKF calculations are at the IMU)
// Also correct for changes to the origin height
if ((frontend->_originHgtMode & (1<<2)) == 0) {
// Any sensor height drift corrections relative to the WGS-84 reference are applied to the origin.
posD = outputDataNew.position.z + posOffsetNED.z; posD = outputDataNew.position.z + posOffsetNED.z;
} else {
// The origin height is static and corrections are applied to the local vertical position
// so that height returned by getLLH() = height returned by getOriginLLH - posD
posD = outputDataNew.position.z + posOffsetNED.z + 0.01f * (float)EKF_origin.alt - (float)ekfGpsRefHgt;
}
// Return the current height solution status // Return the current height solution status
return filterStatus.flags.vert_pos; return filterStatus.flags.vert_pos;