From be5fa49c98607ac8ab811264628377a663b7ebee Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 21 May 2024 20:56:48 +1000 Subject: [PATCH] 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. --- libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 04bf81ec20..48f7016851 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -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;