37#include "generated/airframe.h"
38#include "generated/modules.h"
39#include "pprzlink/dl_protocol.h"
56#ifndef AIR_DATA_BARO_CALIB
57#define AIR_DATA_BARO_CALIB {}
59#ifndef AIR_DATA_DIFF_CALIB
60#define AIR_DATA_DIFF_CALIB {}
62#ifndef AIR_DATA_AIRSPEED_CALIB
63#define AIR_DATA_AIRSPEED_CALIB {}
65#ifndef AIR_DATA_TEMP_CALIB
66#define AIR_DATA_TEMP_CALIB {}
70#ifndef AIR_DATA_BARO_ID
71#define AIR_DATA_BARO_ID ABI_BROADCAST
73#ifndef AIR_DATA_AIRSPEED_ID
74#define AIR_DATA_AIRSPEED_ID ABI_BROADCAST
76#ifndef AIR_DATA_TEMP_ID
77#define AIR_DATA_TEMP_ID ABI_BROADCAST
81#ifndef AIR_DATA_BARO_SEND_ID
82#define AIR_DATA_BARO_SEND_ID ABI_BROADCAST
84#ifndef AIR_DATA_AIRSPEED_SEND_ID
85#define AIR_DATA_AIRSPEED_SEND_ID ABI_BROADCAST
89#ifndef AIR_DATA_SEND_ALL_SENSORS
90#define AIR_DATA_SEND_ALL_SENSORS FALSE
96#ifndef AIR_DATA_TIMEOUT
97#define AIR_DATA_TIMEOUT 0.5f
99#ifndef AIR_DATA_TIMEOUT_FACTOR
100#define AIR_DATA_TIMEOUT_FACTOR 5.f
104#ifndef AIR_DATA_ALIGN_NB
105#define AIR_DATA_ALIGN_NB 20
109#ifndef AIR_DATA_DIFF_MAX
110#define AIR_DATA_DIFF_MAX 10000.f
114#ifndef AIR_DATA_CALIB_TIME
115#define AIR_DATA_CALIB_TIME 2.f
123#ifndef AIR_DATA_CALIB_AT_BOOT
124#define AIR_DATA_CALIB_AT_BOOT TRUE
126#ifndef AIR_DATA_CALIB_BOOT_DELAY
127#define AIR_DATA_CALIB_BOOT_DELAY 3.f
133#ifndef AIR_DATA_DIFF_REST_MAX
134#define AIR_DATA_DIFF_REST_MAX 20.f
138#ifndef AIR_DATA_CALIB_ARMED
139#define AIR_DATA_CALIB_ARMED FALSE
143#define AIR_DATA_IS_ESTIMATE(_id) ((_id) == AIRSPEED_WE_QUAD_ID)
146#ifndef USE_AIRSPEED_AIR_DATA
147#define USE_AIRSPEED_AIR_DATA TRUE
185 if (
s->scale == 0.f) {
194 float freq =
Min(
s->filter_freq, 0.4f /
s->dt);
197 s->filter_dt =
s->dt;
203 if (
s->filter_freq <= 0.f ||
s->dt <= 0.f) {
207 if (
s->filter_dt <= 0.f || fabsf(
s->dt -
s->filter_dt) > 0.2f *
s->filter_dt) {
240 float dt = now -
s->time;
241 s->dt = (
s->dt > 0.f) ? 0.9f *
s->dt + 0.1f *
dt :
dt;
268 for (
uint8_t k = 0; k < nb; k++) {
274 if (
s->abi_id ==
id) {
290 return s->calibrated.offset ? 0 : 1;
306 default:
return UINT8_MAX;
341 float geoid_separation = 0;
342 if (
state.ned_initialized_f) {
366#if USE_AIRSPEED_AIR_DATA
381 if (!isfinite(pressure) || pressure < 1000.f || pressure > 120000.f) {
390#if AIR_DATA_SEND_ALL_SENSORS
391 AbiSendMsgBARO_ABS(sender_id,
stamp,
s->value);
400 s->calib_sum += pressure -
air_data.pressure /
s->scale;
403 s->offset =
s->calib_sum /
s->calib_cnt;
404 s->calibrated.offset =
true;
420#if AIR_DATA_SEND_ALL_SENSORS
421 AbiSendMsgBARO_DIFF(sender_id,
s->value);
428 air_data.airspeed_filter =
s->filter_freq;
437 if (!isfinite(eas)) {
446#if AIR_DATA_SEND_ALL_SENSORS
448 AbiSendMsgAIRSPEED(sender_id,
s->value);
455 air_data.airspeed_filter =
s->filter_freq;
463 if (!isfinite(temp) || temp < -100.f || temp > 150.f) {
488 if (bit_is_set(flag, 0)) {
493 if (bit_is_set(flag, 1)) {
511 char msg[] =
"Air data calibration refused: armed";
517 for (
uint8_t k = 0; k < 2; k++) {
543 if (
s->calib_cnt == 0) {
546 float mean =
s->calib_sum /
s->calib_cnt;
549 s->calibrated.offset =
true;
553 int len = snprintf(msg,
sizeof(msg),
"Airspeed sensor %d: %d Pa at rest, check it or ZeroAirspeed",
554 s->abi_id, (
int)((mean -
s->offset) *
s->scale));
566 if (
ref != NULL &&
ref->calib_cnt > 0) {
567 float p_ref = (
ref->calib_sum /
ref->calib_cnt -
ref->offset) *
ref->scale;
569 ref->calibrated.offset =
true;
572 if (
s !=
ref &&
s->calib_cnt > 0 && !(
calib_boot &&
s->calibrated.offset)) {
573 s->offset =
s->calib_sum /
s->calib_cnt - p_ref /
s->scale;
574 s->calibrated.offset =
true;
604 s->filter_freq = Max(freq, 0.f);
605 air_data.airspeed_filter =
s->filter_freq;
606 if (
s->filter_freq > 0.f &&
s->filter_dt > 0.f) {
633 preflight_error(result,
"Airspeed sensor %d not zeroed, ZeroAirspeed",
s->abi_id);
644#if PERIODIC_TELEMETRY
651 return (t != NULL && t->
time > 0.f) ? t->
value : NAN;
679 pprz_msg_send_BARO_RAW(trans,
dev, AC_ID, &
s->abi_id, &
s->raw, &
s->value, &
s->offset, &temp);
690 bool is_diff =
false;
706 float diff = is_diff ?
s->value : 0.f;
708 pprz_msg_send_AIRSPEED_RAW(trans,
dev, AC_ID, &
s->abi_id, &
raw, &
s->offset, &diff, &temp, &airspeed);
713 pprz_msg_send_AIR_DATA(trans,
dev, AC_ID,
720static void send_amsl(
struct transport_tx *trans,
struct link_device *
dev)
722 const float MeterPerFeet = 0.3048;
723 float amsl_baro_ft =
air_data.amsl_baro / MeterPerFeet;
725 pprz_msg_send_AMSL(trans,
dev, AC_ID, &amsl_baro_ft, &amsl_gps_ft);
736 for (
uint8_t k = 0; k < 4; k++) {
746 air_data.diffs[i].calibrated.offset =
true;
784#if PERIODIC_TELEMETRY
825#if AIR_DATA_CALIB_AT_BOOT
826 static bool boot_calib_done =
false;
828 boot_calib_done =
true;
841 if (DL_WIND_INFO_ac_id(buf) != AC_ID) {
return; }
842 uint8_t flags = DL_WIND_INFO_flags(buf);
845 if (bit_is_set(flags, 0)) {
846 wind.
x = DL_WIND_INFO_north(buf);
847 wind.
y = DL_WIND_INFO_east(buf);
852 if (bit_is_set(flags, 1)) {
853 upwind = DL_WIND_INFO_up(buf);
857 if (bit_is_set(flags, 2)) {
858 air_data.tas = DL_WIND_INFO_airspeed(buf);
865 pprz_msg_send_WIND_INFO_RET(trans,
dev, AC_ID, &flags, &wind.
y, &wind.
x, &upwind, &airspeed);
896 return sqrtf(q * two_div_rho_0) *
sign;
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
Main include for ABI (AirBorneInterface).
#define TEMPERATURE_TYPE_DIFF
differential pressure (pitot) sensor
#define ABI_DISABLE
Reserved ABI ID to disable callback.
#define TEMPERATURE_TYPE_AIR
Types of the TEMPERATURE_RAW message, what the temperature measures.
struct abi_struct abi_event
#define TEMPERATURE_TYPE_BARO
barometer die
#define ABI_BROADCAST
Broadcast address.
#define TEMPERATURE_TYPE_OTHER
anything else
#define TEMPERATURE_TYPE_IMU
IMU die.
#define AIR_DATA_SENDER_ID
air_data: the calibrated and selected sensor on BARO_ABS, BARO_DIFF, AIRSPEED and TEMPERATURE.
float eas_from_dynamic_pressure(float q)
Calculate equivalent airspeed from dynamic pressure.
void air_data_ZeroAirspeed(bool zero)
Zero the differential pressures (setting handler).
void air_data_periodic(void)
Check health.
#define AIR_DATA_AIRSPEED_CALIB
static uint8_t air_data_rank_temp(struct air_data_sensor_t *s)
Temperature: first an air temperature probe, then a differential pressure sensor, then a barometer.
#define AIR_DATA_TIMEOUT_FACTOR
#define AIR_DATA_IS_ESTIMATE(_id)
Airspeed estimates (not sensors): only used when no sensor is healthy, and not sent as AIRSPEED.
static void diff_raw_cb(uint8_t sender_id, uint32_t stamp, float pressure)
static bool air_data_sensor_healthy(struct air_data_sensor_t *s, float now)
Is the sensor sending valid values.
static abi_event temp_raw_ev
static void send_airspeed_raw(struct transport_tx *trans, struct link_device *dev)
static uint8_t baro_health_counter
counter to check baro health
static void compute_tas_factor(void)
Internal utility function to compute the current tas factor.
#define AIR_DATA_BARO_CALIB
Calibration of the sensors in the airframe, in a section with prefix AIR_DATA_, for example <define n...
static float air_data_sensor_temp(uint8_t abi_id)
Temperature of a sensor (sent with the same ABI id), NAN if unknown.
#define AIR_DATA_DIFF_REST_MAX
Maximum differential pressure at rest (Pa), above it a warning is sent at boot and in the preflight c...
#define AIR_DATA_AIRSPEED_ID
struct AirData air_data
global AirData state Statically initialized, the persistent settings are loaded before air_data_init(...
float air_data_get_amsl(void)
Return AMSL (altitude AboveSeaLevel).
static abi_event airspeed_raw_ev
void air_data_SetAirspeedFilter(float freq)
#define AIR_DATA_AIRSPEED_SEND_ID
static struct air_data_sensor_t * air_data_select_baro(float now)
#define AIR_DATA_TEMP_CALIB
void air_data_CalibrateBaro(bool calibrate)
Line up the barometers with the one in use and reset the QFE (setting handler).
static void send_air_data(struct transport_tx *trans, struct link_device *dev)
static void air_data_sensor_defaults(struct air_data_sensor_t *s)
Defaults of the values not set in the airframe calibration.
float eas_from_tas(float tas)
Calculate equivalent airspeed from true airspeed.
static void airspeed_raw_cb(uint8_t sender_id, uint32_t stamp, float eas)
static uint8_t air_data_rank_baro(struct air_data_sensor_t *s)
Barometers: first the calibrated ones (a barometer that starts late is lined up before it is used).
#define AIR_DATA_DIFF_CALIB
static struct air_data_sensor_t * air_data_select(struct air_data_sensor_t *sensors[], uint8_t nb, uint8_t id, uint8_t(*rank)(struct air_data_sensor_t *s), float now)
Select a sensor: the one of the setting (id) if healthy, otherwise the healthy one with the lowest ra...
static float air_data_sensor_filter(struct air_data_sensor_t *s, float raw)
Low-pass filter of a raw value, the filter follows the rate of the sensor.
#define AIR_DATA_CALIB_TIME
Calibration time (s).
static struct air_data_sensor_t * air_data_select_temp(float now)
static float calib_start
Start time of the calibration (s).
static uint8_t air_data_rank_airspeed(struct air_data_sensor_t *s)
Airspeed: first the differential pressures, then the airspeed sensors, then an airspeed estimate.
static bool calib_baro
Line up the barometers.
static void air_data_finish_calibration(void)
Zero the differential pressures (otherwise only check the ones with an offset at rest) and/or line up...
float tas_from_dynamic_pressure(float q)
Calculate true airspeed from dynamic pressure.
static void incidence_cb(uint8_t sender_id, uint8_t flag, float aoa, float sideslip)
#define AIR_DATA_CALIB_BOOT_DELAY
static struct air_data_sensor_t * air_data_next(struct air_data_sensor_t *sensors, uint8_t *idx)
Next sensor in use for the round-robin telemetry, NULL if none.
#define AIR_DATA_DIFF_MAX
Maximum differential pressure (Pa), about 130 m/s.
static abi_event diff_raw_ev
#define AIR_DATA_CALIB_ARMED
Allow the calibration while the motors are armed.
static abi_event incidence_ev
static bool calib_running
Calibration state.
static void send_amsl(struct transport_tx *trans, struct link_device *dev)
void air_data_parse_WIND_INFO(struct link_device *dev, struct transport_tx *trans, uint8_t *buf)
Parse datalink wind info message.
static void temp_raw_cb(uint8_t sender_id, uint32_t stamp, float temp, uint8_t type)
static void air_data_set_pressure(uint32_t stamp, float pressure)
static void air_data_start_calibration(bool boot, bool baro, bool diff)
static void air_data_filter_init(struct air_data_sensor_t *s, float value)
(Re)initialize the low-pass filter with the measured interval, from its last output if running
static struct air_data_sensor_t * air_data_select_airspeed(float now)
#define AIR_DATA_ALIGN_NB
Number of values to line up a barometer that starts after the calibration.
#define AIR_DATA_TIMEOUT
A sensor is unhealthy without a valid value for AIR_DATA_TIMEOUT_FACTOR times its interval,...
static void send_baro_raw(struct transport_tx *trans, struct link_device *dev)
void air_data_init(void)
AirData initialization.
struct air_data_sensor_t * air_data_get_sensor(struct air_data_sensor_t *sensors, uint8_t abi_id, bool create)
Find (or create) the sensor of an ABI id.
static bool calib_diff
Zero the differential pressures (otherwise only check them at rest).
static void baro_raw_cb(uint8_t sender_id, uint32_t stamp, float pressure)
float get_tas_factor(float p, float t)
Calculate true airspeed (TAS) factor.
static void air_data_sensor_update(struct air_data_sensor_t *s, uint32_t stamp, float raw, float now)
Store a new raw value of a sensor.
float tas_from_eas(float eas)
Calculate true airspeed from equivalent airspeed.
static bool calib_boot
Calibration at boot: keep the barometer offsets from the airframe.
#define AIR_DATA_BARO_SEND_ID
Sensors in the BARO_RAW and AIRSPEED_RAW telemetry (ABI id), ABI_BROADCAST for all in turn,...
#define AIR_DATA_BARO_ID
Sensors to use (ABI id), ABI_BROADCAST for the first healthy one.
static void air_data_set_airspeed(float eas, bool send)
static abi_event baro_raw_ev
float dt
Filtered interval between the values (s), for the health check.
#define AIR_DATA_MAX_SENSORS
Maximum number of sensors of each kind (barometers, differential pressure, airspeed,...
uint8_t type
Temperature type (TEMPERATURE_TYPE_*), only for the temperatures.
bool offset
Offset calibrated (airframe or calibration).
float value
Last calibrated value (Pa, m/s or deg Celcius).
struct air_data_calib_t calibrated
Calibration bitmask.
uint8_t abi_id
ABI sender id of the driver, ABI_DISABLE when unused.
uint32_t stamp
Driver timestamp of the last value (us).
float time
System time of the last valid value (s), 0 if never.
float raw
Last raw value (Pa, m/s or deg Celcius).
One air data sensor: barometer, differential pressure, airspeed or temperature.
bool autopilot_get_motors_on(void)
get motors status
Core autopilot interface common to all firmwares.
Common code for AP and FBW telemetry.
static struct uart_periph * dev
static float float_vect2_norm(const struct FloatVect2 *v)
#define KelvinOfCelsius(_t)
Convert temperature from Celsius to Kelvin.
#define PPRZ_ISA_SEA_LEVEL_TEMP
ISA sea level standard temperature in Kelvin.
static float pprz_isa_pressure_of_altitude(float altitude)
Get pressure in Pa from absolute altitude (using simplified equation).
#define PPRZ_ISA_AIR_DENSITY
standard air density in kg/m^3
static float pprz_isa_temperature_of_altitude(float alt)
Get ISA temperature from a MSL altitude.
#define CelsiusOfKelvin(_t)
Convert temperature from Kelvin to Celsius.
#define PPRZ_ISA_SEA_LEVEL_PRESSURE
ISA sea level standard atmospheric pressure in Pascal.
static float pprz_isa_height_of_pressure_full(float pressure, float ref_p)
Get relative altitude from pressure (using full equation).
static float pprz_isa_ref_pressure_of_height_full(float pressure, float height)
Get reference pressure (QFE or QNH) from current pressure and height.
static struct LlaCoor_f * stateGetPositionLla_f(void)
Get position in LLA coordinates (float).
static bool stateIsGlobalCoordinateValid(void)
Test if global coordinates are valid.
float stateGetHmslOrigin_f(void)
Get the HMSL of the frame origin (float).
struct LlaCoor_f stateGetLlaOrigin_f(void)
Get the LLA position of the frame origin (float).
static void stateSetAngleOfAttack_f(uint16_t id, float aoa)
Set angle of attack in radians (float).
static void stateSetSideslip_f(uint16_t id, float sideslip)
Set sideslip angle in radians (float).
static void stateSetVerticalWindspeed_f(uint16_t id, float v_windspeed)
Set vertical windspeed (float).
static float stateGetAirspeed_f(void)
Get airspeed (float).
static void stateSetHorizontalWindspeed_f(uint16_t id, struct FloatVect2 *h_windspeed)
Set horizontal windspeed (float).
static void stateSetAirspeed_f(uint16_t id, float airspeed)
Set airspeed (float).
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, const float tau, const float sample_time, const float value)
Init a second order Butterworth filter.
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, const float value)
Update second order Butterworth low pass filter state with a new value.
static float get_butterworth_2_low_pass(const Butterworth2LowPass *filter)
Get current value of the second order Butterworth low pass filter.
static float sign(float x)
sign function
float alt
in meters (normally above WGS84 reference ellipsoid)
Paparazzi atmospheric pressure conversion utilities.
void preflight_error(struct preflight_result_t *result, const char *fmt,...)
Register a preflight error used inside the preflight checking functions.
void preflight_success(struct preflight_result_t *result, const char *fmt,...)
Register a preflight success used inside the preflight checking functions.
void preflight_warning(struct preflight_result_t *result, const char *fmt,...)
Register a preflight warning used inside the preflight checking functions.
void preflight_check_register(struct preflight_check_t *check, preflight_check_f func)
Register a preflight check and add it to the linked list.
API to get/set the generic vehicle states.
Architecture independent timing functions.
static float get_sys_time_float(void)
Get the time in seconds since startup.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.