[modules][math] QNH functions

sw/airborne/math/pprz_isa.h

@@ -42,20 +42,29 @@ extern "C" {
 #include <math.h>
 
 // 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)
 {
@@ -71,7 +80,7 @@ static inline float pprz_isa_altitude_of_pressure(float pressure)
  *
  * @param pressure current pressure in Pascal (Pa)
  * @param ref reference pressure (QFE) when height = 0
- * @return altitude in pressure in ISA conditions
+ * @return altitude in m in ISA conditions
  */
 static inline float pprz_isa_height_of_pressure(float pressure, float ref)
 {
@@ -105,6 +114,60 @@ static inline float pprz_isa_pressure_of_height(float altitude, float ref)
   return (ref * expf((-1. / PPRZ_ISA_M_OF_P_CONST) * altitude));
 }
 
+
+/**
+ * Get absolute altitude from pressure and QNH (using simplified equation).
+ * Referrence pressure is QNH (pressure at sea level).
+ *
+ * @param pressure current pressure in Pascal (Pa)
+ * @param qnh pressure at sea level in hPa
+ * @return altitude in m in ISA conditions
+ */
+static inline float pprz_isa_altitude_of_pressure_qnh(float pressure, float qnh)
+{
+  if (pressure > 0.) {
+    return (PPRZ_ISA_M_OF_P_CONST * logf(qnh * 100.0f / pressure));
+  } else {
+    return 0.;
+  }
+}
+
+/**
+ * Get absolute altitude from pressure and QNH (using full equation).
+ * Referrence pressure is QNH (pressure at sea level).
+ *
+ * @param pressure current pressure in Pascal (Pa)
+ * @param qnh pressure at sea level in hPa
+ * @return altitude in m in ISA conditions
+ */
+static inline float pprz_isa_altitude_of_pressure_qnh_full(float pressure, float qnh)
+{
+  if (qnh > 0.) {
+    const float prel = pressure / (qnh * 100.0f);
+    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 QNH from current pressure and altitude (using full equation).
+ *
+ * @param pressure current pressure in Pascal (Pa)
+ * @param alt altitude in m in ISA conditions
+ * @return qnh pressure at sea level in hPa
+ */
+static inline float pprz_isa_qnh_of_pressure(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) / 100.0f;
+}
+
 #ifdef __cplusplus
 } /* extern "C" */
 #endif

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_altitude_of_pressure_qnh_full(qnh.baro_pressure, qnh.qnh) /
+    MeterPerFeet;
   qnh.baro_counter = 10;
 }
 
@@ -77,15 +72,7 @@ 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_qnh_of_pressure(qnh.baro_pressure, h);
 }
 
 float GetAmsl(void)