From 3fbeda3e1db0ad78b5b650a49fd58c0eba94a4f5 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Tue, 7 Feb 2023 15:59:29 -0500 Subject: [PATCH] AP_Baro: fix bug in alt error arming check get_altitude_difference already subtracts MSL altitude --- libraries/AP_Baro/AP_Baro.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 9dac72a5aa..5e059da7e3 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -1088,7 +1088,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);