Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-08 08:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Andrew Tridgell 2019-07-03 12:36:07 +10:00
parent e8d9a1fae9
commit 92783bccfa
1 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -201,8 +201,10 @@ void NavEKF3_core::ResetHeight(void)
// Return true if the height datum reset has been performed // Return true if the height datum reset has been performed
bool NavEKF3_core::resetHeightDatum(void) bool NavEKF3_core::resetHeightDatum(void)
{ {
if (activeHgtSource == HGT_SOURCE_RNG) { if (activeHgtSource == HGT_SOURCE_RNG || !onGround) {
// by definition the height datum is at ground level so cannot perform the reset // only allow resets when on the ground.
// If using using rangefinder for height then never perform a
// reset of the height datum
return false; return false;
} }
// record the old height estimate // record the old height estimate
@ -212,7 +214,7 @@ bool NavEKF3_core::resetHeightDatum(void)
// reset the height state // reset the height state
stateStruct.position.z = 0.0f; 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 // 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 (!gpsGoodToAlign) {
// if we don't have GPS lock then we shouldn't be doing a // if we don't have GPS lock then we shouldn't be doing a
// resetHeightDatum, but if we do then the best option is // resetHeightDatum, but if we do then the best option is