From 92783bccfaa720f6a1de8ee5718a0d5fb3d27c6c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 3 Jul 2019 12:36:07 +1000
Subject: [PATCH] AP_NavEKF3: don't allow height datum reset when not on ground
---
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 8 +++++---
1 file changed, 5 insertions(+), 3 deletions(-)
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
index bc41c43388..38176bc7ae 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
@@ -201,8 +201,10 @@ void NavEKF3_core::ResetHeight(void)
// Return true if the height datum reset has been performed
bool NavEKF3_core::resetHeightDatum(void)
{
- if (activeHgtSource == HGT_SOURCE_RNG) {
- // by definition the height datum is at ground level so cannot perform the reset
+ if (activeHgtSource == HGT_SOURCE_RNG || !onGround) {
+ // only allow resets when on the ground.
+ // If using using rangefinder for height then never perform a
+ // reset of the height datum
return false;
}
// record the old height estimate
@@ -212,7 +214,7 @@ bool NavEKF3_core::resetHeightDatum(void)
// reset the height state
stateStruct.position.z = 0.0f;
// adjust the height of the EKF origin so that the origin plus baro height before and after the reset is the same
- if (validOrigin && !inFlight) {
+ if (validOrigin) {
if (!gpsGoodToAlign) {
// if we don't have GPS lock then we shouldn't be doing a
// resetHeightDatum, but if we do then the best option is