From 660ab611754647c106b47a4cb1eef7ec1b51e6f1 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 29 Apr 2024 14:42:57 +1000 Subject: [PATCH 1/3] autotest: add test for EK3_OGN_HGT_MASK bug --- Tools/autotest/arducopter.py | 38 ++++++++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 610c3bb963259..6bd736a1db26c 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -11377,6 +11377,43 @@ def GuidedForceArm(self): self.disarm_vehicle(force=True) self.reboot_sitl() + def EK3_OGN_HGT_MASK_climbing(self): + '''check combination of height bits doesn't cause climb''' + self.context_push() + self.set_parameters({ + 'EK3_OGN_HGT_MASK': 5, + }) + self.reboot_sitl() + + expected_alt = 10 + + self.change_mode('GUIDED') + self.wait_ready_to_arm() + current_alt = self.get_altitude() + + expected_alt_abs = current_alt + expected_alt + + self.takeoff(expected_alt, mode='GUIDED') + self.delay_sim_time(5) + + def check_altitude(mav, m): + m_type = m.get_type() + epsilon = 10 # in metres + if m_type == 'GPS_RAW_INT': + got_gps_alt = m.alt * 0.001 + if abs(expected_alt_abs - got_gps_alt) > epsilon: + raise NotAchievedException(f"Bad GPS altitude (got={got_gps_alt} want={expected_alt_abs})") + elif m_type == 'GLOBAL_POSITION_INT': + if abs(expected_alt - m.relative_alt * 0.001) > epsilon: + raise NotAchievedException("Bad canonical altitude") + + self.install_message_hook_context(check_altitude) + + self.delay_sim_time(1500) + + self.context_pop() + self.reboot_sitl(force=True) + def tests2b(self): # this block currently around 9.5mins here '''return list of all tests''' ret = ([ @@ -11461,6 +11498,7 @@ def tests2b(self): # this block currently around 9.5mins here self.GuidedModeThrust, self.CompassMot, self.AutoRTL, + self.EK3_OGN_HGT_MASK_climbing, self.EK3_OGN_HGT_MASK, self.FarOrigin, self.GuidedForceArm, From d3713e56501e5ab8d65243b39401effe585441d3 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 21 May 2024 20:26:26 +1000 Subject: [PATCH 2/3] 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 258bd6bcf588e..1440a18a49828 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 3810c672b6cda..d8a28e15ad4f8 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 From e8b6304a1aba4da29d03ea3086ba12fd3c58ca6c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 21 May 2024 20:56:48 +1000 Subject: [PATCH 3/3] 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 04bf81ec20829..48f7016851080 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;