From 5fded75eb60d8bb0bc5d21b8e4b8022622a565ca Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 21 May 2024 20:26:26 +1000 Subject: [PATCH] AP_NavEKF3: Adjust sensor height when EK3_OGN_HGT_MASK bit 2 is set --- libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 2 ++ libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 9 +++++++++ 2 files changed, 11 insertions(+) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 258bd6bcf5..1440a18a49 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -685,8 +685,10 @@ void NavEKF3_core::readGpsData() gpsDataNew.lat = gpsloc.lat; gpsDataNew.lng = gpsloc.lng; if ((frontend->_originHgtMode & (1<<2)) == 0) { + // the height adjustment to match GPS is being achieved by adjusting the origin height gpsDataNew.hgt = (ftype)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt); } else { + // the height adjustment to match GPS is being achieved by adjusting the measurements gpsDataNew.hgt = 0.01 * (gpsloc.alt - EKF_origin.alt); } storedGPS.push(gpsDataNew); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 3810c672b6..d8a28e15ad 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -1278,6 +1278,11 @@ void NavEKF3_core::selectHeightForFusion() hgtMea = MAX(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd); // correct for terrain position relative to datum hgtMea -= terrainState; + // correct sensor so that local position height adjusts to match GPS + if (frontend->_originHgtMode & (1 << 1) && frontend->_originHgtMode & (1 << 2)) { + // offset has to be applied to the measurement, not the NED origin + hgtMea += (float)(ekfGpsRefHgt - 0.01 * (double)EKF_origin.alt); + } velPosObs[5] = -hgtMea; // enable fusion fuseHgtData = true; @@ -1304,6 +1309,10 @@ void NavEKF3_core::selectHeightForFusion() } else if (baroDataToFuse && (activeHgtSource == AP_NavEKF_Source::SourceZ::BARO)) { // using Baro data hgtMea = baroDataDelayed.hgt - baroHgtOffset; + // correct sensor so that local position height adjusts to match GPS + if (frontend->_originHgtMode & (1 << 0) && frontend->_originHgtMode & (1 << 2)) { + hgtMea += (float)(ekfGpsRefHgt - 0.01 * (double)EKF_origin.alt); + } // enable fusion fuseHgtData = true; // set the observation noise