Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: 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 f44e5b4afb
commit 6c54ba3e7a
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_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -203,8 +203,10 @@ void NavEKF2_core::ResetHeight(void)
// Return true if the height datum reset has been performed
bool NavEKF2_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
@ -215,7 +217,7 @@ bool NavEKF2_core::resetHeightDatum(void)
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