From b0011ce014fd0da0e2b413977cd5a2b8b26e4074 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 21 Nov 2022 20:26:30 +1100 Subject: [PATCH] AP_NavEKF3: Don't allow range finder use to start if terrain state is stale --- libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 41a78c6a35..a14de0bfa8 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -1087,7 +1087,7 @@ void NavEKF3_core::selectHeightForFusion() // determine if we are above or below the height switch region ftype rangeMaxUse = 1e-4 * (ftype)_rng->max_distance_cm_orient(ROTATION_PITCH_270) * (ftype)frontend->_useRngSwHgt; bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse; - bool belowLowerSwHgt = (terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse; + bool belowLowerSwHgt = ((terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse) && (imuSampleTime_ms - gndHgtValidTime_ms < 1000); // If the terrain height is consistent and we are moving slowly, then it can be // used as a height reference in combination with a range finder