Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "modules/core/abi.h"
33 #include "math/pprz_isa.h"
34 #include "state.h"
35 #include "generated/airframe.h"
36 #include "pprzlink/dl_protocol.h"
37 
41 
44 #ifndef AIR_DATA_BARO_ABS_ID
45 #define AIR_DATA_BARO_ABS_ID ABI_BROADCAST
46 #endif
48 
51 #ifndef AIR_DATA_BARO_DIFF_ID
52 #define AIR_DATA_BARO_DIFF_ID ABI_BROADCAST
53 #endif
55 
58 #ifndef AIR_DATA_TEMPERATURE_ID
59 #define AIR_DATA_TEMPERATURE_ID ABI_BROADCAST
60 #endif
62 
65 #ifndef AIR_DATA_AIRSPEED_ID
66 #define AIR_DATA_AIRSPEED_ID ABI_BROADCAST
67 #endif
69 
72 #ifndef AIR_DATA_INCIDENCE_ID
73 #define AIR_DATA_INCIDENCE_ID ABI_BROADCAST
74 #endif
76 
78 #ifndef AIR_DATA_TAS_FACTOR
79 #define AIR_DATA_TAS_FACTOR 1.0
80 #endif
81 
83 #ifndef AIR_DATA_CALC_AIRSPEED
84 #define AIR_DATA_CALC_AIRSPEED TRUE
85 #endif
86 
88 #ifndef AIR_DATA_CALC_TAS_FACTOR
89 #define AIR_DATA_CALC_TAS_FACTOR TRUE
90 #endif
91 
93 #ifndef AIR_DATA_CALC_AMSL_BARO
94 #define AIR_DATA_CALC_AMSL_BARO FALSE
95 #endif
96 
97 
98 #ifndef USE_AIRSPEED_AIR_DATA
99 #if USE_AIRSPEED
100 #define USE_AIRSPEED_AIR_DATA TRUE
101 PRINT_CONFIG_MSG("USE_AIRSPEED_AIR_DATA automatically set to TRUE")
102 #endif
103 #endif
104 
105 /*
106  * Internal variable to keep track of validity.
107  */
108 
111 
112 
113 static void pressure_abs_cb(uint8_t __attribute__((unused)) sender_id, uint32_t __attribute__((unused)) stamp, float pressure)
114 {
116 
117  // calculate QNH from pressure and absolute altitude if that is available
120  // in the meantime use geoid separation at local reference frame origin
121  float geoid_separation = 0;
122  if (state.ned_initialized_f) {
123  geoid_separation = state.ned_origin_f.lla.alt - state.ned_origin_f.hmsl;
124  }
125  float h = stateGetPositionLla_f()->alt - geoid_separation;
127  air_data.calc_qnh_once = false;
128  }
129 
130  if (air_data.calc_amsl_baro && air_data.qnh > 0) {
132  air_data.qnh * 100.f);
133  air_data.amsl_baro_valid = true;
134  }
135 
136  /* reset baro health counter */
137  baro_health_counter = 10;
138 }
139 
140 static void pressure_diff_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
141 {
143  if (air_data.calc_airspeed) {
146 #if USE_AIRSPEED_AIR_DATA
148 #endif
149  }
150 }
151 
152 static void temperature_cb(uint8_t __attribute__((unused)) sender_id, float temp)
153 {
154  air_data.temperature = temp;
155  /* only calculate tas factor if enabled and we have airspeed and valid data */
157  air_data.pressure > 0) {
159  }
160 }
161 
162 static void airspeed_cb(uint8_t __attribute__((unused)) sender_id, float eas)
163 {
164  air_data.airspeed = eas;
165  if (air_data.calc_airspeed) {
167 #if USE_AIRSPEED_AIR_DATA
169 #endif
170  }
171 }
172 
173 static void incidence_cb(uint8_t __attribute__((unused)) sender_id, uint8_t flag, float aoa, float sideslip)
174 {
175  if (bit_is_set(flag, 0)) {
176  // update angle of attack
177  air_data.aoa = aoa;
179  }
180  if (bit_is_set(flag, 1)) {
181  // update sideslip angle
184  }
185 }
186 
187 #if PERIODIC_TELEMETRY
189 
190 static void send_baro_raw(struct transport_tx *trans, struct link_device *dev)
191 {
192  pprz_msg_send_BARO_RAW(trans, dev, AC_ID,
194 }
195 
196 static void send_air_data(struct transport_tx *trans, struct link_device *dev)
197 {
198  pprz_msg_send_AIR_DATA(trans, dev, AC_ID,
202  &air_data.tas);
203 }
204 
205 static void send_amsl(struct transport_tx *trans, struct link_device *dev)
206 {
207  const float MeterPerFeet = 0.3048;
208  float amsl_baro_ft = air_data.amsl_baro / MeterPerFeet;
209  float amsl_gps_ft = stateGetPositionLla_f()->alt / MeterPerFeet;
210  pprz_msg_send_AMSL(trans, dev, AC_ID, &amsl_baro_ft, &amsl_gps_ft);
211 }
212 #endif
213 
217 void air_data_init(void)
218 {
223  air_data.calc_qnh_once = true;
224  air_data.amsl_baro_valid = false;
225 
226  /* initialize the output variables
227  * pressure, qnh, temperature and airspeed to invalid values,
228  * rest to zero
229  */
230  air_data.pressure = -1.0f;
231  air_data.qnh = -1.0f;
232  air_data.airspeed = -1.0f;
233  air_data.tas = -1.0f;
234  air_data.temperature = -1000.0f;
235  air_data.differential = 0.0f;
236  air_data.amsl_baro = 0.0f;
237  air_data.aoa = 0.0f;
238  air_data.sideslip = 0.0f;
239  air_data.wind_speed = 0.0f;
240  air_data.wind_dir = 0.0f;
241 
242  /* internal variables */
244 
247  AbiBindMsgTEMPERATURE(AIR_DATA_TEMPERATURE_ID, &temperature_ev, temperature_cb);
248  AbiBindMsgAIRSPEED(AIR_DATA_AIRSPEED_ID, &airspeed_ev, airspeed_cb);
249  AbiBindMsgINCIDENCE(AIR_DATA_INCIDENCE_ID, &incidence_ev, incidence_cb);
250 
251 #if PERIODIC_TELEMETRY
255 #endif
256 }
257 
258 float air_data_get_amsl(void)
259 {
260  // If it has be calculated and baro is OK
262  return air_data.amsl_baro;
263  }
264  // Otherwise use real altitude (from GPS)
265  return stateGetPositionLla_f()->alt;
266 }
267 
269 {
270  // Watchdog on baro
271  if (baro_health_counter > 0) {
273  } else {
274  air_data.amsl_baro_valid = false;
275  }
276 }
277 
278 void air_data_parse_WIND_INFO(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf)
279 {
280  if (DL_WIND_INFO_ac_id(buf) != AC_ID) { return; }
281  uint8_t flags = DL_WIND_INFO_flags(buf);
282  struct FloatVect2 wind = { 0.f, 0.f };
283  float upwind = 0.f;
284  if (bit_is_set(flags, 0)) {
285  wind.x = DL_WIND_INFO_north(buf);
286  wind.y = DL_WIND_INFO_east(buf);
289  air_data.wind_dir = atan2f(wind.y, wind.x);
290  }
291  if (bit_is_set(flags, 1)) {
292  upwind = DL_WIND_INFO_up(buf);
294  }
295 #if !USE_AIRSPEED
296  if (bit_is_set(flags, 2)) {
297  air_data.tas = DL_WIND_INFO_airspeed(buf);
300  }
301 #endif
302 #ifdef WIND_INFO_RET
303  float airspeed = stateGetAirspeed_f();
304  pprz_msg_send_WIND_INFO_RET(trans, dev, AC_ID, &flags, &wind.y, &wind.x, &upwind, &airspeed);
305 #endif
306 }
307 
322 {
323  /* q (dynamic pressure) = total pressure - static pressure
324  * q = 1/2*rho*speed^2
325  * speed = sqrt(2*q/rho)
326  * With rho = air density at sea level.
327  * Lower bound of q at zero, no flying backwards guys...
328  */
329  const float two_div_rho_0 = 2.0 / PPRZ_ISA_AIR_DENSITY;
330  return sqrtf(Max(q * two_div_rho_0, 0));
331 }
332 
349 float get_tas_factor(float p, float t)
350 {
351  /* factor to convert EAS to TAS:
352  * sqrt(rho0 / rho) = sqrt((p0 * T) / (p * T0))
353  * convert input temp to Kelvin
354  */
355  return sqrtf((PPRZ_ISA_SEA_LEVEL_PRESSURE * KelvinOfCelsius(t)) /
357 }
358 
362 static void compute_tas_factor(void)
363 {
364  // update tas factor if requested
366  if (air_data.pressure > 0.f && air_data.temperature > -900.f) {
367  // compute air density from pressure and temperature
369  }
370  else {
371  // compute air density from altitude in ISA condition
372  const float z = air_data_get_amsl();
373  const float p = pprz_isa_pressure_of_altitude(z);
374  const float t = pprz_isa_temperature_of_altitude(z);
376  }
377  }
378 }
379 
389 float tas_from_eas(float eas)
390 {
392  return air_data.tas_factor * eas;
393 }
394 
404 float eas_from_tas(float tas)
405 {
407  return tas / air_data.tas_factor;
408 }
409 
419 {
421 }
AirData::amsl_baro
float amsl_baro
altitude above sea level in m from pressure and QNH
Definition: air_data.h:48
stateSetAirspeed_f
static void stateSetAirspeed_f(float airspeed)
Set airspeed (float).
Definition: state.h:1309
AirData::wind_dir
float wind_dir
wind direction (rad, 0 north, >0 clockwise)
Definition: air_data.h:58
AIR_DATA_CALC_TAS_FACTOR
#define AIR_DATA_CALC_TAS_FACTOR
Calculate tas_factor from temp and pressure by default.
Definition: air_data.c:89
send_air_data
static void send_air_data(struct transport_tx *trans, struct link_device *dev)
Definition: air_data.c:196
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
air_data
struct AirData air_data
global AirData state
Definition: air_data.c:40
tas_from_eas
float tas_from_eas(float eas)
Calculate true airspeed from equivalent airspeed.
Definition: air_data.c:389
eas_from_tas
float eas_from_tas(float tas)
Calculate equivalent airspeed from true airspeed.
Definition: air_data.c:404
AIR_DATA_CALC_AIRSPEED
#define AIR_DATA_CALC_AIRSPEED
Calculate Airspeed from differential pressure by default.
Definition: air_data.c:84
eas_from_dynamic_pressure
float eas_from_dynamic_pressure(float q)
Calculate equivalent airspeed from dynamic pressure.
Definition: air_data.c:321
air_data_init
void air_data_init(void)
AirData initialization.
Definition: air_data.c:217
h
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
Definition: UKF_Wind_Estimator.c:821
abi.h
AirData::tas_factor
float tas_factor
factor to convert equivalent airspeed (EAS) to true airspeed (TAS)
Definition: air_data.h:46
pressure_abs_cb
static void pressure_abs_cb(uint8_t sender_id, uint32_t stamp, float pressure)
Definition: air_data.c:113
send_baro_raw
static void send_baro_raw(struct transport_tx *trans, struct link_device *dev)
Definition: air_data.c:190
AIR_DATA_BARO_ABS_ID
#define AIR_DATA_BARO_ABS_ID
ABI binding for absolute pressure.
Definition: air_data.c:45
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:66
stateSetHorizontalWindspeed_f
static void stateSetHorizontalWindspeed_f(struct FloatVect2 *h_windspeed)
Set horizontal windspeed (float).
Definition: state.h:1291
air_data_periodic
void air_data_periodic(void)
Check health.
Definition: air_data.c:268
pprz_isa_ref_pressure_of_height_full
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
AirData::calc_amsl_baro
bool calc_amsl_baro
if TRUE, calculate amsl_baro
Definition: air_data.h:52
AIR_DATA_INCIDENCE_ID
#define AIR_DATA_INCIDENCE_ID
ABI binding for incidence angles.
Definition: air_data.c:73
pressure_diff_ev
static abi_event pressure_diff_ev
Definition: air_data.c:54
LtpDef_f::lla
struct LlaCoor_f lla
origin of local frame in LLA
Definition: pprz_geodetic_float.h:95
air_data_parse_WIND_INFO
void air_data_parse_WIND_INFO(struct link_device *dev, struct transport_tx *trans, uint8_t *buf)
Parse datalink wind info message.
Definition: air_data.c:278
pprz_isa_temperature_of_altitude
static float pprz_isa_temperature_of_altitude(float alt)
Get ISA temperature from a MSL altitude.
Definition: pprz_isa.h:181
FloatVect2
Definition: pprz_algebra_float.h:49
telemetry.h
AIR_DATA_TAS_FACTOR
#define AIR_DATA_TAS_FACTOR
Default factor to convert estimated airspeed (EAS) to true airspeed (TAS)
Definition: air_data.c:79
baro_health_counter
static uint8_t baro_health_counter
counter to check baro health
Definition: air_data.c:110
float_vect2_norm
static float float_vect2_norm(struct FloatVect2 *v)
Definition: pprz_algebra_float.h:139
pressure_abs_ev
static abi_event pressure_abs_ev
Definition: air_data.c:47
LtpDef_f::hmsl
float hmsl
Height above mean sea level in meters.
Definition: pprz_geodetic_float.h:97
AirData::calc_qnh_once
bool calc_qnh_once
flag to calculate QNH with next pressure measurement
Definition: air_data.h:51
airspeed_cb
static void airspeed_cb(uint8_t sender_id, float eas)
Definition: air_data.c:162
stateGetAirspeed_f
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
stateSetVerticalWindspeed_f
static void stateSetVerticalWindspeed_f(float v_windspeed)
Set vertical windspeed (float).
Definition: state.h:1300
airspeed_ev
static abi_event airspeed_ev
Definition: air_data.c:68
AirData::airspeed
float airspeed
Equivalent Air Speed (equals to Calibrated Air Speed at low speed/altitude) (in m/s,...
Definition: air_data.h:44
PPRZ_ISA_AIR_DENSITY
#define PPRZ_ISA_AIR_DENSITY
standard air density in kg/m^3
Definition: pprz_isa.h:64
AirData::qnh
float qnh
Barometric pressure adjusted to sea level in hPa, -1 if unknown.
Definition: air_data.h:47
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
AirData::sideslip
float sideslip
sideslip angle (rad)
Definition: air_data.h:56
State::ned_origin_f
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:220
temperature_ev
static abi_event temperature_ev
Definition: air_data.c:61
AIR_DATA_CALC_AMSL_BARO
#define AIR_DATA_CALC_AMSL_BARO
Don't calculate AMSL from baro and QNH by default.
Definition: air_data.c:94
pprz_isa_height_of_pressure_full
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
send_amsl
static void send_amsl(struct transport_tx *trans, struct link_device *dev)
Definition: air_data.c:205
AirData::calc_airspeed
bool calc_airspeed
if TRUE, calculate airspeed from differential pressure
Definition: air_data.h:50
stateSetAngleOfAttack_f
static void stateSetAngleOfAttack_f(float aoa)
Set angle of attack in radians (float).
Definition: state.h:1318
incidence_cb
static void incidence_cb(uint8_t sender_id, uint8_t flag, float aoa, float sideslip)
Definition: air_data.c:173
AirData::wind_speed
float wind_speed
wind speed (m/s)
Definition: air_data.h:57
CelsiusOfKelvin
#define CelsiusOfKelvin(_t)
Convert temperature from Kelvin to Celsius.
Definition: pprz_isa.h:71
AirData::differential
float differential
Differential pressure (total - static pressure) (Pa)
Definition: air_data.h:41
LlaCoor_f::alt
float alt
in meters (normally above WGS84 reference ellipsoid)
Definition: pprz_geodetic_float.h:57
AIR_DATA_BARO_DIFF_ID
#define AIR_DATA_BARO_DIFF_ID
ABI binding for differential pressure.
Definition: air_data.c:52
PPRZ_ISA_SEA_LEVEL_TEMP
#define PPRZ_ISA_SEA_LEVEL_TEMP
ISA sea level standard temperature in Kelvin.
Definition: pprz_isa.h:52
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
pprz_isa_pressure_of_altitude
static float pprz_isa_pressure_of_altitude(float altitude)
Get pressure in Pa from absolute altitude (using simplified equation).
Definition: pprz_isa.h:117
tas_from_dynamic_pressure
float tas_from_dynamic_pressure(float q)
Calculate true airspeed from dynamic pressure.
Definition: air_data.c:418
temperature_cb
static void temperature_cb(uint8_t sender_id, float temp)
Definition: air_data.c:152
PRINT_CONFIG_MSG
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
get_tas_factor
float get_tas_factor(float p, float t)
Calculate true airspeed (TAS) factor.
Definition: air_data.c:349
AirData::temperature
float temperature
temperature in degrees Celcius, -1000 if unknown
Definition: air_data.h:42
AirData::amsl_baro_valid
bool amsl_baro_valid
TRUE if amsl_baro is currently valid.
Definition: air_data.h:49
incidence_ev
static abi_event incidence_ev
Definition: air_data.c:75
AirData::pressure
float pressure
Static atmospheric pressure (Pa), -1 if unknown.
Definition: air_data.h:40
compute_tas_factor
static void compute_tas_factor(void)
Internal utility function to compute current tas factor if needed.
Definition: air_data.c:362
stateIsGlobalCoordinateValid
static bool stateIsGlobalCoordinateValid(void)
Test if global coordinates are valid.
Definition: state.h:515
AIR_DATA_TEMPERATURE_ID
#define AIR_DATA_TEMPERATURE_ID
ABI binding for temperature.
Definition: air_data.c:59
KelvinOfCelsius
#define KelvinOfCelsius(_t)
Convert temperature from Celsius to Kelvin.
Definition: pprz_isa.h:73
State::ned_initialized_f
bool ned_initialized_f
True if local float coordinate frame is initialsed.
Definition: state.h:223
FloatVect2::x
float x
Definition: pprz_algebra_float.h:50
AirData::aoa
float aoa
angle of attack (rad)
Definition: air_data.h:55
air_data.h
state.h
AirData
Air Data strucute.
Definition: air_data.h:39
pressure_diff_cb
static void pressure_diff_cb(uint8_t sender_id, float pressure)
Definition: air_data.c:140
stateGetPositionLla_f
static struct LlaCoor_f * stateGetPositionLla_f(void)
Get position in LLA coordinates (float).
Definition: state.h:728
state
struct State state
Definition: state.c:36
air_data_get_amsl
float air_data_get_amsl(void)
Return AMSL (altitude AboveSeaLevel).
Definition: air_data.c:258
AIR_DATA_AIRSPEED_ID
#define AIR_DATA_AIRSPEED_ID
ABI binding for airspeed.
Definition: air_data.c:66
AirData::calc_tas_factor
bool calc_tas_factor
if TRUE, calculate tas_factor when getting a temp measurement
Definition: air_data.h:53
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
p
static float p[2][2]
Definition: ins_alt_float.c:257
PPRZ_ISA_SEA_LEVEL_PRESSURE
#define PPRZ_ISA_SEA_LEVEL_PRESSURE
ISA sea level standard atmospheric pressure in Pascal.
Definition: pprz_isa.h:50
pprz_isa.h
Paparazzi atmospheric pressure conversion utilities.
AirData::tas
float tas
True Air Speed (TAS) in m/s, -1 if unknown.
Definition: air_data.h:45
stateSetSideslip_f
static void stateSetSideslip_f(float sideslip)
Set sideslip angle in radians (float).
Definition: state.h:1327