mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-31 12:23:23 +08:00
Merge pull request #853 from paparazzi/air_data_improvements
Converts air_data to a module and adds: - math functions for QNH, etc. from #837 - subscribe to BARO_DIFF via ABI - subscribe to TEMPERATURE via ABI - calculate airspeed from differential pressure if enabled (default) - calculate QNH from abs pressure and altitude (enabled by default) - calculate AMSL from baro pressure and QNH (disabled by default) - calculate true airspeed (TAS) from equivalent airspeed (EAS) using a `tas_factor` (with default to 1.0) - calculate `tas_factor` from ISA model and current pressure and temperature (enabled by default) - send new AIR_DATA and AMSL messages
This commit is contained in:
Felix Ruess
@@ -58,6 +58,7 @@ twog_1.0 + aspirin + ETS baro + ETS speed
|
||||
<define name="BARO_ETS_SYNC_SEND"/>
|
||||
</load>
|
||||
<load name="baro_sim.xml"/>
|
||||
<load name="air_data.xml"/>
|
||||
<load name="digital_cam.xml">
|
||||
<define name="DC_SHUTTER_GPIO" value="GPIOB,GPIO22"/><!-- 18:aux 22:camsw-->
|
||||
</load>
|
||||
|
||||
@@ -36,6 +36,7 @@
|
||||
<modules>
|
||||
<load name="airspeed_ets.xml"/>
|
||||
<load name="baro_ets.xml"/>
|
||||
<load name="air_data.xml"/>
|
||||
<load name="infrared_adc.xml"/>
|
||||
</modules>
|
||||
|
||||
|
||||
@@ -47,6 +47,7 @@
|
||||
<modules>
|
||||
<load name="sys_mon.xml"/>
|
||||
<load name="baro_sim.xml"/>
|
||||
<load name="air_data.xml"/>
|
||||
|
||||
<!-- extra navigation routines -->
|
||||
<load name="nav_bungee_takeoff.xml"/>
|
||||
|
||||
@@ -63,6 +63,7 @@
|
||||
</load>
|
||||
<load name="gps_ubx_ucenter.xml"/>
|
||||
<load name="geo_mag.xml"/>
|
||||
<load name="air_data.xml"/>
|
||||
</modules>
|
||||
|
||||
<servos driver="Pwm">
|
||||
|
||||
@@ -223,7 +223,7 @@
|
||||
|
||||
<load name="nav_catapult.xml"/>
|
||||
<load name="nav_line.xml"/>
|
||||
|
||||
<load name="air_data.xml"/>
|
||||
</modules>
|
||||
|
||||
<firmware name="fixedwing">
|
||||
|
||||
@@ -73,10 +73,8 @@ $(TARGET).srcs += subsystems/commands.c
|
||||
$(TARGET).srcs += state.c
|
||||
|
||||
#
|
||||
# AIR DATA and BARO (if needed)
|
||||
# BARO_BOARD (if existing/configured)
|
||||
#
|
||||
$(TARGET).srcs += subsystems/air_data.c
|
||||
|
||||
include $(CFG_SHARED)/baro_board.makefile
|
||||
|
||||
|
||||
|
||||
@@ -161,8 +161,6 @@ ap_srcs += state.c
|
||||
ap_srcs += subsystems/settings.c
|
||||
ap_srcs += $(SRC_ARCH)/subsystems/settings_arch.c
|
||||
|
||||
# AIR DATA
|
||||
ap_srcs += subsystems/air_data.c
|
||||
|
||||
# BARO
|
||||
include $(CFG_SHARED)/baro_board.makefile
|
||||
|
||||
+15
-2
@@ -1944,8 +1944,21 @@
|
||||
<field name="diff" type="float"/>
|
||||
</message>
|
||||
|
||||
<!--222 is free -->
|
||||
<!--223 is free -->
|
||||
<message name="AIR_DATA" id="222">
|
||||
<field name="pressure" type="float" unit="Pa"/>
|
||||
<field name="diff_p" type="float" unit="Pa"/>
|
||||
<field name="temp" type="float" unit="deg celcius"/>
|
||||
<field name="qnh" type="float" unit="hPa"/>
|
||||
<field name="amsl_baro" type="float" unit="m"/>
|
||||
<field name="airspeed" type="float" unit="m/s"/>
|
||||
<field name="tas_factor" type="float"/>
|
||||
</message>
|
||||
|
||||
<message name="AMSL" id="223">
|
||||
<field name="AMSL_BARO" type="float" unit="ft" alt_unit="m"/>
|
||||
<field name="AMSL_GPS" type="float" unit="ft" alt_unit="m"/>
|
||||
</message>
|
||||
|
||||
<!--224 is free -->
|
||||
|
||||
<message name="VIDEO_SYNC" id="225">
|
||||
|
||||
@@ -0,0 +1,41 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="air_data">
|
||||
<doc>
|
||||
<description>
|
||||
AirData interface.
|
||||
Provides an interface for baro absolute and differential pressure as well as airspeed.
|
||||
Subscribes to BARO_ABS, BARO_DIFF and TEMPERATURE ABI messages and calculates QNH and true airspeed from it.
|
||||
Also enables you to fly on "flight levels" using AMSL (AltitudeAboveSeaLevel) calculated from current pressure and QNH.
|
||||
|
||||
When using this module to provide airspeed you need a differential pressure sensor module publishing the BARO_DIFF ABI message. Make sure to disable other modules which otherwise directly set the airspeed in the state interface. E.g. when using the airspeed_ms45xx.xml module, define USE_AIRSPEED_MS45XX to FALSE.
|
||||
</description>
|
||||
<define name="AIR_DATA_BARO_ABS_ID" value="ABI_SENDER_ID" description="ABI sender id for absolute baro measurement (default: ABI_BROADCAST)"/>
|
||||
<define name="AIR_DATA_BARO_DIFF_ID" value="ABI_SENDER_ID" description="ABI sender id for differential baro measurement (default: ABI_BROADCAST)"/>
|
||||
<define name="AIR_DATA_TEMPERATURE_ID" value="ABI_SENDER_ID" description="ABI sender id for temperature measurement (default: ABI_BROADCAST)"/>
|
||||
<define name="AIR_DATA_CALC_AIRSPEED" value="FALSE|TRUE" description="Calculate Airspeed from differential pressure (default: TRUE)"/>
|
||||
<define name="AIR_DATA_CALC_TAS_FACTOR" value="FALSE|TRUE" description="Calculate TAS factor from temp and pressure (default: TRUE)"/>
|
||||
<define name="AIR_DATA_CALC_AMSL_BARO" value="FALSE|TRUE" description="Calculate AMS from baro and QNH (default: FALSE)"/>
|
||||
<define name="USE_AIRSPEED_AIR_DATA" value="TRUE|FALSE" description="set airspeed in state interface (defaults to TRUE if USE_AIRSPEED)"/>
|
||||
</doc>
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings name="air_data">
|
||||
<dl_setting min="800" max="1200" step="1" module="air_data/air_data" var="air_data.qnh" shortname="QNH"/>
|
||||
<dl_setting min="0.8" max="1.3" step="0.01" module="air_data/air_data" var="air_data.tas_factor" shortname="TASfactor" param="AIR_DATA_TAS_FACTOR"/>
|
||||
<dl_setting min="0" max="1" step="1" var="air_data.calc_qnh_once" module="air_data/air_data" shortname="calcQNH"/>
|
||||
<dl_setting min="0" max="1" step="1" var="air_data.calc_airspeed" module="air_data/air_data" shortname="calcAirspeed" param="AIR_DATA_CALC_AIRSPEED"/>
|
||||
<dl_setting min="0" max="1" step="1" var="air_data.calc_tas_factor" module="air_data/air_data" shortname="calcTASfactor" param="AIR_DATA_CALC_TAS_FACTOR"/>
|
||||
<dl_setting min="0" max="1" step="1" var="air_data.calc_amsl_baro" module="air_data/air_data" shortname="calcAMSL" param="AIR_DATA_CALC_AMSL_BARO"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
<header>
|
||||
<file name="air_data.h"/>
|
||||
</header>
|
||||
<init fun="air_data_init()"/>
|
||||
<periodic fun="air_data_periodic()" freq="10"/>
|
||||
<makefile>
|
||||
<file name="air_data.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -53,7 +53,6 @@
|
||||
#if USE_AHRS_ALIGNER
|
||||
#include "subsystems/ahrs/ahrs_aligner.h"
|
||||
#endif
|
||||
#include "subsystems/air_data.h"
|
||||
#if USE_BARO_BOARD
|
||||
#include "subsystems/sensors/baro.h"
|
||||
PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BOARD)
|
||||
@@ -205,7 +204,6 @@ void init_ap( void ) {
|
||||
register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status);
|
||||
#endif
|
||||
|
||||
air_data_init();
|
||||
#if USE_BARO_BOARD
|
||||
baro_init();
|
||||
#endif
|
||||
|
||||
@@ -52,7 +52,6 @@
|
||||
|
||||
#include "subsystems/imu.h"
|
||||
#include "subsystems/gps.h"
|
||||
#include "subsystems/air_data.h"
|
||||
|
||||
#if USE_BARO_BOARD
|
||||
#include "subsystems/sensors/baro.h"
|
||||
@@ -152,7 +151,6 @@ STATIC_INLINE void main_init( void ) {
|
||||
|
||||
radio_control_init();
|
||||
|
||||
air_data_init();
|
||||
#if USE_BARO_BOARD
|
||||
baro_init();
|
||||
#endif
|
||||
|
||||
+70
-14
@@ -42,20 +42,31 @@ 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)
|
||||
/** standard air density in kg/m^3 */
|
||||
#define PPRZ_ISA_AIR_DENSITY 1.225
|
||||
|
||||
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)
|
||||
{
|
||||
@@ -68,15 +79,19 @@ static inline float pprz_isa_altitude_of_pressure(float pressure)
|
||||
|
||||
/**
|
||||
* Get relative altitude from pressure (using simplified equation).
|
||||
* Given the current pressure and a reference pressure (at height=0),
|
||||
* calculate the height above the reference in meters.
|
||||
* If you pass QNH as reference pressure, you get the height above sea level.
|
||||
* Using QFE as reference pressure, you get height above the airfield.
|
||||
*
|
||||
* @param pressure current pressure in Pascal (Pa)
|
||||
* @param ref reference pressure (QFE) when height = 0
|
||||
* @return altitude in pressure in ISA conditions
|
||||
* @param ref_p reference pressure (QFE) when height=0 or QNH at sea level
|
||||
* @return height in m above reference in ISA conditions
|
||||
*/
|
||||
static inline float pprz_isa_height_of_pressure(float pressure, float ref)
|
||||
static inline float pprz_isa_height_of_pressure(float pressure, float ref_p)
|
||||
{
|
||||
if (pressure > 0. && ref > 0.) {
|
||||
return (PPRZ_ISA_M_OF_P_CONST * logf(ref / pressure));
|
||||
if (pressure > 0. && ref_p > 0.) {
|
||||
return (PPRZ_ISA_M_OF_P_CONST * logf(ref_p / pressure));
|
||||
} else {
|
||||
return 0.;
|
||||
}
|
||||
@@ -86,7 +101,7 @@ static inline float pprz_isa_height_of_pressure(float pressure, float ref)
|
||||
* Get pressure in Pa from absolute altitude (using simplified equation).
|
||||
*
|
||||
* @param altitude current absolute altitude in meters
|
||||
* @return static pressure in Pa in ISA conditions
|
||||
* @return static pressure in Pa at given altitude in ISA conditions
|
||||
*/
|
||||
static inline float pprz_isa_pressure_of_altitude(float altitude)
|
||||
{
|
||||
@@ -96,13 +111,54 @@ static inline float pprz_isa_pressure_of_altitude(float altitude)
|
||||
/**
|
||||
* Get pressure in Pa from height (using simplified equation).
|
||||
*
|
||||
* @param altitude current relative altitude in meters
|
||||
* @param ref reference pressure (QFE) when height = 0
|
||||
* @return static pressure in Pa in ISA conditions
|
||||
* @param height current height over reference (relative altitude) in meters
|
||||
* @param ref_p reference pressure (QFE or QNH) when height = 0
|
||||
* @return static pressure in Pa at given height in ISA conditions
|
||||
*/
|
||||
static inline float pprz_isa_pressure_of_height(float altitude, float ref)
|
||||
static inline float pprz_isa_pressure_of_height(float height, float ref_p)
|
||||
{
|
||||
return (ref * expf((-1. / PPRZ_ISA_M_OF_P_CONST) * altitude));
|
||||
return (ref_p * expf((-1. / PPRZ_ISA_M_OF_P_CONST) * height));
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Get relative altitude from pressure (using full equation).
|
||||
* Given the current pressure and a reference pressure (at height=0),
|
||||
* calculate the height above the reference in meters.
|
||||
* If you pass QNH as reference pressure, you get the height above sea level.
|
||||
* Using QFE as reference pressure, you get height above the airfield.
|
||||
*
|
||||
* @param pressure current pressure in Pascal (Pa)
|
||||
* @param ref_p reference pressure (QFE or QNH) in Pa
|
||||
* @return height above reference in m in ISA conditions
|
||||
*/
|
||||
static inline float pprz_isa_height_of_pressure_full(float pressure, float ref_p)
|
||||
{
|
||||
if (ref_p > 0.) {
|
||||
const float prel = pressure / ref_p;
|
||||
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 reference pressure (QFE or QNH) from current pressure and height.
|
||||
* (using full equation)
|
||||
*
|
||||
* @param pressure current pressure in Pascal (Pa)
|
||||
* @param height height above referece (sea level for QNH, airfield alt for QFE) in m
|
||||
* @return reference pressure at height=0 in Pa
|
||||
*/
|
||||
static inline float pprz_isa_ref_pressure_of_height_full(float pressure, float height)
|
||||
{
|
||||
// Trel = 1 - L*h/T0;
|
||||
const float Trel = 1.0 - PPRZ_ISA_TEMP_LAPS_RATE * height / 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);
|
||||
}
|
||||
|
||||
#ifdef __cplusplus
|
||||
|
||||
@@ -0,0 +1,304 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Gautier Hattenberger
|
||||
* 2014 Felix Ruess <felix.ruess@gmail.com>
|
||||
*
|
||||
* This file is part of paparazzi
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file modules/air_data/air_data.c
|
||||
* Air Data interface
|
||||
* - pressures
|
||||
* - airspeed
|
||||
* - angle of attack and sideslip
|
||||
* - wind
|
||||
*/
|
||||
|
||||
#include "modules/air_data/air_data.h"
|
||||
#include "subsystems/abi.h"
|
||||
#include "math/pprz_isa.h"
|
||||
#include "state.h"
|
||||
|
||||
|
||||
/** global AirData state
|
||||
*/
|
||||
struct AirData air_data;
|
||||
|
||||
/** ABI binding for absolute pressure
|
||||
*/
|
||||
#ifndef AIR_DATA_BARO_ABS_ID
|
||||
#define AIR_DATA_BARO_ABS_ID ABI_BROADCAST
|
||||
#endif
|
||||
static abi_event pressure_abs_ev;
|
||||
|
||||
/** ABI binding for differential pressure
|
||||
*/
|
||||
#ifndef AIR_DATA_BARO_DIFF_ID
|
||||
#define AIR_DATA_BARO_DIFF_ID ABI_BROADCAST
|
||||
#endif
|
||||
static abi_event pressure_diff_ev;
|
||||
|
||||
/** ABI binding for temperature
|
||||
*/
|
||||
#ifndef AIR_DATA_TEMPERATURE_ID
|
||||
#define AIR_DATA_TEMPERATURE_ID ABI_BROADCAST
|
||||
#endif
|
||||
static abi_event temperature_ev;
|
||||
|
||||
/** Default factor to convert estimated airspeed (EAS) to true airspeed (TAS) */
|
||||
#ifndef AIR_DATA_TAS_FACTOR
|
||||
#define AIR_DATA_TAS_FACTOR 1.0
|
||||
#endif
|
||||
|
||||
/** Calculate Airspeed from differential pressure by default */
|
||||
#ifndef AIR_DATA_CALC_AIRSPEED
|
||||
#define AIR_DATA_CALC_AIRSPEED TRUE
|
||||
#endif
|
||||
|
||||
/** Calculate tas_factor from temp and pressure by default */
|
||||
#ifndef AIR_DATA_CALC_TAS_FACTOR
|
||||
#define AIR_DATA_CALC_TAS_FACTOR TRUE
|
||||
#endif
|
||||
|
||||
/** Don't calculate AMSL from baro and QNH by default */
|
||||
#ifndef AIR_DATA_CALC_AMSL_BARO
|
||||
#define AIR_DATA_CALC_AMSL_BARO FALSE
|
||||
#endif
|
||||
|
||||
|
||||
#ifndef USE_AIRSPEED_AIR_DATA
|
||||
#if USE_AIRSPEED
|
||||
#define USE_AIRSPEED_AIR_DATA TRUE
|
||||
PRINT_CONFIG_MSG("USE_AIRSPEED_AIR_DATA automatically set to TRUE")
|
||||
#endif
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Internal variable to keep track of validity.
|
||||
*/
|
||||
|
||||
/** counter to check baro health */
|
||||
static uint8_t baro_health_counter;
|
||||
|
||||
|
||||
static void pressure_abs_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure)
|
||||
{
|
||||
air_data.pressure = *pressure;
|
||||
|
||||
// calculate QNH from pressure and absolute alitude if that is available
|
||||
if (air_data.calc_qnh_once && stateIsGlobalCoordinateValid()) {
|
||||
float h = stateGetPositionLla_f()->alt;
|
||||
air_data.qnh = pprz_isa_ref_pressure_of_height_full(air_data.pressure, h) / 100.f;
|
||||
air_data.calc_qnh_once = FALSE;
|
||||
}
|
||||
|
||||
if (air_data.calc_amsl_baro && air_data.qnh > 0) {
|
||||
air_data.amsl_baro = pprz_isa_height_of_pressure_full(air_data.pressure,
|
||||
air_data.qnh * 100.f);
|
||||
air_data.amsl_baro_valid = TRUE;
|
||||
}
|
||||
|
||||
/* reset baro health counter */
|
||||
baro_health_counter = 10;
|
||||
}
|
||||
|
||||
static void pressure_diff_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure)
|
||||
{
|
||||
air_data.differential = *pressure;
|
||||
if (air_data.calc_airspeed) {
|
||||
air_data.airspeed = tas_from_dynamic_pressure(air_data.differential);
|
||||
#if USE_AIRSPEED_AIR_DATA
|
||||
stateSetAirspeed_f(&air_data.airspeed);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
static void temperature_cb(uint8_t __attribute__((unused)) sender_id, const float *temp)
|
||||
{
|
||||
air_data.temperature = *temp;
|
||||
if (air_data.calc_tas_factor && baro_health_counter > 0 && air_data.pressure > 0) {
|
||||
air_data.tas_factor = get_tas_factor(air_data.pressure, air_data.temperature);
|
||||
}
|
||||
}
|
||||
|
||||
#if PERIODIC_TELEMETRY
|
||||
#include "subsystems/datalink/telemetry.h"
|
||||
|
||||
static void send_baro_raw(void)
|
||||
{
|
||||
DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice,
|
||||
&air_data.pressure, &air_data.differential);
|
||||
}
|
||||
|
||||
static void send_air_data(void)
|
||||
{
|
||||
DOWNLINK_SEND_AIR_DATA(DefaultChannel, DefaultDevice,
|
||||
&air_data.pressure, &air_data.differential,
|
||||
&air_data.temperature, &air_data.qnh,
|
||||
&air_data.amsl_baro, &air_data.airspeed,
|
||||
&air_data.tas_factor);
|
||||
}
|
||||
|
||||
static void send_amsl(void)
|
||||
{
|
||||
const float MeterPerFeet = 0.3048;
|
||||
float amsl_baro_ft = air_data.amsl_baro / MeterPerFeet;
|
||||
float amsl_gps_ft = stateGetPositionLla_f()->alt / MeterPerFeet;
|
||||
DOWNLINK_SEND_AMSL(DefaultChannel, DefaultDevice, &amsl_baro_ft, &amsl_gps_ft);
|
||||
}
|
||||
#endif
|
||||
|
||||
/** AirData initialization. Called at startup.
|
||||
* Bind ABI messages
|
||||
*/
|
||||
void air_data_init(void)
|
||||
{
|
||||
air_data.calc_airspeed = AIR_DATA_CALC_AIRSPEED;
|
||||
air_data.calc_tas_factor = AIR_DATA_CALC_TAS_FACTOR;
|
||||
air_data.calc_amsl_baro = AIR_DATA_CALC_AMSL_BARO;
|
||||
air_data.tas_factor = AIR_DATA_TAS_FACTOR;
|
||||
air_data.calc_qnh_once = TRUE;
|
||||
air_data.amsl_baro_valid = FALSE;
|
||||
|
||||
/* initialize the output variables
|
||||
* pressure, qnh, temperature and airspeed to invalid values,
|
||||
* rest to zero
|
||||
*/
|
||||
air_data.pressure = -1.0f;
|
||||
air_data.qnh = -1.0f;
|
||||
air_data.airspeed = -1.0f;
|
||||
air_data.temperature = -1000.0f;
|
||||
air_data.differential = 0.0f;
|
||||
air_data.amsl_baro = 0.0f;
|
||||
air_data.aoa = 0.0f;
|
||||
air_data.sideslip = 0.0f;
|
||||
air_data.wind_speed = 0.0f;
|
||||
air_data.wind_dir = 0.0f;
|
||||
|
||||
/* internal variables */
|
||||
baro_health_counter = 0;
|
||||
|
||||
AbiBindMsgBARO_ABS(AIR_DATA_BARO_ABS_ID, &pressure_abs_ev, pressure_abs_cb);
|
||||
AbiBindMsgBARO_DIFF(AIR_DATA_BARO_DIFF_ID, &pressure_diff_ev, pressure_diff_cb);
|
||||
AbiBindMsgTEMPERATURE(AIR_DATA_TEMPERATURE_ID, &temperature_ev, temperature_cb);
|
||||
|
||||
#if PERIODIC_TELEMETRY
|
||||
register_periodic_telemetry(DefaultPeriodic, "BARO_RAW", send_baro_raw);
|
||||
register_periodic_telemetry(DefaultPeriodic, "AIR_DATA", send_air_data);
|
||||
register_periodic_telemetry(DefaultPeriodic, "AMSL", send_amsl);
|
||||
#endif
|
||||
}
|
||||
|
||||
float air_data_get_amsl(void)
|
||||
{
|
||||
// If it has be calculated and baro is OK
|
||||
if (air_data.amsl_baro_valid) {
|
||||
return air_data.amsl_baro;
|
||||
}
|
||||
// Otherwise use real altitude (from GPS)
|
||||
return stateGetPositionLla_f()->alt;
|
||||
}
|
||||
|
||||
void air_data_periodic(void)
|
||||
{
|
||||
// Watchdog on baro
|
||||
if (baro_health_counter > 0) {
|
||||
baro_health_counter--;
|
||||
}
|
||||
else {
|
||||
air_data.amsl_baro_valid = FALSE;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Calculate equivalent airspeed from dynamic pressure.
|
||||
* Dynamic pressure @f$q@f$ (also called impact pressure) is the
|
||||
* difference between total(pitot) and static pressure.
|
||||
*
|
||||
* Airspeed from dynamic pressure:
|
||||
* @f[ v = \frac12 \sqrt{\frac{2q}{\rho}} @f]
|
||||
* with @f$\rho@f$ as air density.
|
||||
* Using standard sea level air density @f$\rho_0@f$ gives you equivalent airspeed (EAS).
|
||||
*
|
||||
* @param q dynamic pressure in Pa
|
||||
* @return equivalent airspeed in m/s
|
||||
*/
|
||||
float eas_from_dynamic_pressure(float q)
|
||||
{
|
||||
/* q (dynamic pressure) = total pressure - static pressure
|
||||
* q = 1/2*rho*speed^2
|
||||
* speed = sqrt(2*q/rho)
|
||||
* With rho = air density at sea level.
|
||||
* Lower bound of q at zero, no flying backwards guys...
|
||||
*/
|
||||
const float two_div_rho_0 = 2.0 / PPRZ_ISA_AIR_DENSITY;
|
||||
return sqrtf(Max(q * two_div_rho_0, 0));
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate true airspeed (TAS) factor.
|
||||
* TAS = tas_factor * EAS
|
||||
*
|
||||
* True airspeed (TAS) from equivalent airspeed (EAS):
|
||||
* @f[\mbox{TAS} = \mbox{EAS} \sqrt{\frac{\rho_0}{\rho}}@f]
|
||||
* and @f$ \frac{\rho_0}{\rho} = \frac{p_0T}{pT_0}@f$ where
|
||||
* - @f$p@f$ is the air pressure at the flight condition
|
||||
* - @f$p_0@f$ is the air pressure at sea level = 101325 Pa
|
||||
* - @f$T@f$ is the air temperature at the flight condition
|
||||
* - @f$T_0@f$ is the air temperature at sea level = 288.15 K
|
||||
*
|
||||
* @param p current air pressure in Pa
|
||||
* @param t current air temperature in degrees Celcius
|
||||
* @return tas factor
|
||||
*/
|
||||
float get_tas_factor(float p, float t)
|
||||
{
|
||||
/* factor to convert EAS to TAS:
|
||||
* sqrt(rho0 / rho) = sqrt((p0 * T) / (p * T0))
|
||||
* convert input temp to Kelvin
|
||||
*/
|
||||
return sqrtf((PPRZ_ISA_SEA_LEVEL_PRESSURE * (t + 274.15)) /
|
||||
(p * PPRZ_ISA_SEA_LEVEL_TEMP));
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate true airspeed from equivalent airspeed.
|
||||
*
|
||||
* True airspeed (TAS) from EAS:
|
||||
* TAS = air_data.tas_factor * EAS
|
||||
*
|
||||
* @param eas equivalent airspeed (EAS) in m/s
|
||||
* @return true airspeed in m/s
|
||||
*/
|
||||
float tas_from_eas(float eas)
|
||||
{
|
||||
return air_data.tas_factor * eas;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate true airspeed from dynamic pressure.
|
||||
* Dynamic pressure @f$q@f$ (also called impact pressure) is the
|
||||
* difference between total(pitot) and static pressure.
|
||||
*
|
||||
* @param q dynamic pressure in Pa
|
||||
* @return true airspeed in m/s
|
||||
*/
|
||||
float tas_from_dynamic_pressure(float q)
|
||||
{
|
||||
return tas_from_eas(eas_from_dynamic_pressure(q));
|
||||
}
|
||||
@@ -0,0 +1,114 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Gautier Hattenberger
|
||||
* 2014 Felix Ruess <felix.ruess@gmail.com>
|
||||
*
|
||||
* This file is part of paparazzi
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file modules/air_data/air_data.h
|
||||
* Air Data interface
|
||||
* - pressures
|
||||
* - airspeed
|
||||
* - angle of attack and sideslip
|
||||
* - wind
|
||||
*/
|
||||
|
||||
#ifndef AIR_DATA_H
|
||||
#define AIR_DATA_H
|
||||
|
||||
#include "std.h"
|
||||
|
||||
/** Air Data strucute */
|
||||
struct AirData {
|
||||
float pressure; ///< Static atmospheric pressure (Pa), -1 if unknown
|
||||
float differential; ///< Differential pressure (total - static pressure) (Pa)
|
||||
float temperature; ///< temperature in degrees Celcius, -1000 if unknown
|
||||
|
||||
float airspeed; ///< Conventional Air Speed in m/s, -1 if unknown
|
||||
float tas_factor; ///< factor to convert equivalent airspeed (EAS) to true airspeed (TAS)
|
||||
float qnh; ///< Barometric pressure adjusted to sea level in hPa, -1 if unknown
|
||||
float amsl_baro; ///< altitude above sea level in m from pressure and QNH
|
||||
bool_t amsl_baro_valid; ///< TRUE if #amsl_baro is currently valid
|
||||
bool_t calc_airspeed; ///< if TRUE, calculate airspeed from differential pressure
|
||||
bool_t calc_qnh_once; ///< flag to calculate QNH with next pressure measurement
|
||||
bool_t calc_amsl_baro; ///< if TRUE, calculate #amsl_baro
|
||||
bool_t calc_tas_factor; ///< if TRUE, calculate #tas_factor when getting a temp measurement
|
||||
|
||||
float aoa; ///< angle of attack (rad)
|
||||
float sideslip; ///< sideslip angle (rad)
|
||||
float wind_speed; ///< wind speed (m/s)
|
||||
float wind_dir; ///< wind direction (rad, 0 north, >0 clockwise)
|
||||
};
|
||||
|
||||
/** global AirData state
|
||||
*/
|
||||
extern struct AirData air_data;
|
||||
|
||||
/** AirData initialization. Called at startup.
|
||||
*/
|
||||
extern void air_data_init(void);
|
||||
|
||||
/** Check health. Needs to be called periodically.
|
||||
*/
|
||||
extern void air_data_periodic(void);
|
||||
|
||||
/** Return AMSL (altitude AboveSeaLevel).
|
||||
* If AMSL from baro is valid, return that, otherwise from gps.
|
||||
*/
|
||||
extern float air_data_get_amsl(void);
|
||||
|
||||
/**
|
||||
* Calculate equivalent airspeed from dynamic pressure.
|
||||
* Dynamic pressure @f$q@f$ (also called impact pressure) is the
|
||||
* difference between total(pitot) and static pressure.
|
||||
*
|
||||
* @param q dynamic pressure in Pa
|
||||
* @return equivalent airspeed in m/s
|
||||
*/
|
||||
extern float eas_from_dynamic_pressure(float q);
|
||||
|
||||
/**
|
||||
* Calculate true airspeed (TAS) factor.
|
||||
* TAS = tas_factor * EAS
|
||||
*
|
||||
* @param p current air pressure in Pa
|
||||
* @param t current air temperature in degrees Celcius
|
||||
* @return tas factor
|
||||
*/
|
||||
extern float get_tas_factor(float p, float t);
|
||||
|
||||
/**
|
||||
* Calculate true airspeed from equivalent airspeed.
|
||||
*
|
||||
* @param eas equivalent airspeed (EAS) in m/s
|
||||
* @return true airspeed in m/s
|
||||
*/
|
||||
extern float tas_from_eas(float eas);
|
||||
|
||||
/**
|
||||
* Calculate true airspeed from dynamic pressure.
|
||||
* Dynamic pressure @f$q@f$ (also called impact pressure) is the
|
||||
* difference between total(pitot) and static pressure.
|
||||
*
|
||||
* @param q dynamic pressure in Pa
|
||||
* @return true airspeed in m/s
|
||||
*/
|
||||
extern float tas_from_dynamic_pressure(float q);
|
||||
|
||||
|
||||
#endif
|
||||
@@ -1,68 +0,0 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Gautier Hattenberger
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file subsystems/air_data.c
|
||||
* Air Data interface
|
||||
* - pressures
|
||||
* - airspeed
|
||||
* - angle of attack and sideslip
|
||||
* - wind
|
||||
*/
|
||||
|
||||
#include "subsystems/air_data.h"
|
||||
#include "subsystems/abi.h"
|
||||
|
||||
/** global AirData state
|
||||
*/
|
||||
struct AirData air_data;
|
||||
|
||||
/** ABI bindings
|
||||
*/
|
||||
#ifndef AIR_DATA_BARO_ABS_ID
|
||||
#define AIR_DATA_BARO_ABS_ID ABI_BROADCAST
|
||||
#endif
|
||||
static abi_event pressure_abs_ev;
|
||||
|
||||
static void pressure_abs_cb(uint8_t __attribute__((unused)) sender_id, const float * pressure) {
|
||||
air_data.pressure = *pressure;
|
||||
}
|
||||
|
||||
#if PERIODIC_TELEMETRY
|
||||
#include "subsystems/datalink/telemetry.h"
|
||||
|
||||
static void send_baro_raw(void) {
|
||||
DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice,
|
||||
&air_data.pressure, &air_data.differential);
|
||||
}
|
||||
#endif
|
||||
|
||||
/** AirData initialization. Called at startup.
|
||||
* Bind ABI messages
|
||||
*/
|
||||
void air_data_init( void ) {
|
||||
AbiBindMsgBARO_ABS(AIR_DATA_BARO_ABS_ID, &pressure_abs_ev, pressure_abs_cb);
|
||||
|
||||
#if PERIODIC_TELEMETRY
|
||||
register_periodic_telemetry(DefaultPeriodic, "BARO_RAW", send_baro_raw);
|
||||
#endif
|
||||
}
|
||||
|
||||
@@ -1,56 +0,0 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Gautier Hattenberger
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file subsystems/air_data.h
|
||||
* Air Data interface
|
||||
* - pressures
|
||||
* - airspeed
|
||||
* - angle of attack and sideslip
|
||||
* - wind
|
||||
*/
|
||||
|
||||
#ifndef AIR_DATA_H
|
||||
#define AIR_DATA_H
|
||||
|
||||
#include "std.h"
|
||||
|
||||
/** Air Data strucute */
|
||||
struct AirData {
|
||||
float pressure; ///< Static atmospheric pressure (Pa)
|
||||
float differential; ///< Differential pressure (dynamic - static pressure) (Pa)
|
||||
float airspeed; ///< Conventional Air Speed (m/s)
|
||||
float aoa; ///< angle of attack (rad)
|
||||
float sideslip; ///< sideslip angle (rad)
|
||||
float wind_speed; ///< wind speed (m/s)
|
||||
float wind_dir; ///< wind direction (rad, 0 north, >0 clockwise)
|
||||
};
|
||||
|
||||
/** global AirData state
|
||||
*/
|
||||
extern struct AirData air_data;
|
||||
|
||||
/** AirData initialization. Called at startup.
|
||||
*/
|
||||
extern void air_data_init( void );
|
||||
|
||||
#endif /* AIR_DATA_H */
|
||||
|
||||
Reference in New Issue
Block a user