From 128d9b0e41d9bb09beb1375edfb1612d004b6959 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 28 Apr 2024 16:53:12 +1000 Subject: [PATCH] AP_Airspeed: added atmospheric tables for high altitude flight this gets altitude and EAS2TAS much more accurately up to around 150k feet AMSL. Enabled on boards using EKF double --- libraries/AP_Airspeed/AP_Airspeed_SITL.cpp | 5 +---- libraries/AP_Airspeed/Airspeed_Calibration.cpp | 6 +++--- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Airspeed/AP_Airspeed_SITL.cpp b/libraries/AP_Airspeed/AP_Airspeed_SITL.cpp index 92ab4357cc..ea3e13fef2 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_SITL.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_SITL.cpp @@ -33,11 +33,8 @@ bool AP_Airspeed_SITL::get_temperature(float &temperature) // this was mostly swiped from SIM_Airspeed_DLVR: const float sim_alt = sitl->state.altitude; - float sigma, delta, theta; - AP_Baro::SimpleAtmosphere(sim_alt * 0.001f, sigma, delta, theta); - // To Do: Add a sensor board temperature offset parameter - temperature = (KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta)) + 25.0; + temperature = AP_Baro::get_temperatureC_for_alt_amsl(sim_alt); return true; } diff --git a/libraries/AP_Airspeed/Airspeed_Calibration.cpp b/libraries/AP_Airspeed/Airspeed_Calibration.cpp index 1cafcfc6e4..0a416d2150 100644 --- a/libraries/AP_Airspeed/Airspeed_Calibration.cpp +++ b/libraries/AP_Airspeed/Airspeed_Calibration.cpp @@ -12,7 +12,7 @@ #include #include #include -#include +#include #include "AP_Airspeed.h" @@ -131,7 +131,7 @@ void AP_Airspeed::update_calibration(uint8_t i, const Vector3f &vground, int16_t // calculate true airspeed, assuming a airspeed ratio of 1.0 float dpress = MAX(get_differential_pressure(i), 0); - float true_airspeed = sqrtf(dpress) * AP::baro().get_EAS2TAS(); + float true_airspeed = sqrtf(dpress) * AP::ahrs().get_EAS2TAS(); float zratio = state[i].calibration.update(true_airspeed, vground, max_airspeed_allowed_during_cal); @@ -177,7 +177,7 @@ void AP_Airspeed::send_airspeed_calibration(const Vector3f &vground) vy: vground.y, vz: vground.z, diff_pressure: get_differential_pressure(primary), - EAS2TAS: AP::baro().get_EAS2TAS(), + EAS2TAS: AP::ahrs().get_EAS2TAS(), ratio: param[primary].ratio.get(), state_x: state[primary].calibration.state.x, state_y: state[primary].calibration.state.y,