Paparazzi UAS  v5.12_stable-4-g9b43e9b
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 
70 #ifndef AIR_DATA_TAS_FACTOR
71 #define AIR_DATA_TAS_FACTOR 1.0
72 #endif
73 
75 #ifndef AIR_DATA_CALC_AIRSPEED
76 #define AIR_DATA_CALC_AIRSPEED TRUE
77 #endif
78 
80 #ifndef AIR_DATA_CALC_TAS_FACTOR
81 #define AIR_DATA_CALC_TAS_FACTOR TRUE
82 #endif
83 
85 #ifndef AIR_DATA_CALC_AMSL_BARO
86 #define AIR_DATA_CALC_AMSL_BARO FALSE
87 #endif
88 
89 
90 #ifndef USE_AIRSPEED_AIR_DATA
91 #if USE_AIRSPEED
92 #define USE_AIRSPEED_AIR_DATA TRUE
93 PRINT_CONFIG_MSG("USE_AIRSPEED_AIR_DATA automatically set to TRUE")
94 #endif
95 #endif
96 
97 /*
98  * Internal variable to keep track of validity.
99  */
100 
103 
104 
105 static void pressure_abs_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
106 {
108 
109  // calculate QNH from pressure and absolute altitude if that is available
112  // in the meantime use geoid separation at local reference frame origin
113  float geoid_separation = 0;
114  if (state.ned_initialized_f) {
115  geoid_separation = state.ned_origin_f.lla.alt - state.ned_origin_f.hmsl;
116  }
117  float h = stateGetPositionLla_f()->alt - geoid_separation;
119  air_data.calc_qnh_once = false;
120  }
121 
122  if (air_data.calc_amsl_baro && air_data.qnh > 0) {
124  air_data.qnh * 100.f);
125  air_data.amsl_baro_valid = true;
126  }
127 
128  /* reset baro health counter */
129  baro_health_counter = 10;
130 }
131 
132 static void pressure_diff_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
133 {
135  if (air_data.calc_airspeed) {
138 #if USE_AIRSPEED_AIR_DATA
140 #endif
141  }
142 }
143 
144 static void temperature_cb(uint8_t __attribute__((unused)) sender_id, float temp)
145 {
146  air_data.temperature = temp;
147  /* only calculate tas factor if enabled and we have airspeed and valid data */
148  if (air_data.calc_tas_factor && air_data.airspeed > 0 && baro_health_counter > 0 &&
149  air_data.pressure > 0) {
151  }
152 }
153 
154 static void airspeed_cb(uint8_t __attribute__((unused)) sender_id, float eas)
155 {
156  air_data.airspeed = eas;
157  if (air_data.calc_airspeed) {
159 #if USE_AIRSPEED_AIR_DATA
161 #endif
162  }
163 }
164 
165 #if PERIODIC_TELEMETRY
167 
168 static void send_baro_raw(struct transport_tx *trans, struct link_device *dev)
169 {
170  pprz_msg_send_BARO_RAW(trans, dev, AC_ID,
172 }
173 
174 static void send_air_data(struct transport_tx *trans, struct link_device *dev)
175 {
176  pprz_msg_send_AIR_DATA(trans, dev, AC_ID,
180  &air_data.tas);
181 }
182 
183 static void send_amsl(struct transport_tx *trans, struct link_device *dev)
184 {
185  const float MeterPerFeet = 0.3048;
186  float amsl_baro_ft = air_data.amsl_baro / MeterPerFeet;
187  float amsl_gps_ft = stateGetPositionLla_f()->alt / MeterPerFeet;
188  pprz_msg_send_AMSL(trans, dev, AC_ID, &amsl_baro_ft, &amsl_gps_ft);
189 }
190 #endif
191 
195 void air_data_init(void)
196 {
201  air_data.calc_qnh_once = true;
202  air_data.amsl_baro_valid = false;
203 
204  /* initialize the output variables
205  * pressure, qnh, temperature and airspeed to invalid values,
206  * rest to zero
207  */
208  air_data.pressure = -1.0f;
209  air_data.qnh = -1.0f;
210  air_data.airspeed = -1.0f;
211  air_data.tas = -1.0f;
212  air_data.temperature = -1000.0f;
213  air_data.differential = 0.0f;
214  air_data.amsl_baro = 0.0f;
215  air_data.aoa = 0.0f;
216  air_data.sideslip = 0.0f;
217  air_data.wind_speed = 0.0f;
218  air_data.wind_dir = 0.0f;
219 
220  /* internal variables */
221  baro_health_counter = 0;
222 
223  AbiBindMsgBARO_ABS(AIR_DATA_BARO_ABS_ID, &pressure_abs_ev, pressure_abs_cb);
224  AbiBindMsgBARO_DIFF(AIR_DATA_BARO_DIFF_ID, &pressure_diff_ev, pressure_diff_cb);
225  AbiBindMsgTEMPERATURE(AIR_DATA_TEMPERATURE_ID, &temperature_ev, temperature_cb);
226  AbiBindMsgAIRSPEED(AIR_DATA_AIRSPEED_ID, &airspeed_ev, airspeed_cb);
227 
228 #if PERIODIC_TELEMETRY
232 #endif
233 }
234 
235 float air_data_get_amsl(void)
236 {
237  // If it has be calculated and baro is OK
239  return air_data.amsl_baro;
240  }
241  // Otherwise use real altitude (from GPS)
242  return stateGetPositionLla_f()->alt;
243 }
244 
246 {
247  // Watchdog on baro
248  if (baro_health_counter > 0) {
249  baro_health_counter--;
250  } else {
251  air_data.amsl_baro_valid = false;
252  }
253 }
254 
255 
270 {
271  /* q (dynamic pressure) = total pressure - static pressure
272  * q = 1/2*rho*speed^2
273  * speed = sqrt(2*q/rho)
274  * With rho = air density at sea level.
275  * Lower bound of q at zero, no flying backwards guys...
276  */
277  const float two_div_rho_0 = 2.0 / PPRZ_ISA_AIR_DENSITY;
278  return sqrtf(Max(q * two_div_rho_0, 0));
279 }
280 
297 float get_tas_factor(float p, float t)
298 {
299  /* factor to convert EAS to TAS:
300  * sqrt(rho0 / rho) = sqrt((p0 * T) / (p * T0))
301  * convert input temp to Kelvin
302  */
303  return sqrtf((PPRZ_ISA_SEA_LEVEL_PRESSURE * (t + 274.15)) /
305 }
306 
316 float tas_from_eas(float eas)
317 {
318  return air_data.tas_factor * eas;
319 }
320 
330 {
332 }
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
float eas_from_dynamic_pressure(float q)
Calculate equivalent airspeed from dynamic pressure.
Definition: air_data.c:269
static void send_amsl(struct transport_tx *trans, struct link_device *dev)
Definition: air_data.c:183
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:81
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_abs_cb(uint8_t sender_id, float pressure)
Definition: air_data.c:105
static void pressure_diff_cb(uint8_t sender_id, float pressure)
Definition: air_data.c:132
void air_data_periodic(void)
Check health.
Definition: air_data.c:245
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:159
float air_data_get_amsl(void)
Return AMSL (altitude AboveSeaLevel).
Definition: air_data.c:235
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:102
float get_tas_factor(float p, float t)
Calculate true airspeed (TAS) factor.
Definition: air_data.c:297
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:316
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:71
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:329
float wind_dir
wind direction (rad, 0 north, >0 clockwise)
Definition: air_data.h:56
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:76
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:139
#define Max(x, y)
Definition: main_fbw.c:53
void air_data_init(void)
AirData initialization.
Definition: air_data.c:195
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:168
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 void send_air_data(struct transport_tx *trans, struct link_device *dev)
Definition: air_data.c:174
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:144
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]
static abi_event temperature_ev
Definition: air_data.c:60
#define AIR_DATA_CALC_AMSL_BARO
Don't calculate AMSL from baro and QNH by default.
Definition: air_data.c:86
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
float wind_speed
wind speed (m/s)
Definition: air_data.h:55
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:154