Paparazzi UAS  v5.8.2_stable-0-g6260b7c
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 
63 #ifndef AIR_DATA_TAS_FACTOR
64 #define AIR_DATA_TAS_FACTOR 1.0
65 #endif
66 
68 #ifndef AIR_DATA_CALC_AIRSPEED
69 #define AIR_DATA_CALC_AIRSPEED TRUE
70 #endif
71 
73 #ifndef AIR_DATA_CALC_TAS_FACTOR
74 #define AIR_DATA_CALC_TAS_FACTOR TRUE
75 #endif
76 
78 #ifndef AIR_DATA_CALC_AMSL_BARO
79 #define AIR_DATA_CALC_AMSL_BARO FALSE
80 #endif
81 
82 
83 #ifndef USE_AIRSPEED_AIR_DATA
84 #if USE_AIRSPEED
85 #define USE_AIRSPEED_AIR_DATA TRUE
86 PRINT_CONFIG_MSG("USE_AIRSPEED_AIR_DATA automatically set to TRUE")
87 #endif
88 #endif
89 
90 /*
91  * Internal variable to keep track of validity.
92  */
93 
96 
97 
98 static void pressure_abs_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
99 {
101 
102  // calculate QNH from pressure and absolute altitude if that is available
105  // in the meantime use geoid separation at local reference frame origin
106  float geoid_separation = 0;
107  if (state.ned_initialized_f) {
108  geoid_separation = state.ned_origin_f.lla.alt - state.ned_origin_f.hmsl;
109  }
110  float h = stateGetPositionLla_f()->alt - geoid_separation;
113  }
114 
115  if (air_data.calc_amsl_baro && air_data.qnh > 0) {
117  air_data.qnh * 100.f);
119  }
120 
121  /* reset baro health counter */
122  baro_health_counter = 10;
123 }
124 
125 static void pressure_diff_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
126 {
128  if (air_data.calc_airspeed) {
131 #if USE_AIRSPEED_AIR_DATA
133 #endif
134  }
135 }
136 
137 static void temperature_cb(uint8_t __attribute__((unused)) sender_id, float temp)
138 {
139  air_data.temperature = temp;
140  /* only calculate tas factor if enabled and we have airspeed and valid data */
141  if (air_data.calc_tas_factor && air_data.airspeed > 0 && baro_health_counter > 0 &&
142  air_data.pressure > 0) {
144  }
145 }
146 
147 #if PERIODIC_TELEMETRY
149 
150 static void send_baro_raw(struct transport_tx *trans, struct link_device *dev)
151 {
152  pprz_msg_send_BARO_RAW(trans, dev, AC_ID,
154 }
155 
156 static void send_air_data(struct transport_tx *trans, struct link_device *dev)
157 {
158  pprz_msg_send_AIR_DATA(trans, dev, AC_ID,
162  &air_data.tas);
163 }
164 
165 static void send_amsl(struct transport_tx *trans, struct link_device *dev)
166 {
167  const float MeterPerFeet = 0.3048;
168  float amsl_baro_ft = air_data.amsl_baro / MeterPerFeet;
169  float amsl_gps_ft = stateGetPositionLla_f()->alt / MeterPerFeet;
170  pprz_msg_send_AMSL(trans, dev, AC_ID, &amsl_baro_ft, &amsl_gps_ft);
171 }
172 #endif
173 
177 void air_data_init(void)
178 {
185 
186  /* initialize the output variables
187  * pressure, qnh, temperature and airspeed to invalid values,
188  * rest to zero
189  */
190  air_data.pressure = -1.0f;
191  air_data.qnh = -1.0f;
192  air_data.airspeed = -1.0f;
193  air_data.tas = -1.0f;
194  air_data.temperature = -1000.0f;
195  air_data.differential = 0.0f;
196  air_data.amsl_baro = 0.0f;
197  air_data.aoa = 0.0f;
198  air_data.sideslip = 0.0f;
199  air_data.wind_speed = 0.0f;
200  air_data.wind_dir = 0.0f;
201 
202  /* internal variables */
203  baro_health_counter = 0;
204 
205  AbiBindMsgBARO_ABS(AIR_DATA_BARO_ABS_ID, &pressure_abs_ev, pressure_abs_cb);
206  AbiBindMsgBARO_DIFF(AIR_DATA_BARO_DIFF_ID, &pressure_diff_ev, pressure_diff_cb);
207  AbiBindMsgTEMPERATURE(AIR_DATA_TEMPERATURE_ID, &temperature_ev, temperature_cb);
208 
209 #if PERIODIC_TELEMETRY
210  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_BARO_RAW, send_baro_raw);
211  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIR_DATA, send_air_data);
212  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AMSL, send_amsl);
213 #endif
214 }
215 
216 float air_data_get_amsl(void)
217 {
218  // If it has be calculated and baro is OK
220  return air_data.amsl_baro;
221  }
222  // Otherwise use real altitude (from GPS)
223  return stateGetPositionLla_f()->alt;
224 }
225 
227 {
228  // Watchdog on baro
229  if (baro_health_counter > 0) {
230  baro_health_counter--;
231  } else {
233  }
234 }
235 
236 
251 {
252  /* q (dynamic pressure) = total pressure - static pressure
253  * q = 1/2*rho*speed^2
254  * speed = sqrt(2*q/rho)
255  * With rho = air density at sea level.
256  * Lower bound of q at zero, no flying backwards guys...
257  */
258  const float two_div_rho_0 = 2.0 / PPRZ_ISA_AIR_DENSITY;
259  return sqrtf(Max(q * two_div_rho_0, 0));
260 }
261 
278 float get_tas_factor(float p, float t)
279 {
280  /* factor to convert EAS to TAS:
281  * sqrt(rho0 / rho) = sqrt((p0 * T) / (p * T0))
282  * convert input temp to Kelvin
283  */
284  return sqrtf((PPRZ_ISA_SEA_LEVEL_PRESSURE * (t + 274.15)) /
286 }
287 
297 float tas_from_eas(float eas)
298 {
299  return air_data.tas_factor * eas;
300 }
301 
311 {
313 }
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:250
Generic transmission transport header.
Definition: transport.h:89
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:74
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:216
static void pressure_abs_cb(uint8_t sender_id, float pressure)
Definition: air_data.c:98
static void pressure_diff_cb(uint8_t sender_id, float pressure)
Definition: air_data.c:125
void air_data_periodic(void)
Check health.
Definition: air_data.c:226
float qnh
Barometric pressure adjusted to sea level in hPa, -1 if unknown.
Definition: air_data.h:45
bool_t amsl_baro_valid
TRUE if amsl_baro is currently valid.
Definition: air_data.h:47
Main include for ABI (AirBorneInterface).
bool_t ned_initialized_f
True if local float coordinate frame is initialsed.
Definition: state.h:219
static struct LlaCoor_f * stateGetPositionLla_f(void)
Get position in LLA coordinates (float).
Definition: state.h:711
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:216
#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:95
float get_tas_factor(float p, float t)
Calculate true airspeed (TAS) factor.
Definition: air_data.c:278
#define FALSE
Definition: std.h:5
static bool_t stateIsGlobalCoordinateValid(void)
Test if global coordinates are valid.
Definition: state.h:498
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:297
#define TRUE
Definition: std.h:4
#define Max(x, y)
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:64
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.
float tas_from_dynamic_pressure(float q)
Calculate true airspeed from dynamic pressure.
Definition: air_data.c:310
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:69
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
bool_t calc_amsl_baro
if TRUE, calculate amsl_baro
Definition: air_data.h:50
bool_t calc_airspeed
if TRUE, calculate airspeed from differential pressure
Definition: air_data.h:48
void air_data_init(void)
AirData initialization.
Definition: air_data.c:177
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:69
float alt
in meters above WGS84 reference ellipsoid
#define PPRZ_ISA_SEA_LEVEL_PRESSURE
ISA sea level standard atmospheric pressure in Pascal.
Definition: pprz_isa.h:50
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
Air Data strucute.
Definition: air_data.h:37
bool_t calc_qnh_once
flag to calculate QNH with next pressure measurement
Definition: air_data.h:49
static void stateSetAirspeed_f(float airspeed)
Set airspeed (float).
Definition: state.h:1254
static void temperature_cb(uint8_t sender_id, float temp)
Definition: air_data.c:137
float pressure
Static atmospheric pressure (Pa), -1 if unknown.
Definition: air_data.h:38
static float p[2][2]
bool_t calc_tas_factor
if TRUE, calculate tas_factor when getting a temp measurement
Definition: air_data.h:51
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:79
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