From 7b20e4ba428903eb5b9046da8139be5b227c9c94 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 29 Jan 2024 16:54:08 +1100 Subject: [PATCH] Tracker: correct altitude pressure offset calculation the value nav_status.altitude_offset is expected to be a correction for differences between the barometers. The user calibrates this value with a MAV_CMD_PREFLIGHT_CALIBRATION call. Without this patch we were passing in the raw barometric pressure values for the tracker and the tracked vehicle. It seems get_altitude_difference is expecting a sea-level pressure as its first argument now, as it subtracts the field elevation from the pressure-difference altitude calculations. Change our call to provide a sea-level-adjusted value --- AntennaTracker/tracking.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/AntennaTracker/tracking.cpp b/AntennaTracker/tracking.cpp index 872c9d186f..86ee22434d 100644 --- a/AntennaTracker/tracking.cpp +++ b/AntennaTracker/tracking.cpp @@ -152,7 +152,7 @@ void Tracker::tracking_update_position(const mavlink_global_position_int_t &msg) */ void Tracker::tracking_update_pressure(const mavlink_scaled_pressure_t &msg) { - float local_pressure = barometer.get_pressure(); + float local_pressure = barometer.get_sealevel_pressure(barometer.get_pressure()); float aircraft_pressure = msg.press_abs*100.0f; // calculate altitude difference based on difference in barometric pressure @@ -166,6 +166,7 @@ void Tracker::tracking_update_pressure(const mavlink_scaled_pressure_t &msg) nav_status.altitude_offset = -alt_diff; nav_status.alt_difference_baro = 0; nav_status.need_altitude_calibration = false; + gcs().send_text(MAV_SEVERITY_INFO, "Pressure alt delta=%f", alt_diff); #if HAL_LOGGING_ENABLED logger.Write_NamedValueFloat("NAV_ALT_OFS", nav_status.altitude_offset); #endif