diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 4cf2f4e725..2d7371ae43 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -1112,7 +1112,8 @@ bool AP_Baro::arming_checks(size_t buflen, char *buffer) const const auto &gps = AP::gps(); if (_alt_error_max > 0 && gps.status() >= AP_GPS::GPS_Status::GPS_OK_FIX_3D) { const float alt_amsl = gps.location().alt*0.01; - const float alt_pressure = get_altitude_difference(SSL_AIR_PRESSURE, get_pressure()); + // note the addition of _field_elevation_active as this is subtracted in get_altitude_difference() + const float alt_pressure = get_altitude_difference(SSL_AIR_PRESSURE, get_pressure()) + _field_elevation_active; const float error = fabsf(alt_amsl - alt_pressure); if (error > _alt_error_max) { hal.util->snprintf(buffer, buflen, "GPS alt error %.0fm (see BARO_ALTERR_MAX)", error);