Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
30
32#include "modules/core/abi.h"
33#include "math/pprz_isa.h"
34#include "state.h"
35#include "autopilot.h"
36#include "mcu_periph/sys_time.h"
37#include "generated/airframe.h"
38#include "generated/modules.h"
39#include "pprzlink/dl_protocol.h"
41#include <string.h>
42#include <stdio.h>
43
56#ifndef AIR_DATA_BARO_CALIB
57#define AIR_DATA_BARO_CALIB {}
58#endif
59#ifndef AIR_DATA_DIFF_CALIB
60#define AIR_DATA_DIFF_CALIB {}
61#endif
62#ifndef AIR_DATA_AIRSPEED_CALIB
63#define AIR_DATA_AIRSPEED_CALIB {}
64#endif
65#ifndef AIR_DATA_TEMP_CALIB
66#define AIR_DATA_TEMP_CALIB {}
67#endif
68
70#ifndef AIR_DATA_BARO_ID
71#define AIR_DATA_BARO_ID ABI_BROADCAST
72#endif
73#ifndef AIR_DATA_AIRSPEED_ID
74#define AIR_DATA_AIRSPEED_ID ABI_BROADCAST
75#endif
76#ifndef AIR_DATA_TEMP_ID
77#define AIR_DATA_TEMP_ID ABI_BROADCAST
78#endif
79
81#ifndef AIR_DATA_BARO_SEND_ID
82#define AIR_DATA_BARO_SEND_ID ABI_BROADCAST
83#endif
84#ifndef AIR_DATA_AIRSPEED_SEND_ID
85#define AIR_DATA_AIRSPEED_SEND_ID ABI_BROADCAST
86#endif
87
89#ifndef AIR_DATA_SEND_ALL_SENSORS
90#define AIR_DATA_SEND_ALL_SENSORS FALSE
91#endif
92
96#ifndef AIR_DATA_TIMEOUT
97#define AIR_DATA_TIMEOUT 0.5f
98#endif
99#ifndef AIR_DATA_TIMEOUT_FACTOR
100#define AIR_DATA_TIMEOUT_FACTOR 5.f
101#endif
102
104#ifndef AIR_DATA_ALIGN_NB
105#define AIR_DATA_ALIGN_NB 20
106#endif
107
109#ifndef AIR_DATA_DIFF_MAX
110#define AIR_DATA_DIFF_MAX 10000.f
111#endif
112
114#ifndef AIR_DATA_CALIB_TIME
115#define AIR_DATA_CALIB_TIME 2.f
116#endif
117
123#ifndef AIR_DATA_CALIB_AT_BOOT
124#define AIR_DATA_CALIB_AT_BOOT TRUE
125#endif
126#ifndef AIR_DATA_CALIB_BOOT_DELAY
127#define AIR_DATA_CALIB_BOOT_DELAY 3.f
128#endif
129
133#ifndef AIR_DATA_DIFF_REST_MAX
134#define AIR_DATA_DIFF_REST_MAX 20.f
135#endif
136
138#ifndef AIR_DATA_CALIB_ARMED
139#define AIR_DATA_CALIB_ARMED FALSE
140#endif
141
143#define AIR_DATA_IS_ESTIMATE(_id) ((_id) == AIRSPEED_WE_QUAD_ID)
144
146#ifndef USE_AIRSPEED_AIR_DATA
147#define USE_AIRSPEED_AIR_DATA TRUE
148#endif
149
154 .baros = AIR_DATA_BARO_CALIB,
155 .diffs = AIR_DATA_DIFF_CALIB,
156 .airspeeds = AIR_DATA_AIRSPEED_CALIB,
157 .temps = AIR_DATA_TEMP_CALIB,
158 .baro_id = AIR_DATA_BARO_ID,
159 .airspeed_id = AIR_DATA_AIRSPEED_ID,
160 .temp_id = AIR_DATA_TEMP_ID,
161 .baro_send_id = AIR_DATA_BARO_SEND_ID,
162 .airspeed_send_id = AIR_DATA_AIRSPEED_SEND_ID,
163 .calib_armed = AIR_DATA_CALIB_ARMED,
164};
165
167
170
172static bool calib_running = false;
173static bool calib_boot = false;
174static bool calib_baro = false;
175static bool calib_diff = false;
176static float calib_start = 0.f;
177
178/*
179 * Sensors
180 */
181
184{
185 if (s->scale == 0.f) {
186 s->scale = 1.f;
187 }
188}
189
191static void air_data_filter_init(struct air_data_sensor_t *s, float value)
192{
193 // the bilinear transform needs a cut-off below the Nyquist frequency
194 float freq = Min(s->filter_freq, 0.4f / s->dt);
195 float init = (s->filter_dt > 0.f) ? get_butterworth_2_low_pass(&s->filter) : value;
196 init_butterworth_2_low_pass(&s->filter, 1.f / (2.f * M_PI * freq), s->dt, init);
197 s->filter_dt = s->dt;
198}
199
201static float air_data_sensor_filter(struct air_data_sensor_t *s, float raw)
202{
203 if (s->filter_freq <= 0.f || s->dt <= 0.f) {
204 s->filter_dt = 0.f;
205 return raw;
206 }
207 if (s->filter_dt <= 0.f || fabsf(s->dt - s->filter_dt) > 0.2f * s->filter_dt) {
209 }
210 return update_butterworth_2_low_pass(&s->filter, raw);
211}
212
214{
215 if (abi_id == ABI_DISABLE || abi_id == ABI_BROADCAST) {
216 return NULL;
217 }
218 // First the sensor itself (for example from the airframe calibration), then a free slot
219 for (uint8_t i = 0; i < AIR_DATA_MAX_SENSORS; i++) {
220 if (sensors[i].abi_id == abi_id) {
221 return &sensors[i];
222 }
223 }
224 if (create) {
225 for (uint8_t i = 0; i < AIR_DATA_MAX_SENSORS; i++) {
226 if (sensors[i].abi_id == ABI_DISABLE) {
227 sensors[i].abi_id = abi_id;
229 return &sensors[i];
230 }
231 }
232 }
233 return NULL;
234}
235
237static void air_data_sensor_update(struct air_data_sensor_t *s, uint32_t stamp, float raw, float now)
238{
239 if (s->time > 0.f) {
240 float dt = now - s->time;
241 s->dt = (s->dt > 0.f) ? 0.9f * s->dt + 0.1f * dt : dt;
242 }
243 s->raw = raw;
244 s->value = (air_data_sensor_filter(s, raw) - s->offset) * s->scale;
245 s->stamp = stamp;
246 s->time = now;
247 if (calib_running) {
248 s->calib_sum += raw;
249 s->calib_cnt++;
250 }
251}
252
254static bool air_data_sensor_healthy(struct air_data_sensor_t *s, float now)
255{
256 return s->abi_id != ABI_DISABLE && s->time > 0.f &&
257 (now - s->time) < Max(AIR_DATA_TIMEOUT, AIR_DATA_TIMEOUT_FACTOR * s->dt);
258}
259
264 uint8_t (*rank)(struct air_data_sensor_t *s), float now)
265{
266 struct air_data_sensor_t *best = NULL;
267 uint8_t best_rank = UINT8_MAX;
268 for (uint8_t k = 0; k < nb; k++) {
269 for (uint8_t i = 0; i < AIR_DATA_MAX_SENSORS; i++) {
270 struct air_data_sensor_t *s = &sensors[k][i];
271 if (!air_data_sensor_healthy(s, now)) {
272 continue;
273 }
274 if (s->abi_id == id) {
275 return s;
276 }
277 uint8_t r = rank(s);
278 if (r < best_rank) {
279 best = s;
280 best_rank = r;
281 }
282 }
283 }
284 return best;
285}
286
289{
290 return s->calibrated.offset ? 0 : 1;
291}
292
295{
296 return AIR_DATA_IS_ESTIMATE(s->abi_id) ? 1 : 0;
297}
298
301{
302 switch (s->type) {
303 case TEMPERATURE_TYPE_AIR: return 0;
304 case TEMPERATURE_TYPE_DIFF: return 1;
305 case TEMPERATURE_TYPE_BARO: return 2;
306 default: return UINT8_MAX;
307 }
308}
309
311{
312 struct air_data_sensor_t *sensors[] = {air_data.baros};
313 return air_data_select(sensors, 1, air_data.baro_id, air_data_rank_baro, now);
314}
315
317{
318 struct air_data_sensor_t *sensors[] = {air_data.diffs, air_data.airspeeds};
319 return air_data_select(sensors, 2, air_data.airspeed_id, air_data_rank_airspeed, now);
320}
321
323{
324 struct air_data_sensor_t *sensors[] = {air_data.temps};
325 return air_data_select(sensors, 1, air_data.temp_id, air_data_rank_temp, now);
326}
327
328
329/*
330 * Outputs of the sensors in use
331 */
332
333static void air_data_set_pressure(uint32_t stamp, float pressure)
334{
335 air_data.pressure = pressure;
336
337 // calculate QNH from pressure and absolute altitude if that is available
338 if (air_data.calc_qnh_once && stateIsGlobalCoordinateValid()) {
340 // in the meantime use geoid separation at local reference frame origin
341 float geoid_separation = 0;
342 if (state.ned_initialized_f) {
343 geoid_separation = stateGetLlaOrigin_f().alt - stateGetHmslOrigin_f();
344 }
345 float h = stateGetPositionLla_f()->alt - geoid_separation;
347 air_data.calc_qnh_once = false;
348 }
349
350 if (air_data.qnh > 0) {
352 air_data.qnh * 100.f);
353 air_data.amsl_baro_valid = true;
354 }
355
356 /* reset baro health counter */
358
359 AbiSendMsgBARO_ABS(AIR_DATA_SENDER_ID, stamp, pressure);
360}
361
362static void air_data_set_airspeed(float eas, bool send)
363{
364 air_data.airspeed = eas;
365 air_data.tas = tas_from_eas(air_data.airspeed);
366#if USE_AIRSPEED_AIR_DATA
367 stateSetAirspeed_f(MODULE_AIR_DATA_ID, air_data.airspeed);
368#endif
369 if (send) {
370 AbiSendMsgAIRSPEED(AIR_DATA_SENDER_ID, air_data.airspeed);
371 }
372}
373
374/*
375 * ABI callbacks of the drivers
376 */
377
378static void baro_raw_cb(uint8_t sender_id, uint32_t stamp, float pressure)
379{
380 // bad pressure (Pa), don't use
381 if (!isfinite(pressure) || pressure < 1000.f || pressure > 120000.f) {
382 return;
383 }
384 struct air_data_sensor_t *s = air_data_get_sensor(air_data.baros, sender_id, true);
385 if (s == NULL) {
386 return;
387 }
388 float now = get_sys_time_float();
389 air_data_sensor_update(s, stamp, pressure, now);
390#if AIR_DATA_SEND_ALL_SENSORS
391 AbiSendMsgBARO_ABS(sender_id, stamp, s->value);
392#endif
393
394 struct air_data_sensor_t *sel = air_data_select_baro(now);
395 air_data.baro_used = (sel != NULL) ? sel->abi_id : ABI_DISABLE;
396 if (sel == s) {
398 } else if (!calib_running && !s->calibrated.offset && sel != NULL && sel->calibrated.offset) {
399 // a barometer that starts after the calibration: line it up with the one in use
400 s->calib_sum += pressure - air_data.pressure / s->scale;
401 s->calib_cnt++;
402 if (s->calib_cnt >= AIR_DATA_ALIGN_NB) {
403 s->offset = s->calib_sum / s->calib_cnt;
404 s->calibrated.offset = true;
405 }
406 }
407}
408
409static void diff_raw_cb(uint8_t sender_id, uint32_t stamp, float pressure)
410{
411 if (!isfinite(pressure) || fabsf(pressure) > AIR_DATA_DIFF_MAX) {
412 return;
413 }
414 struct air_data_sensor_t *s = air_data_get_sensor(air_data.diffs, sender_id, true);
415 if (s == NULL) {
416 return;
417 }
418 float now = get_sys_time_float();
419 air_data_sensor_update(s, stamp, pressure, now);
420#if AIR_DATA_SEND_ALL_SENSORS
421 AbiSendMsgBARO_DIFF(sender_id, s->value);
422 AbiSendMsgAIRSPEED(sender_id, eas_from_dynamic_pressure(s->value));
423#endif
424
426 air_data.airspeed_used = (sel != NULL) ? sel->abi_id : ABI_DISABLE;
427 if (sel == s) {
428 air_data.airspeed_filter = s->filter_freq;
429 air_data.differential = s->value;
430 AbiSendMsgBARO_DIFF(AIR_DATA_SENDER_ID, s->value);
432 }
433}
434
435static void airspeed_raw_cb(uint8_t sender_id, uint32_t stamp, float eas)
436{
437 if (!isfinite(eas)) {
438 return;
439 }
440 struct air_data_sensor_t *s = air_data_get_sensor(air_data.airspeeds, sender_id, true);
441 if (s == NULL) {
442 return;
443 }
444 float now = get_sys_time_float();
445 air_data_sensor_update(s, stamp, eas, now);
446#if AIR_DATA_SEND_ALL_SENSORS
447 if (!AIR_DATA_IS_ESTIMATE(sender_id)) {
448 AbiSendMsgAIRSPEED(sender_id, s->value);
449 }
450#endif
451
453 air_data.airspeed_used = (sel != NULL) ? sel->abi_id : ABI_DISABLE;
454 if (sel == s) {
455 air_data.airspeed_filter = s->filter_freq;
456 // an estimate isn't sent, the estimators (e.g. ins_ekf2) would fuse it
457 air_data_set_airspeed(s->value, !AIR_DATA_IS_ESTIMATE(sender_id));
458 }
459}
460
461static void temp_raw_cb(uint8_t sender_id, uint32_t stamp, float temp, uint8_t type)
462{
463 if (!isfinite(temp) || temp < -100.f || temp > 150.f) {
464 return;
465 }
466 // only air temperatures: not the IMU and other ones (their ids also overlap with the baro ids)
468 return;
469 }
470 struct air_data_sensor_t *s = air_data_get_sensor(air_data.temps, sender_id, true);
471 if (s == NULL) {
472 return;
473 }
474 s->type = type;
475 float now = get_sys_time_float();
476 air_data_sensor_update(s, stamp, temp, now);
477
478 struct air_data_sensor_t *sel = air_data_select_temp(now);
479 air_data.temp_used = (sel != NULL) ? sel->abi_id : ABI_DISABLE;
480 if (sel == s) {
481 air_data.temperature = s->value;
482 AbiSendMsgTEMPERATURE(AIR_DATA_SENDER_ID, air_data.temperature);
483 }
484}
485
486static void incidence_cb(uint8_t __attribute__((unused)) sender_id, uint8_t flag, float aoa, float sideslip)
487{
488 if (bit_is_set(flag, 0)) {
489 // update angle of attack
490 air_data.aoa = aoa;
491 stateSetAngleOfAttack_f(MODULE_AIR_DATA_ID, aoa);
492 }
493 if (bit_is_set(flag, 1)) {
494 // update sideslip angle
495 air_data.sideslip = sideslip;
496 stateSetSideslip_f(MODULE_AIR_DATA_ID, sideslip);
497 }
498}
499
500/*
501 * Calibration
502 */
503
504static void air_data_start_calibration(bool boot, bool baro, bool diff)
505{
506 air_data.calibrate_baro = false;
507 air_data.zero_airspeed = false;
508 if (autopilot_get_motors_on() && !air_data.calib_armed) {
509 // refused while armed
510#if DOWNLINK
511 char msg[] = "Air data calibration refused: armed";
512 DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, strlen(msg), msg);
513#endif
514 return;
515 }
516 struct air_data_sensor_t *sensors[] = {air_data.baros, air_data.diffs};
517 for (uint8_t k = 0; k < 2; k++) {
518 for (uint8_t i = 0; i < AIR_DATA_MAX_SENSORS; i++) {
519 sensors[k][i].calib_sum = 0.f;
520 sensors[k][i].calib_cnt = 0;
521 }
522 }
523 calib_boot = boot;
524 calib_baro = baro;
525 calib_diff = diff;
527 calib_running = true;
528 air_data.calibrate_baro = baro;
529 air_data.zero_airspeed = diff;
530}
531
536{
537 calib_running = false;
538 air_data.calibrate_baro = false;
539 air_data.zero_airspeed = false;
540
541 for (uint8_t i = 0; i < AIR_DATA_MAX_SENSORS; i++) {
542 struct air_data_sensor_t *s = &air_data.diffs[i];
543 if (s->calib_cnt == 0) {
544 continue;
545 }
546 float mean = s->calib_sum / s->calib_cnt;
547 if (calib_diff) {
548 s->offset = mean;
549 s->calibrated.offset = true;
550 } else if (s->calibrated.offset && fabsf((mean - s->offset) * s->scale) > AIR_DATA_DIFF_REST_MAX) {
551#if DOWNLINK
552 char msg[64];
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));
555 if (len > 0) {
556 DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, Min(len, (int)sizeof(msg) - 1), msg);
557 }
558#endif
559 }
560 }
561
562 if (!calib_baro) {
563 return;
564 }
566 if (ref != NULL && ref->calib_cnt > 0) {
567 float p_ref = (ref->calib_sum / ref->calib_cnt - ref->offset) * ref->scale;
568 air_data.qfe = p_ref;
569 ref->calibrated.offset = true;
570 for (uint8_t i = 0; i < AIR_DATA_MAX_SENSORS; i++) {
571 struct air_data_sensor_t *s = &air_data.baros[i];
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;
575 }
576 }
577 air_data.calc_qnh_once = true;
578 }
579}
580
581void air_data_CalibrateBaro(bool calibrate)
582{
583 if (calibrate && !calib_running) {
584 air_data_start_calibration(false, true, false);
585 }
586}
587
589{
590 if (zero && !calib_running) {
591 air_data_start_calibration(false, false, true);
592 }
593}
594
596{
597 struct air_data_sensor_t *s = air_data_get_sensor(air_data.diffs, air_data.airspeed_used, false);
598 if (s == NULL) {
599 s = air_data_get_sensor(air_data.airspeeds, air_data.airspeed_used, false);
600 }
601 if (s == NULL) {
602 return;
603 }
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) {
607 air_data_filter_init(s, s->raw);
608 }
609}
610
611/*
612 * Checks and telemetry
613 */
614
615#if PREFLIGHT_CHECKS
617
618static struct preflight_check_t air_data_pfc;
619
620static void air_data_preflight(struct preflight_result_t *result)
621{
622 uint16_t fail_cnt = result->fail_cnt;
623 uint16_t warning_cnt = result->warning_cnt;
624 if (calib_running) {
625 preflight_error(result, "Air data calibrating");
626 }
627 // Not zeroed (not in DIFF_CALIB): ZeroAirspeed before arming
628 // At rest before arming: warn on a large differential pressure (wrong offset, broken sensor or wind), don't block
629 float now = get_sys_time_float();
630 for (uint8_t i = 0; i < AIR_DATA_MAX_SENSORS; i++) {
631 struct air_data_sensor_t *s = &air_data.diffs[i];
632 if (s->abi_id != ABI_DISABLE && !s->calibrated.offset) {
633 preflight_error(result, "Airspeed sensor %d not zeroed, ZeroAirspeed", s->abi_id);
634 } else if (air_data_sensor_healthy(s, now) && fabsf(s->value) > AIR_DATA_DIFF_REST_MAX) {
635 preflight_warning(result, "Airspeed sensor %d: %d Pa at rest", s->abi_id, (int)s->value);
636 }
637 }
638 if (result->fail_cnt == fail_cnt && result->warning_cnt == warning_cnt) {
639 preflight_success(result, "Air data ok");
640 }
641}
642#endif
643
644#if PERIODIC_TELEMETRY
646
649{
650 struct air_data_sensor_t *t = air_data_get_sensor(air_data.temps, abi_id, false);
651 return (t != NULL && t->time > 0.f) ? t->value : NAN;
652}
653
656{
657 for (uint8_t i = 0; i < AIR_DATA_MAX_SENSORS; i++) {
658 uint8_t j = (*idx + i) % AIR_DATA_MAX_SENSORS;
659 if (sensors[j].abi_id != ABI_DISABLE) {
660 *idx = (j + 1) % AIR_DATA_MAX_SENSORS;
661 return &sensors[j];
662 }
663 }
664 return NULL;
665}
666
667static void send_baro_raw(struct transport_tx *trans, struct link_device *dev)
668{
669 static uint8_t idx = 0;
670 if (air_data.baro_send_id == ABI_DISABLE) {
671 return;
672 }
673 struct air_data_sensor_t *s = (air_data.baro_send_id == ABI_BROADCAST) ? air_data_next(air_data.baros, &idx) :
674 air_data_get_sensor(air_data.baros, air_data.baro_send_id, false);
675 if (s == NULL) {
676 return;
677 }
678 float temp = air_data_sensor_temp(s->abi_id);
679 pprz_msg_send_BARO_RAW(trans, dev, AC_ID, &s->abi_id, &s->raw, &s->value, &s->offset, &temp);
680}
681
682static void send_airspeed_raw(struct transport_tx *trans, struct link_device *dev)
683{
684 // the differential pressure sensors, then the airspeed sensors
685 static uint8_t idx = 0;
686 if (air_data.airspeed_send_id == ABI_DISABLE) {
687 return;
688 }
689 struct air_data_sensor_t *s = NULL;
690 bool is_diff = false;
691 for (uint8_t i = 0; i < 2 * AIR_DATA_MAX_SENSORS && s == NULL; i++) {
692 uint8_t j = (idx + i) % (2 * AIR_DATA_MAX_SENSORS);
693 is_diff = (j < AIR_DATA_MAX_SENSORS);
694 struct air_data_sensor_t *c = is_diff ? &air_data.diffs[j] : &air_data.airspeeds[j - AIR_DATA_MAX_SENSORS];
695 if (c->abi_id != ABI_DISABLE &&
696 (air_data.airspeed_send_id == ABI_BROADCAST || c->abi_id == air_data.airspeed_send_id)) {
697 s = c;
698 idx = (j + 1) % (2 * AIR_DATA_MAX_SENSORS);
699 }
700 }
701 if (s == NULL) {
702 return;
703 }
704 uint16_t raw = 0;
705 float temp = air_data_sensor_temp(s->abi_id);
706 float diff = is_diff ? s->value : 0.f;
707 float airspeed = is_diff ? eas_from_dynamic_pressure(s->value) : s->value;
708 pprz_msg_send_AIRSPEED_RAW(trans, dev, AC_ID, &s->abi_id, &raw, &s->offset, &diff, &temp, &airspeed);
709}
710
711static void send_air_data(struct transport_tx *trans, struct link_device *dev)
712{
713 pprz_msg_send_AIR_DATA(trans, dev, AC_ID,
714 &air_data.pressure, &air_data.differential,
715 &air_data.temperature, &air_data.qnh,
716 &air_data.amsl_baro, &air_data.airspeed,
717 &air_data.tas, &air_data.baro_used, &air_data.airspeed_used);
718}
719
720static void send_amsl(struct transport_tx *trans, struct link_device *dev)
721{
722 const float MeterPerFeet = 0.3048;
723 float amsl_baro_ft = air_data.amsl_baro / MeterPerFeet;
724 float amsl_gps_ft = stateGetPositionLla_f()->alt / MeterPerFeet;
725 pprz_msg_send_AMSL(trans, dev, AC_ID, &amsl_baro_ft, &amsl_gps_ft);
726}
727#endif
728
733{
734 /* the sensors of the airframe calibration, the settings are statically initialized */
735 struct air_data_sensor_t *sensors[] = {air_data.baros, air_data.diffs, air_data.airspeeds, air_data.temps};
736 for (uint8_t k = 0; k < 4; k++) {
737 for (uint8_t i = 0; i < AIR_DATA_MAX_SENSORS; i++) {
738 if (sensors[k][i].abi_id != ABI_DISABLE) {
740 }
741 }
742 }
743 /* the differential pressures in the airframe use its offset (0: the driver offset), the others are not zeroed */
744 for (uint8_t i = 0; i < AIR_DATA_MAX_SENSORS; i++) {
745 if (air_data.diffs[i].abi_id != ABI_DISABLE) {
746 air_data.diffs[i].calibrated.offset = true;
747 }
748 }
749 air_data.baro_used = ABI_DISABLE;
750 air_data.airspeed_used = ABI_DISABLE;
751 air_data.temp_used = ABI_DISABLE;
752 air_data.calibrate_baro = false;
753 air_data.zero_airspeed = false;
754 air_data.qfe = -1.f;
755 air_data.tas_factor = 1.f;
756 air_data.calc_qnh_once = true;
757 air_data.amsl_baro_valid = false;
758
759 /* initialize the output variables
760 * pressure, qnh, temperature and airspeed to invalid values,
761 * rest to zero
762 */
763 air_data.pressure = -1.0f;
764 air_data.qnh = -1.0f;
765 air_data.airspeed = -1.0f;
766 air_data.tas = -1.0f;
767 air_data.temperature = -1000.0f;
768 air_data.differential = 0.0f;
769 air_data.amsl_baro = 0.0f;
770 air_data.aoa = 0.0f;
771 air_data.sideslip = 0.0f;
772 air_data.wind_speed = 0.0f;
773 air_data.wind_dir = 0.0f;
774
775 /* internal variables */
777
778 AbiBindMsgBARO_ABS_RAW(ABI_BROADCAST, &baro_raw_ev, baro_raw_cb);
779 AbiBindMsgBARO_DIFF_RAW(ABI_BROADCAST, &diff_raw_ev, diff_raw_cb);
780 AbiBindMsgAIRSPEED_RAW(ABI_BROADCAST, &airspeed_raw_ev, airspeed_raw_cb);
781 AbiBindMsgTEMPERATURE_RAW(ABI_BROADCAST, &temp_raw_ev, temp_raw_cb);
782 AbiBindMsgINCIDENCE(ABI_BROADCAST, &incidence_ev, incidence_cb);
783
784#if PERIODIC_TELEMETRY
789#endif
790
791#if PREFLIGHT_CHECKS
792 preflight_check_register(&air_data_pfc, air_data_preflight);
793#endif
794}
795
797{
798 // If it has be calculated and baro is OK
799 if (air_data.amsl_baro_valid) {
800 return air_data.amsl_baro;
801 }
802 // Otherwise use real altitude (from GPS)
803 return stateGetPositionLla_f()->alt;
804}
805
807{
808 // Watchdog on baro
809 if (baro_health_counter > 0) {
811 } else {
812 air_data.amsl_baro_valid = false;
813 }
814
815 // Sensors in use, also when they stop sending
816 float now = get_sys_time_float();
818 air_data.baro_used = (s != NULL) ? s->abi_id : ABI_DISABLE;
820 air_data.airspeed_used = (s != NULL) ? s->abi_id : ABI_DISABLE;
821 s = air_data_select_temp(now);
822 air_data.temp_used = (s != NULL) ? s->abi_id : ABI_DISABLE;
823
824 // Calibration
825#if AIR_DATA_CALIB_AT_BOOT
826 static bool boot_calib_done = false;
827 if (!boot_calib_done && now > AIR_DATA_CALIB_BOOT_DELAY) {
828 boot_calib_done = true;
829 if (!calib_running) {
830 air_data_start_calibration(true, true, false);
831 }
832 }
833#endif
836 }
837}
838
839void air_data_parse_WIND_INFO(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf)
840{
841 if (DL_WIND_INFO_ac_id(buf) != AC_ID) { return; }
842 uint8_t flags = DL_WIND_INFO_flags(buf);
843 struct FloatVect2 wind = { 0.f, 0.f };
844 float upwind = 0.f;
845 if (bit_is_set(flags, 0)) {
846 wind.x = DL_WIND_INFO_north(buf);
847 wind.y = DL_WIND_INFO_east(buf);
848 stateSetHorizontalWindspeed_f(MODULE_AIR_DATA_ID, &wind);
849 air_data.wind_speed = float_vect2_norm(&wind);
850 air_data.wind_dir = atan2f(wind.y, wind.x);
851 }
852 if (bit_is_set(flags, 1)) {
853 upwind = DL_WIND_INFO_up(buf);
854 stateSetVerticalWindspeed_f(MODULE_AIR_DATA_ID, upwind);
855 }
856#if !USE_AIRSPEED
857 if (bit_is_set(flags, 2)) {
858 air_data.tas = DL_WIND_INFO_airspeed(buf);
859 air_data.airspeed = eas_from_tas(air_data.tas);
860 stateSetAirspeed_f(MODULE_AIR_DATA_ID, air_data.airspeed);
861 }
862#endif
863#ifdef WIND_INFO_RET
864 float airspeed = stateGetAirspeed_f();
865 pprz_msg_send_WIND_INFO_RET(trans, dev, AC_ID, &flags, &wind.y, &wind.x, &upwind, &airspeed);
866#endif
867}
868
883{
884 /* q (dynamic pressure) = total pressure - static pressure
885 * q = 1/2*rho*speed^2
886 * speed = sqrt(2*q/rho)
887 * With rho = air density at sea level.
888 * Lower bound of q at zero, no flying backwards guys...
889 */
890 const float two_div_rho_0 = 2.0 / PPRZ_ISA_AIR_DENSITY;
891 float sign = 1.0f;
892 if (q < 0) {
893 sign = -1.0f;
894 q = -q;
895 }
896 return sqrtf(q * two_div_rho_0) * sign;
897}
898
915float get_tas_factor(float p, float t)
916{
917 /* factor to convert EAS to TAS:
918 * sqrt(rho0 / rho) = sqrt((p0 * T) / (p * T0))
919 * convert input temp to Kelvin
920 */
921 return sqrtf((PPRZ_ISA_SEA_LEVEL_PRESSURE * KelvinOfCelsius(t)) /
923}
924
928static void compute_tas_factor(void)
929{
930 if (air_data.pressure > 0.f && air_data.temperature > -900.f) {
931 // compute air density from pressure and temperature
932 air_data.tas_factor = get_tas_factor(air_data.pressure, air_data.temperature);
933 }
934 else {
935 // compute air density from altitude in ISA condition
936 const float z = air_data_get_amsl();
937 const float p = pprz_isa_pressure_of_altitude(z);
938 const float t = pprz_isa_temperature_of_altitude(z);
939 air_data.tas_factor = get_tas_factor(p, CelsiusOfKelvin(t));
940 }
941}
942
952float tas_from_eas(float eas)
953{
955 return air_data.tas_factor * eas;
956}
957
967float eas_from_tas(float tas)
968{
970 return tas / air_data.tas_factor;
971}
972
982{
984}
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
Definition abi_common.h:90
#define ABI_DISABLE
Reserved ABI ID to disable callback.
Definition abi_common.h:86
#define TEMPERATURE_TYPE_AIR
Types of the TEMPERATURE_RAW message, what the temperature measures.
Definition abi_common.h:89
struct abi_struct abi_event
Definition abi_common.h:101
#define TEMPERATURE_TYPE_BARO
barometer die
Definition abi_common.h:91
#define ABI_BROADCAST
Broadcast address.
Definition abi_common.h:80
#define TEMPERATURE_TYPE_OTHER
anything else
Definition abi_common.h:93
#define TEMPERATURE_TYPE_IMU
IMU die.
Definition abi_common.h:92
#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.
Definition air_data.c:882
void air_data_ZeroAirspeed(bool zero)
Zero the differential pressures (setting handler).
Definition air_data.c:588
void air_data_periodic(void)
Check health.
Definition air_data.c:806
#define AIR_DATA_AIRSPEED_CALIB
Definition air_data.c:63
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.
Definition air_data.c:300
#define AIR_DATA_TIMEOUT_FACTOR
Definition air_data.c:100
#define AIR_DATA_IS_ESTIMATE(_id)
Airspeed estimates (not sensors): only used when no sensor is healthy, and not sent as AIRSPEED.
Definition air_data.c:143
static void diff_raw_cb(uint8_t sender_id, uint32_t stamp, float pressure)
Definition air_data.c:409
static bool air_data_sensor_healthy(struct air_data_sensor_t *s, float now)
Is the sensor sending valid values.
Definition air_data.c:254
static abi_event temp_raw_ev
Definition air_data.c:166
static void send_airspeed_raw(struct transport_tx *trans, struct link_device *dev)
Definition air_data.c:682
static uint8_t baro_health_counter
counter to check baro health
Definition air_data.c:169
static void compute_tas_factor(void)
Internal utility function to compute the current tas factor.
Definition air_data.c:928
#define AIR_DATA_BARO_CALIB
Calibration of the sensors in the airframe, in a section with prefix AIR_DATA_, for example <define n...
Definition air_data.c:57
static float air_data_sensor_temp(uint8_t abi_id)
Temperature of a sensor (sent with the same ABI id), NAN if unknown.
Definition air_data.c:648
#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...
Definition air_data.c:134
#define AIR_DATA_AIRSPEED_ID
Definition air_data.c:74
struct AirData air_data
global AirData state Statically initialized, the persistent settings are loaded before air_data_init(...
Definition air_data.c:153
float air_data_get_amsl(void)
Return AMSL (altitude AboveSeaLevel).
Definition air_data.c:796
static abi_event airspeed_raw_ev
Definition air_data.c:166
void air_data_SetAirspeedFilter(float freq)
Definition air_data.c:595
#define AIR_DATA_AIRSPEED_SEND_ID
Definition air_data.c:85
static struct air_data_sensor_t * air_data_select_baro(float now)
Definition air_data.c:310
#define AIR_DATA_TEMP_CALIB
Definition air_data.c:66
void air_data_CalibrateBaro(bool calibrate)
Line up the barometers with the one in use and reset the QFE (setting handler).
Definition air_data.c:581
static void send_air_data(struct transport_tx *trans, struct link_device *dev)
Definition air_data.c:711
static void air_data_sensor_defaults(struct air_data_sensor_t *s)
Defaults of the values not set in the airframe calibration.
Definition air_data.c:183
float eas_from_tas(float tas)
Calculate equivalent airspeed from true airspeed.
Definition air_data.c:967
static void airspeed_raw_cb(uint8_t sender_id, uint32_t stamp, float eas)
Definition air_data.c:435
#define AIR_DATA_TEMP_ID
Definition air_data.c:77
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).
Definition air_data.c:288
#define AIR_DATA_DIFF_CALIB
Definition air_data.c:60
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...
Definition air_data.c:263
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.
Definition air_data.c:201
#define AIR_DATA_CALIB_TIME
Calibration time (s).
Definition air_data.c:115
static struct air_data_sensor_t * air_data_select_temp(float now)
Definition air_data.c:322
static float calib_start
Start time of the calibration (s).
Definition air_data.c:176
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.
Definition air_data.c:294
static bool calib_baro
Line up the barometers.
Definition air_data.c:174
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...
Definition air_data.c:535
float tas_from_dynamic_pressure(float q)
Calculate true airspeed from dynamic pressure.
Definition air_data.c:981
static void incidence_cb(uint8_t sender_id, uint8_t flag, float aoa, float sideslip)
Definition air_data.c:486
#define AIR_DATA_CALIB_BOOT_DELAY
Definition air_data.c:127
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.
Definition air_data.c:655
#define AIR_DATA_DIFF_MAX
Maximum differential pressure (Pa), about 130 m/s.
Definition air_data.c:110
static abi_event diff_raw_ev
Definition air_data.c:166
#define AIR_DATA_CALIB_ARMED
Allow the calibration while the motors are armed.
Definition air_data.c:139
static abi_event incidence_ev
Definition air_data.c:166
static bool calib_running
Calibration state.
Definition air_data.c:172
static void send_amsl(struct transport_tx *trans, struct link_device *dev)
Definition air_data.c:720
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:839
static void temp_raw_cb(uint8_t sender_id, uint32_t stamp, float temp, uint8_t type)
Definition air_data.c:461
static void air_data_set_pressure(uint32_t stamp, float pressure)
Definition air_data.c:333
static void air_data_start_calibration(bool boot, bool baro, bool diff)
Definition air_data.c:504
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
Definition air_data.c:191
static struct air_data_sensor_t * air_data_select_airspeed(float now)
Definition air_data.c:316
#define AIR_DATA_ALIGN_NB
Number of values to line up a barometer that starts after the calibration.
Definition air_data.c:105
#define AIR_DATA_TIMEOUT
A sensor is unhealthy without a valid value for AIR_DATA_TIMEOUT_FACTOR times its interval,...
Definition air_data.c:97
static void send_baro_raw(struct transport_tx *trans, struct link_device *dev)
Definition air_data.c:667
void air_data_init(void)
AirData initialization.
Definition air_data.c:732
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.
Definition air_data.c:213
static bool calib_diff
Zero the differential pressures (otherwise only check them at rest).
Definition air_data.c:175
static void baro_raw_cb(uint8_t sender_id, uint32_t stamp, float pressure)
Definition air_data.c:378
float get_tas_factor(float p, float t)
Calculate true airspeed (TAS) factor.
Definition air_data.c:915
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.
Definition air_data.c:237
float tas_from_eas(float eas)
Calculate true airspeed from equivalent airspeed.
Definition air_data.c:952
static bool calib_boot
Calibration at boot: keep the barometer offsets from the airframe.
Definition air_data.c:173
#define AIR_DATA_BARO_SEND_ID
Sensors in the BARO_RAW and AIRSPEED_RAW telemetry (ABI id), ABI_BROADCAST for all in turn,...
Definition air_data.c:82
#define AIR_DATA_BARO_ID
Sensors to use (ABI id), ABI_BROADCAST for the first healthy one.
Definition air_data.c:71
static void air_data_set_airspeed(float eas, bool send)
Definition air_data.c:362
static abi_event baro_raw_ev
Definition air_data.c:166
Air Data interface.
float dt
Filtered interval between the values (s), for the health check.
Definition air_data.h:64
#define AIR_DATA_MAX_SENSORS
Maximum number of sensors of each kind (barometers, differential pressure, airspeed,...
Definition air_data.h:41
uint8_t type
Temperature type (TEMPERATURE_TYPE_*), only for the temperatures.
Definition air_data.h:56
bool offset
Offset calibrated (airframe or calibration).
Definition air_data.h:46
float value
Last calibrated value (Pa, m/s or deg Celcius).
Definition air_data.h:61
struct air_data_calib_t calibrated
Calibration bitmask.
Definition air_data.h:57
uint8_t abi_id
ABI sender id of the driver, ABI_DISABLE when unused.
Definition air_data.h:55
uint32_t stamp
Driver timestamp of the last value (us).
Definition air_data.h:62
float time
System time of the last valid value (s), 0 if never.
Definition air_data.h:63
float raw
Last raw value (Pa, m/s or deg Celcius).
Definition air_data.h:60
Air Data structure.
Definition air_data.h:73
One air data sensor: barometer, differential pressure, airspeed or temperature.
Definition air_data.h:54
bool autopilot_get_motors_on(void)
get motors status
Definition autopilot.c:303
Core autopilot interface common to all firmwares.
#define sensors(...)
static struct uart_periph * dev
#define Min(x, y)
Definition esc_dshot.c:110
static float float_vect2_norm(const struct FloatVect2 *v)
#define KelvinOfCelsius(_t)
Convert temperature from Celsius to Kelvin.
Definition pprz_isa.h:73
#define PPRZ_ISA_SEA_LEVEL_TEMP
ISA sea level standard temperature in Kelvin.
Definition pprz_isa.h:52
static float pprz_isa_pressure_of_altitude(float altitude)
Get pressure in Pa from absolute altitude (using simplified equation).
Definition pprz_isa.h:117
#define PPRZ_ISA_AIR_DENSITY
standard air density in kg/m^3
Definition pprz_isa.h:64
static float pprz_isa_temperature_of_altitude(float alt)
Get ISA temperature from a MSL altitude.
Definition pprz_isa.h:181
#define CelsiusOfKelvin(_t)
Convert temperature from Kelvin to Celsius.
Definition pprz_isa.h:71
#define PPRZ_ISA_SEA_LEVEL_PRESSURE
ISA sea level standard atmospheric pressure in Pascal.
Definition pprz_isa.h:50
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
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
struct State state
Definition state.c:36
static struct LlaCoor_f * stateGetPositionLla_f(void)
Get position in LLA coordinates (float).
Definition state.h:868
static bool stateIsGlobalCoordinateValid(void)
Test if global coordinates are valid.
Definition state.h:631
float stateGetHmslOrigin_f(void)
Get the HMSL of the frame origin (float).
Definition state.c:210
struct LlaCoor_f stateGetLlaOrigin_f(void)
Get the LLA position of the frame origin (float).
Definition state.c:149
static void stateSetAngleOfAttack_f(uint16_t id, float aoa)
Set angle of attack in radians (float).
Definition state.h:1522
static void stateSetSideslip_f(uint16_t id, float sideslip)
Set sideslip angle in radians (float).
Definition state.h:1533
static void stateSetVerticalWindspeed_f(uint16_t id, float v_windspeed)
Set vertical windspeed (float).
Definition state.h:1500
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition state.h:1615
static void stateSetHorizontalWindspeed_f(uint16_t id, struct FloatVect2 *h_windspeed)
Set horizontal windspeed (float).
Definition state.h:1489
static void stateSetAirspeed_f(uint16_t id, float airspeed)
Set airspeed (float).
Definition state.h:1511
static float p[2][2]
static uint32_t s
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
Definition nav_fish.c:232
bool init
Definition nav_gls.c:57
static uint32_t idx
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.
Definition sys_time.h:241
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
uint16_t ref[TCOUPLE_NB]
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.