From 60d8adcca0386c3f371ca42992f2e26ed1aad761 Mon Sep 17 00:00:00 2001 From: priseborough Date: Sat, 20 Aug 2016 17:06:21 +1000 Subject: [PATCH] AP_NavEKF2: Fix height drift on ground using range finder without GPS --- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 3160776271..65a142c75f 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -1336,10 +1336,8 @@ void NavEKF2_core::ConstrainStates() for (uint8_t i=19; i<=21; i++) statesArray[i] = constrain_float(statesArray[i],-0.5f,0.5f); // wind velocity limit 100 m/s (could be based on some multiple of max airspeed * EAS2TAS) - TODO apply circular limit for (uint8_t i=22; i<=23; i++) statesArray[i] = constrain_float(statesArray[i],-100.0f,100.0f); - // constrain the terrain or vertical position state state depending on whether we are using the ground as the height reference - if (inhibitGndState) { - stateStruct.position.z = MIN(stateStruct.position.z, terrainState - rngOnGnd); - } else { + // constrain the terrain state to be below the vehicle height unless we are using terrain as the height datum + if (!inhibitGndState) { terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd); } }