diff --git a/ArduPlane/commands.cpp b/ArduPlane/commands.cpp index 3d388b3da6..623e275e23 100644 --- a/ArduPlane/commands.cpp +++ b/ArduPlane/commands.cpp @@ -122,7 +122,13 @@ void Plane::update_home() } if (ahrs.home_is_set() && !ahrs.home_is_locked()) { Location loc; - if(ahrs.get_position(loc)) { + if(ahrs.get_position(loc) && gps.status() >= AP_GPS::GPS_OK_FIX_3D) { + // we take the altitude directly from the GPS as we are + // about to reset the baro calibration. We can't use AHRS + // altitude or we can end up perpetuating a bias in + // altitude, as AHRS alt depends on home alt, which means + // we would have a circular dependency + loc.alt = gps.location().alt; if (!AP::ahrs().set_home(loc)) { // silently fail }