From 31f398e9ff87612789be2c6608e47f2deccce751 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 7 Oct 2014 18:43:56 +0200 Subject: [PATCH] [modules][math] QNH functions --- sw/airborne/math/pprz_isa.h | 82 +++++++++++++++++++++++++----- sw/airborne/modules/altitude/qnh.c | 20 ++------ 2 files changed, 72 insertions(+), 30 deletions(-) diff --git a/sw/airborne/math/pprz_isa.h b/sw/airborne/math/pprz_isa.h index f5a39069ee2..8bc9ddfc648 100644 --- a/sw/airborne/math/pprz_isa.h +++ b/sw/airborne/math/pprz_isa.h @@ -42,20 +42,29 @@ extern "C" { #include // Standard Atmosphere constants +/** ISA sea level standard atmospheric pressure in Pascal */ #define PPRZ_ISA_SEA_LEVEL_PRESSURE 101325.0 +/** ISA sea level standard temperature in Kelvin */ #define PPRZ_ISA_SEA_LEVEL_TEMP 288.15 +/** temperature laps rate in K/m */ #define PPRZ_ISA_TEMP_LAPS_RATE 0.0065 +/** earth-surface gravitational acceleration in m/s^2 */ #define PPRZ_ISA_GRAVITY 9.80665 -#define PPRZ_ISA_AIR_GAS_CONSTANT (8.31447/0.0289644) +/** universal gas constant in J/(mol*K) */ +#define PPRZ_ISA_GAS_CONSTANT 8.31447 +/** molar mass of dry air in kg/mol */ +#define PPRZ_ISA_MOLAR_MASS 0.0289644 +/** universal gas constant / molar mass of dry air in J*kg/K */ +#define PPRZ_ISA_AIR_GAS_CONSTANT (PPRZ_ISA_GAS_CONSTANT/PPRZ_ISA_MOLAR_MASS) -static const float PPRZ_ISA_M_OF_P_CONST = (PPRZ_ISA_AIR_GAS_CONSTANT* PPRZ_ISA_SEA_LEVEL_TEMP / PPRZ_ISA_GRAVITY); +static const float PPRZ_ISA_M_OF_P_CONST = (PPRZ_ISA_AIR_GAS_CONSTANT * PPRZ_ISA_SEA_LEVEL_TEMP / PPRZ_ISA_GRAVITY); /** * Get absolute altitude from pressure (using simplified equation). * Referrence pressure is standard pressure at sea level * * @param pressure current pressure in Pascal (Pa) - * @return altitude in pressure in ISA conditions + * @return altitude in m in ISA conditions */ static inline float pprz_isa_altitude_of_pressure(float pressure) { @@ -68,15 +77,19 @@ static inline float pprz_isa_altitude_of_pressure(float pressure) /** * Get relative altitude from pressure (using simplified equation). + * Given the current pressure and a reference pressure (at height=0), + * calculate the height above the reference in meters. + * If you pass QNH as reference pressure, you get the height above sea level. + * Using QFE as reference pressure, you get height above the airfield. * * @param pressure current pressure in Pascal (Pa) - * @param ref reference pressure (QFE) when height = 0 - * @return altitude in pressure in ISA conditions + * @param ref_p reference pressure (QFE) when height=0 or QNH at sea level + * @return height in m above reference in ISA conditions */ -static inline float pprz_isa_height_of_pressure(float pressure, float ref) +static inline float pprz_isa_height_of_pressure(float pressure, float ref_p) { - if (pressure > 0. && ref > 0.) { - return (PPRZ_ISA_M_OF_P_CONST * logf(ref / pressure)); + if (pressure > 0. && ref_p > 0.) { + return (PPRZ_ISA_M_OF_P_CONST * logf(ref_p / pressure)); } else { return 0.; } @@ -86,7 +99,7 @@ static inline float pprz_isa_height_of_pressure(float pressure, float ref) * Get pressure in Pa from absolute altitude (using simplified equation). * * @param altitude current absolute altitude in meters - * @return static pressure in Pa in ISA conditions + * @return static pressure in Pa at given altitude in ISA conditions */ static inline float pprz_isa_pressure_of_altitude(float altitude) { @@ -96,13 +109,54 @@ static inline float pprz_isa_pressure_of_altitude(float altitude) /** * Get pressure in Pa from height (using simplified equation). * - * @param altitude current relative altitude in meters - * @param ref reference pressure (QFE) when height = 0 - * @return static pressure in Pa in ISA conditions + * @param height current height over reference (relative altitude) in meters + * @param ref_p reference pressure (QFE or QNH) when height = 0 + * @return static pressure in Pa at given height in ISA conditions */ -static inline float pprz_isa_pressure_of_height(float altitude, float ref) +static inline float pprz_isa_pressure_of_height(float height, float ref_p) { - return (ref * expf((-1. / PPRZ_ISA_M_OF_P_CONST) * altitude)); + return (ref_p * expf((-1. / PPRZ_ISA_M_OF_P_CONST) * height)); +} + + +/** + * Get relative altitude from pressure (using full equation). + * Given the current pressure and a reference pressure (at height=0), + * calculate the height above the reference in meters. + * If you pass QNH as reference pressure, you get the height above sea level. + * Using QFE as reference pressure, you get height above the airfield. + * + * @param pressure current pressure in Pascal (Pa) + * @param ref_p reference pressure (QFE or QNH) in Pa + * @return height above reference in m in ISA conditions + */ +static inline float pprz_isa_height_of_pressure_full(float pressure, float ref_p) +{ + if (ref_p > 0.) { + const float prel = pressure / ref_p; + const float inv_expo = PPRZ_ISA_GAS_CONSTANT * PPRZ_ISA_TEMP_LAPS_RATE / + PPRZ_ISA_GRAVITY / PPRZ_ISA_MOLAR_MASS; + return (1 - powf(prel, inv_expo)) * PPRZ_ISA_SEA_LEVEL_TEMP / PPRZ_ISA_TEMP_LAPS_RATE; + } else { + return 0.; + } +} + +/** + * Get reference pressure (QFE or QNH) from current pressure and height. + * (using full equation) + * + * @param pressure current pressure in Pascal (Pa) + * @param height height above referece (sea level for QNH, airfield alt for QFE) in m + * @return reference pressure at height=0 in Pa + */ +static inline float pprz_isa_ref_pressure_of_height_full(float pressure, float alt) +{ + // Trel = 1 - L*h/T0; + const float Trel = 1.0 - PPRZ_ISA_TEMP_LAPS_RATE * alt / PPRZ_ISA_SEA_LEVEL_TEMP; + const float expo = PPRZ_ISA_GRAVITY * PPRZ_ISA_MOLAR_MASS / PPRZ_ISA_GAS_CONSTANT / + PPRZ_ISA_TEMP_LAPS_RATE; + return pressure / pow(Trel, expo); } #ifdef __cplusplus diff --git a/sw/airborne/modules/altitude/qnh.c b/sw/airborne/modules/altitude/qnh.c index e4d4c0e1376..7f9fe223526 100644 --- a/sw/airborne/modules/altitude/qnh.c +++ b/sw/airborne/modules/altitude/qnh.c @@ -29,6 +29,7 @@ #include "subsystems/abi.h" #include "subsystems/sensors/baro.h" #include "generated/airframe.h" +#include "math/pprz_isa.h" #ifndef QNH_BARO_ID #define QNH_BARO_ID ABI_BROADCAST @@ -41,15 +42,9 @@ void received_abs_baro_for_qnh(uint8_t sender_id, const float * pressure); void received_abs_baro_for_qnh(__attribute__((__unused__)) uint8_t sender_id, const float * pressure) { qnh.baro_pressure = *pressure; - const float L = 0.0065; // [K/m] - const float T0 = 288.15; // [K] - const float g = 9.80665; // [m/s^2] - const float M = 0.0289644; // [kg/mol] - const float R = 8.31447; // [J/(mol*K)] - const float InvExpo = R * L / g / M; const float MeterPerFeet = 0.3048; - float prel = qnh.baro_pressure / (qnh.qnh * 100.0f); - qnh.amsl_baro = (1 - pow(prel,InvExpo) ) * T0/L / MeterPerFeet; + qnh.amsl_baro = pprz_isa_height_of_pressure_full(qnh.baro_pressure, qnh.qnh * 100.0f) / + MeterPerFeet; qnh.baro_counter = 10; } @@ -77,15 +72,8 @@ void init_qnh(void) { void compute_qnh(void) { - const float L = 0.0065; // [K/m] - const float T0 = 288.15; // [K] - const float g = 9.80665; // [m/s^2] - const float M = 0.0289644; // [kg/mol] - const float R = 8.31447; // [J/(mol*K)] - const float Expo = g * M / R / L; float h = stateGetPositionLla_f()->alt; - float Trel = 1 - L*h/T0; - qnh.qnh = round(qnh.baro_pressure / pow(Trel,Expo) / 100.0f); + qnh.qnh = pprz_isa_ref_pressure_of_height_full(qnh.baro_pressure, h) / 100.0f; } float GetAmsl(void)