AP_NavEKF3: don't allow height datum reset when not on ground

This commit is contained in:
Andrew Tridgell 2019-07-03 12:36:07 +10:00
parent e8d9a1fae9
commit 92783bccfa

View File

@ -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