Skip to content

Commit

Permalink
AP_NavEKF3: Remove unncessary local position height reporting offset
Browse files Browse the repository at this point in the history
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.
  • Loading branch information
priseborough authored and peterbarker committed May 22, 2024
1 parent d7ebe82 commit 6a2300a
Showing 1 changed file with 1 addition and 11 deletions.
12 changes: 1 addition & 11 deletions libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -260,17 +260,7 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const
// Return true if the estimate is valid
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;
} 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;
}
posD = outputDataNew.position.z + posOffsetNED.z;

// Return the current height solution status
return filterStatus.flags.vert_pos;
Expand Down

0 comments on commit 6a2300a

Please sign in to comment.