Paparazzi UAS  v5.15_devel-230-gc96ce27
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
air_data.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013 Gautier Hattenberger
3  * 2014 Felix Ruess <felix.ruess@gmail.com>
4  *
5  * This file is part of paparazzi
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, see
19  * <http://www.gnu.org/licenses/>.
20  */
21 
32 #include "subsystems/abi.h"
33 #include "math/pprz_isa.h"
34 #include "state.h"
35 #include "generated/airframe.h"
36 
40 
43 #ifndef AIR_DATA_BARO_ABS_ID
44 #define AIR_DATA_BARO_ABS_ID ABI_BROADCAST
45 #endif
47 
50 #ifndef AIR_DATA_BARO_DIFF_ID
51 #define AIR_DATA_BARO_DIFF_ID ABI_BROADCAST
52 #endif
54 
57 #ifndef AIR_DATA_TEMPERATURE_ID
58 #define AIR_DATA_TEMPERATURE_ID ABI_BROADCAST
59 #endif
61 
64 #ifndef AIR_DATA_AIRSPEED_ID
65 #define AIR_DATA_AIRSPEED_ID ABI_BROADCAST
66 #endif
68 
71 #ifndef AIR_DATA_INCIDENCE_ID
72 #define AIR_DATA_INCIDENCE_ID ABI_BROADCAST
73 #endif
75 
77 #ifndef AIR_DATA_TAS_FACTOR
78 #define AIR_DATA_TAS_FACTOR 1.0
79 #endif
80 
82 #ifndef AIR_DATA_CALC_AIRSPEED
83 #define AIR_DATA_CALC_AIRSPEED TRUE
84 #endif
85 
87 #ifndef AIR_DATA_CALC_TAS_FACTOR
88 #define AIR_DATA_CALC_TAS_FACTOR TRUE
89 #endif
90 
92 #ifndef AIR_DATA_CALC_AMSL_BARO
93 #define AIR_DATA_CALC_AMSL_BARO FALSE
94 #endif
95 
96 
97 #ifndef USE_AIRSPEED_AIR_DATA
98 #if USE_AIRSPEED
99 #define USE_AIRSPEED_AIR_DATA TRUE
100 PRINT_CONFIG_MSG("USE_AIRSPEED_AIR_DATA automatically set to TRUE")
101 #endif
102 #endif
103 
104 /*
105  * Internal variable to keep track of validity.
106  */
107 
110 
111 
112 static void pressure_abs_cb(uint8_t __attribute__((unused)) sender_id, uint32_t __attribute__((unused)) stamp, float pressure)
113 {
115 
116  // calculate QNH from pressure and absolute altitude if that is available
119  // in the meantime use geoid separation at local reference frame origin
120  float geoid_separation = 0;
121  if (state.ned_initialized_f) {
122  geoid_separation = state.ned_origin_f.lla.alt - state.ned_origin_f.hmsl;
123  }
124  float h = stateGetPositionLla_f()->alt - geoid_separation;
126  air_data.calc_qnh_once = false;
127  }
128 
129  if (air_data.calc_amsl_baro && air_data.qnh > 0) {
131  air_data.qnh * 100.f);
132  air_data.amsl_baro_valid = true;
133  }
134 
135  /* reset baro health counter */
136  baro_health_counter = 10;
137 }
138 
139 static void pressure_diff_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
140 {
142  if (air_data.calc_airspeed) {
145 #if USE_AIRSPEED_AIR_DATA
147 #endif
148  }
149 }
150 
151 static void temperature_cb(uint8_t __attribute__((unused)) sender_id, float temp)
152 {
153  air_data.temperature = temp;
154  /* only calculate tas factor if enabled and we have airspeed and valid data */
155  if (air_data.calc_tas_factor && air_data.airspeed > 0 && baro_health_counter > 0 &&
156  air_data.pressure > 0) {
158  }
159 }
160 
161 static void airspeed_cb(uint8_t __attribute__((unused)) sender_id, float eas)
162 {
163  air_data.airspeed = eas;
164  if (air_data.calc_airspeed) {
166 #if USE_AIRSPEED_AIR_DATA
168 #endif
169  }
170 }
171 
172 static void incidence_cb(uint8_t __attribute__((unused)) sender_id, uint8_t flag, float aoa, float sideslip)
173 {
174  if (bit_is_set(flag, 0)) {
175  // update angle of attack
176  air_data.aoa = aoa;
178  }
179  if (bit_is_set(flag, 1)) {
180  // update sideslip angle
182  stateSetSideslip_f(sideslip);
183  }
184 }
185 
186 #if PERIODIC_TELEMETRY
188 
189 static void send_baro_raw(struct transport_tx *trans, struct link_device *dev)
190 {
191  pprz_msg_send_BARO_RAW(trans, dev, AC_ID,
193 }
194 
195 static void send_air_data(struct transport_tx *trans, struct link_device *dev)
196 {
197  pprz_msg_send_AIR_DATA(trans, dev, AC_ID,
201  &air_data.tas);
202 }
203 
204 static void send_amsl(struct transport_tx *trans, struct link_device *dev)
205 {
206  const float MeterPerFeet = 0.3048;
207  float amsl_baro_ft = air_data.amsl_baro / MeterPerFeet;
208  float amsl_gps_ft = stateGetPositionLla_f()->alt / MeterPerFeet;
209  pprz_msg_send_AMSL(trans, dev, AC_ID, &amsl_baro_ft, &amsl_gps_ft);
210 }
211 #endif
212 
216 void air_data_init(void)
217 {
222  air_data.calc_qnh_once = true;
223  air_data.amsl_baro_valid = false;
224 
225  /* initialize the output variables
226  * pressure, qnh, temperature and airspeed to invalid values,
227  * rest to zero
228  */
229  air_data.pressure = -1.0f;
230  air_data.qnh = -1.0f;
231  air_data.airspeed = -1.0f;
232  air_data.tas = -1.0f;
233  air_data.temperature = -1000.0f;
234  air_data.differential = 0.0f;
235  air_data.amsl_baro = 0.0f;
236  air_data.aoa = 0.0f;
237  air_data.sideslip = 0.0f;
238  air_data.wind_speed = 0.0f;
239  air_data.wind_dir = 0.0f;
240 
241  /* internal variables */
242  baro_health_counter = 0;
243 
244  AbiBindMsgBARO_ABS(AIR_DATA_BARO_ABS_ID, &pressure_abs_ev, pressure_abs_cb);
245  AbiBindMsgBARO_DIFF(AIR_DATA_BARO_DIFF_ID, &pressure_diff_ev, pressure_diff_cb);
246  AbiBindMsgTEMPERATURE(AIR_DATA_TEMPERATURE_ID, &temperature_ev, temperature_cb);
247  AbiBindMsgAIRSPEED(AIR_DATA_AIRSPEED_ID, &airspeed_ev, airspeed_cb);
248  AbiBindMsgINCIDENCE(AIR_DATA_INCIDENCE_ID, &incidence_ev, incidence_cb);
249 
250 #if PERIODIC_TELEMETRY
254 #endif
255 }
256 
257 float air_data_get_amsl(void)
258 {
259  // If it has be calculated and baro is OK
261  return air_data.amsl_baro;
262  }
263  // Otherwise use real altitude (from GPS)
264  return stateGetPositionLla_f()->alt;
265 }
266 
268 {
269  // Watchdog on baro
270  if (baro_health_counter > 0) {
271  baro_health_counter--;
272  } else {
273  air_data.amsl_baro_valid = false;
274  }
275 }
276 
277 
292 {
293  /* q (dynamic pressure) = total pressure - static pressure
294  * q = 1/2*rho*speed^2
295  * speed = sqrt(2*q/rho)
296  * With rho = air density at sea level.
297  * Lower bound of q at zero, no flying backwards guys...
298  */
299  const float two_div_rho_0 = 2.0 / PPRZ_ISA_AIR_DENSITY;
300  return sqrtf(Max(q * two_div_rho_0, 0));
301 }
302 
319 float get_tas_factor(float p, float t)
320 {
321  /* factor to convert EAS to TAS:
322  * sqrt(rho0 / rho) = sqrt((p0 * T) / (p * T0))
323  * convert input temp to Kelvin
324  */
325  return sqrtf((PPRZ_ISA_SEA_LEVEL_PRESSURE * KelvinOfCelsius(t)) /
327 }
328 
338 float tas_from_eas(float eas)
339 {
340  // update tas factor if requested
342  if (air_data.pressure > 0.f && air_data.temperature > -900.f) {
343  // compute air density from pressure and temperature
345  }
346  else {
347  // compute air density from altitude in ISA condition
348  const float z = air_data_get_amsl();
349  const float p = pprz_isa_pressure_of_altitude(z);
350  const float t = pprz_isa_temperature_of_altitude(z);
352  }
353  }
354  return air_data.tas_factor * eas;
355 }
356 
366 {
368 }
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
static abi_event incidence_ev
Definition: air_data.c:74
float eas_from_dynamic_pressure(float q)
Calculate equivalent airspeed from dynamic pressure.
Definition: air_data.c:291
static float pprz_isa_pressure_of_altitude(float altitude)
Get pressure in Pa from absolute altitude (using simplified equation).
Definition: pprz_isa.h:117
static void send_amsl(struct transport_tx *trans, struct link_device *dev)
Definition: air_data.c:204
float tas
True Air Speed (TAS) in m/s, -1 if unknown.
Definition: air_data.h:43
Periodic telemetry system header (includes downlink utility and generated code).
#define AIR_DATA_CALC_TAS_FACTOR
Calculate tas_factor from temp and pressure by default.
Definition: air_data.c:88
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:220
static abi_event airspeed_ev
Definition: air_data.c:67
bool ned_initialized_f
True if local float coordinate frame is initialsed.
Definition: state.h:223
static void pressure_diff_cb(uint8_t sender_id, float pressure)
Definition: air_data.c:139
void air_data_periodic(void)
Check health.
Definition: air_data.c:267
float qnh
Barometric pressure adjusted to sea level in hPa, -1 if unknown.
Definition: air_data.h:45
Main include for ABI (AirBorneInterface).
static struct LlaCoor_f * stateGetPositionLla_f(void)
Get position in LLA coordinates (float).
Definition: state.h:728
static float pprz_isa_ref_pressure_of_height_full(float pressure, float height)
Get reference pressure (QFE or QNH) from current pressure and height.
Definition: pprz_isa.h:166
float air_data_get_amsl(void)
Return AMSL (altitude AboveSeaLevel).
Definition: air_data.c:257
bool calc_tas_factor
if TRUE, calculate tas_factor when getting a temp measurement
Definition: air_data.h:51
#define PPRZ_ISA_SEA_LEVEL_TEMP
ISA sea level standard temperature in Kelvin.
Definition: pprz_isa.h:52
#define AIR_DATA_BARO_ABS_ID
ABI binding for absolute pressure.
Definition: air_data.c:44
#define AIR_DATA_BARO_DIFF_ID
ABI binding for differential pressure.
Definition: air_data.c:51
static uint8_t baro_health_counter
counter to check baro health
Definition: air_data.c:109
float get_tas_factor(float p, float t)
Calculate true airspeed (TAS) factor.
Definition: air_data.c:319
bool calc_qnh_once
flag to calculate QNH with next pressure measurement
Definition: air_data.h:49
bool amsl_baro_valid
TRUE if amsl_baro is currently valid.
Definition: air_data.h:47
float airspeed
Equivalent Air Speed (equals to Calibrated Air Speed at low speed/altitude) (in m/s, -1 if unknown.
Definition: air_data.h:42
float tas_from_eas(float eas)
Calculate true airspeed from equivalent airspeed.
Definition: air_data.c:338
static void stateSetAngleOfAttack_f(float aoa)
Set angle of attack in radians (float).
Definition: state.h:1318
static abi_event pressure_diff_ev
Definition: air_data.c:53
#define AIR_DATA_TAS_FACTOR
Default factor to convert estimated airspeed (EAS) to true airspeed (TAS)
Definition: air_data.c:78
float hmsl
Height above mean sea level in meters.
#define PPRZ_ISA_AIR_DENSITY
standard air density in kg/m^3
Definition: pprz_isa.h:64
Paparazzi atmospheric pressure conversion utilities.
bool calc_airspeed
if TRUE, calculate airspeed from differential pressure
Definition: air_data.h:48
float tas_from_dynamic_pressure(float q)
Calculate true airspeed from dynamic pressure.
Definition: air_data.c:365
float wind_dir
wind direction (rad, 0 north, >0 clockwise)
Definition: air_data.h:56
unsigned long uint32_t
Definition: types.h:18
#define KelvinOfCelsius(_t)
Convert temperature from Celsius to Kelvin.
Definition: pprz_isa.h:73
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
Air Data interface.
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
#define AIR_DATA_CALC_AIRSPEED
Calculate Airspeed from differential pressure by default.
Definition: air_data.c:83
static float pprz_isa_height_of_pressure_full(float pressure, float ref_p)
Get relative altitude from pressure (using full equation).
Definition: pprz_isa.h:146
#define Max(x, y)
Definition: main_fbw.c:53
#define AIR_DATA_INCIDENCE_ID
ABI binding for incidence angles.
Definition: air_data.c:72
void air_data_init(void)
AirData initialization.
Definition: air_data.c:216
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
static void send_baro_raw(struct transport_tx *trans, struct link_device *dev)
Definition: air_data.c:189
float alt
in meters (normally above WGS84 reference ellipsoid)
#define PPRZ_ISA_SEA_LEVEL_PRESSURE
ISA sea level standard atmospheric pressure in Pascal.
Definition: pprz_isa.h:50
static bool stateIsGlobalCoordinateValid(void)
Test if global coordinates are valid.
Definition: state.h:515
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
static float pprz_isa_temperature_of_altitude(float alt)
Get ISA temperature from a MSL altitude.
Definition: pprz_isa.h:181
static void send_air_data(struct transport_tx *trans, struct link_device *dev)
Definition: air_data.c:195
Air Data strucute.
Definition: air_data.h:37
static void stateSetAirspeed_f(float airspeed)
Set airspeed (float).
Definition: state.h:1309
static void temperature_cb(uint8_t sender_id, float temp)
Definition: air_data.c:151
float pressure
Static atmospheric pressure (Pa), -1 if unknown.
Definition: air_data.h:38
bool calc_amsl_baro
if TRUE, calculate amsl_baro
Definition: air_data.h:50
#define AIR_DATA_AIRSPEED_ID
ABI binding for airspeed.
Definition: air_data.c:65
static float p[2][2]
#define CelsiusOfKelvin(_t)
Convert temperature from Kelvin to Celsius.
Definition: pprz_isa.h:71
static abi_event temperature_ev
Definition: air_data.c:60
static void pressure_abs_cb(uint8_t sender_id, uint32_t stamp, float pressure)
Definition: air_data.c:112
#define AIR_DATA_CALC_AMSL_BARO
Don't calculate AMSL from baro and QNH by default.
Definition: air_data.c:93
float tas_factor
factor to convert equivalent airspeed (EAS) to true airspeed (TAS)
Definition: air_data.h:44
float sideslip
sideslip angle (rad)
Definition: air_data.h:54
static void incidence_cb(uint8_t sender_id, uint8_t flag, float aoa, float sideslip)
Definition: air_data.c:172
float wind_speed
wind speed (m/s)
Definition: air_data.h:55
static void stateSetSideslip_f(float sideslip)
Set sideslip angle in radians (float).
Definition: state.h:1327
struct AirData air_data
global AirData state
Definition: air_data.c:39
float aoa
angle of attack (rad)
Definition: air_data.h:53
static abi_event pressure_abs_ev
Definition: air_data.c:46
float differential
Differential pressure (total - static pressure) (Pa)
Definition: air_data.h:39
#define AIR_DATA_TEMPERATURE_ID
ABI binding for temperature.
Definition: air_data.c:58
float amsl_baro
altitude above sea level in m from pressure and QNH
Definition: air_data.h:46
struct LlaCoor_f lla
origin of local frame in LLA
struct State state
Definition: state.c:36
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
float temperature
temperature in degrees Celcius, -1000 if unknown
Definition: air_data.h:40
static void airspeed_cb(uint8_t sender_id, float eas)
Definition: air_data.c:161