Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
ins_ekf2.cpp
Go to the documentation of this file.
1/*
2 * Copyright (C) 2022-2026 Freek van Tienen <freek.v.tienen@gmail.com>
3 *
4 * This file is part of paparazzi.
5 *
6 * paparazzi is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2, or (at your option)
9 * any later version.
10 *
11 * paparazzi is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with paparazzi; see the file COPYING. If not, write to
18 * the Free Software Foundation, 59 Temple Place - Suite 330,
19 * Boston, MA 02111-1307, USA.
20 */
21
31
34#include "modules/core/abi.h"
35#include "modules/imu/imu.h"
36#include "stabilization/stabilization_attitude.h"
37#include "generated/airframe.h"
38#include "generated/flight_plan.h"
39#include "EKF/ekf.h"
40#include "lib/gnss/gnss_checks.hpp"
41#include "lib/world_magnetic_model/geo_mag_declination.h"
42#include "math/pprz_isa.h"
44#include "mcu_periph/sys_time.h"
45#include "autopilot.h"
46
47using namespace estimator;
48
50#if defined SITL && USE_NPS
51#include "nps_autopilot.h"
52#include <stdio.h>
53#endif
54
55/* Options of the previous EKF2 version, replaced by the PX4 parameters (see conf/modules/ins_ekf2.xml) */
56#if defined(INS_EKF2_FUSION_MODE) || defined(INS_EKF2_VDIST_SENSOR_TYPE) || defined(INS_EKF2_MAG_FUSION_TYPE)
57#error "INS_EKF2_FUSION_MODE, VDIST_SENSOR_TYPE and MAG_FUSION_TYPE are replaced by INS_EKF2_GPS_CTRL, EV_CTRL, OF_CTRL, DRAG_CTRL, HGT_REF and MAG_TYPE"
58#endif
59#if defined(INS_EKF2_GPS_CHECK_MASK) || defined(INS_EKF2_GPS_REQ_EPH) || defined(INS_EKF2_GPS_REQ_EPV) || defined(INS_EKF2_GPS_REQ_SACC) || defined(INS_EKF2_GPS_REQ_NSATS) || defined(INS_EKF2_GPS_REQ_PDOP)
60#error "INS_EKF2_GPS_CHECK_MASK and GPS_REQ_* are replaced by INS_EKF2_GNSS_CHECK and GNSS_REQ_*"
61#endif
62#if defined(INS_EKF2_FLOW_SENSOR_DELAY) || defined(INS_EKF2_MIN_FLOW_QUALITY) || defined(INS_EKF2_MAX_FLOW_RATE) || defined(INS_EKF2_FLOW_NOISE) || defined(INS_EKF2_FLOW_NOISE_QMIN) || defined(INS_EKF2_FLOW_INNOV_GATE) || defined(INS_EKF2_FLOW_POS_X) || defined(INS_EKF2_FLOW_POS_Y) || defined(INS_EKF2_FLOW_POS_Z)
63#error "INS_EKF2_FLOW_* are replaced by INS_EKF2_OF_DELAY, OF_QMIN, OF_MAX_RATE, OF_N_MIN, OF_N_MAX, OF_GATE and OF_POS_*"
64#endif
65#if defined(INS_EKF2_SONAR_MIN_RANGE) || defined(INS_EKF2_SONAR_MAX_RANGE) || defined(INS_EKF2_RANGE_MAIN_AGL)
66#error "INS_EKF2_SONAR_* and RANGE_MAIN_AGL are replaced by INS_EKF2_RNG_MIN_DIST, RNG_MAX_DIST and RNG_CTRL"
67#endif
68#if defined(INS_EKF2_RELHEADING_DISTANCE) || defined(INS_EKF2_RELHEADING_ERR) || defined(INS_EKF2_GPS_COURSE_YAW)
69#error "INS_EKF2_RELHEADING_DISTANCE/_ERR and GPS_COURSE_YAW are replaced by GPS_UBX_HEADING_BASELINE/_TOL (gps_ublox) and the EKF-GSF yaw estimator (yaw from the GNSS velocity)"
70#endif
71#if defined(INS_EKF2_GPS_ANTENNA_DISTANCE) || defined(INS_EKF2_GPS_ANTENNA_DISTANCE_ERR)
72#error "INS_EKF2_GPS_ANTENNA_DISTANCE/_ERR are replaced by GPS_UBX_HEADING_BASELINE/_TOL in the gps_ublox module, which drops the heading itself"
73#endif
74
76#ifndef USE_INS_NAV_INIT
77#define USE_INS_NAV_INIT TRUE
78#endif
79
80/*
81 * EKF2 parameters
82 *
83 * All PX4 EKF2 parameters have the PX4 default and can be changed in the airframe as
84 * INS_EKF2_<name> for EKF2_<name>, e.g. INS_EKF2_GPS_CTRL, INS_EKF2_HGT_REF, INS_EKF2_MAG_TYPE
85 * (see generated/ekf2_params.h in sw/ext/px4_ekf2 and the PX4 parameter reference).
86 * The GNSS checks are INS_EKF2_GNSS_<name> for GNSS_<name>.
87 */
88
90#define GNSS_CHECK_NSATS (1<<0)
91#define GNSS_CHECK_PDOP (1<<1)
92#define GNSS_CHECK_EPH (1<<2)
93#define GNSS_CHECK_EPV (1<<3)
94#define GNSS_CHECK_SACC (1<<4)
95#define GNSS_CHECK_HDRIFT (1<<5)
96#define GNSS_CHECK_VDRIFT (1<<6)
97#define GNSS_CHECK_HSPEED (1<<7)
98#define GNSS_CHECK_VSPEED (1<<8)
99#define GNSS_CHECK_SPOOFED (1<<9)
100#define GNSS_CHECK_FIX (1<<10)
101#define GNSS_CHECK_JAMMED (1<<11)
102
104#ifndef INS_EKF2_GNSS_CHECK
105#define INS_EKF2_GNSS_CHECK (GNSS_CHECK_NSATS | GNSS_CHECK_EPH | GNSS_CHECK_SACC | GNSS_CHECK_FIX)
106#endif
108
109
110#define EKF2_GPS_CTRL_HPOS 1
111#define EKF2_GPS_CTRL_VPOS 2
112#define EKF2_GPS_CTRL_VEL 4
113#define EKF2_GPS_CTRL_YAW 8
114
116#define EKF2_EV_CTRL_HPOS 1
117#define EKF2_EV_CTRL_VPOS 2
118#define EKF2_EV_CTRL_VEL 4
119#define EKF2_EV_CTRL_YAW 8
120
122#define EKF2_HGT_REF_BARO 0
123#define EKF2_HGT_REF_GNSS 1
124#define EKF2_HGT_REF_RANGE 2
125#define EKF2_HGT_REF_EV 3
126
128#define EKF2_MAG_TYPE_AUTO 0
129#define EKF2_MAG_TYPE_HEADING 1
130#define EKF2_MAG_TYPE_NONE 5
131#define EKF2_MAG_TYPE_INIT 6
132
136#if INS_EKF2_OPTITRACK
137#ifndef INS_EKF2_GPS_CTRL
138#define INS_EKF2_GPS_CTRL 0
139#endif
140#ifndef INS_EKF2_EV_CTRL
141#define INS_EKF2_EV_CTRL (EKF2_EV_CTRL_HPOS | EKF2_EV_CTRL_VPOS | EKF2_EV_CTRL_VEL | EKF2_EV_CTRL_YAW)
142#endif
143#ifndef INS_EKF2_HGT_REF
144#define INS_EKF2_HGT_REF EKF2_HGT_REF_EV
145#endif
146#ifndef INS_EKF2_MAG_TYPE
147#define INS_EKF2_MAG_TYPE EKF2_MAG_TYPE_NONE
148#endif
149#ifndef INS_EKF2_BARO_CTRL
150#define INS_EKF2_BARO_CTRL 0
151#endif
152#ifndef INS_EKF2_EVP_NOISE
153#define INS_EKF2_EVP_NOISE 0.02f
154#endif
155#ifndef INS_EKF2_EVA_NOISE
156#define INS_EKF2_EVA_NOISE 0.05f
157#endif
158#ifndef INS_EKF2_EV_DELAY
159#define INS_EKF2_EV_DELAY 175.f
160#endif
161#endif
162
165#ifndef INS_EKF2_GNSS_REQ_EPH
166#define INS_EKF2_GNSS_REQ_EPH 5.0f
167#endif
168#ifndef INS_EKF2_GNSS_REQ_SACC
169#define INS_EKF2_GNSS_REQ_SACC 1.0f
170#endif
171
174#ifndef INS_EKF2_HGT_REF
175#define INS_EKF2_HGT_REF EKF2_HGT_REF_BARO
176#endif
177
187#ifndef INS_EKF2_AT_REST_DETECTION
188#define INS_EKF2_AT_REST_DETECTION TRUE
189#endif
190#ifndef INS_EKF2_AT_REST_GYRO
191#define INS_EKF2_AT_REST_GYRO 0.3f
192#endif
193#ifndef INS_EKF2_AT_REST_ACCEL
194#define INS_EKF2_AT_REST_ACCEL 0.1f
195#endif
196#ifndef INS_EKF2_AT_REST_RATE
197#define INS_EKF2_AT_REST_RATE 1.f
198#endif
199#ifndef INS_EKF2_AT_REST_TIME
200#define INS_EKF2_AT_REST_TIME 1.f
201#endif
202
203/* Vehicle specific defaults, as PX4 (rc.fw_defaults and rc.mc_defaults) */
204#if defined(FIXEDWING_FIRMWARE)
205#ifndef INS_EKF2_ARSP_THR
206#define INS_EKF2_ARSP_THR 8.f
207#endif
208#ifndef INS_EKF2_FUSE_BETA
209#define INS_EKF2_FUSE_BETA 1
210#endif
211#ifndef INS_EKF2_MAG_ACCLIM
212#define INS_EKF2_MAG_ACCLIM 0.f
213#endif
214#ifndef INS_EKF2_RNG_QLTY_T
215#define INS_EKF2_RNG_QLTY_T 3.f
216#endif
217#else
218#ifndef INS_EKF2_RNG_FOG
219#define INS_EKF2_RNG_FOG 1.f
220#endif
221#endif
222
223/* All parameters with the PX4 defaults, the values above can be used in the airframe */
224#if INS_EKF2_LOG_INPUTS
225#define INS_EKF2_PARAM_TABLE // parameters by name, to log them
226#endif
227#include "generated/ekf2_params.h"
228
229PRINT_CONFIG_VAR(INS_EKF2_GPS_CTRL)
230PRINT_CONFIG_VAR(INS_EKF2_EV_CTRL)
232PRINT_CONFIG_VAR(INS_EKF2_MAG_TYPE)
233
234
235#ifndef INS_EKF2_GPS_DELAY
236#define INS_EKF2_GPS_DELAY 110
237#endif
239
240
241#ifndef INS_EKF2_RNG_MIN_DIST
242#define INS_EKF2_RNG_MIN_DIST 0.001f
243#endif
245
246
247#ifndef INS_EKF2_RNG_MAX_DIST
248#define INS_EKF2_RNG_MAX_DIST 4.f
249#endif
251
252
253#ifndef INS_EKF2_OF_MAX_RATE
254#define INS_EKF2_OF_MAX_RATE 200.f
255#endif
257
258
261#ifndef INS_EKF2_AIRSPEED_ID
262#define INS_EKF2_AIRSPEED_ID AIR_DATA_SENDER_ID
263#endif
265
266
267#ifndef INS_EKF2_BARO_ID
268#define INS_EKF2_BARO_ID AIR_DATA_SENDER_ID
269#endif
271
272
273#ifndef INS_EKF2_TEMPERATURE_ID
274#define INS_EKF2_TEMPERATURE_ID AIR_DATA_SENDER_ID
275#endif
277
278
279#ifndef INS_EKF2_AGL_ID
280#define INS_EKF2_AGL_ID ABI_BROADCAST
281#endif
283
284/* default Gyro to use in INS */
285#ifndef INS_EKF2_GYRO_ID
286#define INS_EKF2_GYRO_ID ABI_BROADCAST
287#endif
289
290/* default Accelerometer to use in INS */
291#ifndef INS_EKF2_ACCEL_ID
292#define INS_EKF2_ACCEL_ID ABI_BROADCAST
293#endif
295
296/* default Magnetometer to use in INS */
297#ifndef INS_EKF2_MAG_ID
298#define INS_EKF2_MAG_ID ABI_BROADCAST
299#endif
301
302/* default GPS to use in INS */
303#ifndef INS_EKF2_GPS_ID
304#define INS_EKF2_GPS_ID GPS_MULTI_ID
305#endif
307
308/* default RELPOS to use for heading in INS */
309#ifndef INS_EKF2_RELPOS_ID
310#define INS_EKF2_RELPOS_ID ABI_BROADCAST
311#endif
313
314/* default Optical Flow to use in INS */
315#ifndef INS_EKF2_OF_ID
316#define INS_EKF2_OF_ID ABI_BROADCAST
317#endif
319
320/* GPS antenna position from the CoG in meters, as the PX4 SENS_GNSS0_OFF* parameters */
321#ifndef INS_EKF2_GPS_POS_X
322#define INS_EKF2_GPS_POS_X 0
323#endif
325
326#ifndef INS_EKF2_GPS_POS_Y
327#define INS_EKF2_GPS_POS_Y 0
328#endif
330
331#ifndef INS_EKF2_GPS_POS_Z
332#define INS_EKF2_GPS_POS_Z 0
333#endif
335
336/* GNSS heading measurement noise when the receiver gives no accuracy (rad) */
337#ifndef INS_EKF2_GPS_YAW_NOISE
338#define INS_EKF2_GPS_YAW_NOISE 0.1f
339#endif
341
342#ifdef INS_EXT_VISION_ROTATION
343struct FloatQuat ins_ext_vision_rot;
344#endif
345
346/* All registered ABI events */
358
359/* All ABI callbacks */
360static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure);
361static void temperature_cb(uint8_t sender_id, float temp);
362static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance);
363static void gyro_int_cb(uint8_t sender_id, uint32_t stamp, struct FloatRates *delta_gyro, uint16_t dt);
364static void accel_int_cb(uint8_t sender_id, uint32_t stamp, struct FloatVect3 *delta_accel, uint16_t dt);
365static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag);
366static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
367static void relpos_cb(uint8_t sender_id, uint32_t stamp, struct RelPosNED *relpos);
368static void optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x, int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence);
369static void airspeed_cb(uint8_t sender_id, float eas);
370static void reset_cb(uint8_t sender_id, uint8_t flag);
371
372/* Static local functions */
373static void ins_ekf2_publish_attitude(uint64_t time_us);
374
375/* Static local variables */
376static Ekf ekf;
378static GnssChecks gnss_checks;
379struct ekf2_t ekf2;
380static struct extVisionSample sample_ev;
381
389{
390 static uint32_t last_now = 0;
391 static uint64_t wraps = 0;
393 if (now < last_now && (last_now - now) > (1UL << 31)) {
394 wraps += (1ULL << 32);
395 }
396 last_now = now;
397
398 // A stamp more than half the range later than now was taken before the last wrap
399 uint64_t time_us = wraps + stamp;
400 if (stamp > now && (stamp - now) > (1UL << 31) && time_us >= (1ULL << 32)) {
401 time_us -= (1ULL << 32);
402 }
403 return time_us;
404}
405
408{
409 uint64_t time_us = ekf2_time_us(stamp);
410 return (time_us > delay_ms * 1000ULL) ? time_us - delay_ms * 1000ULL : 0;
411}
412
413/*
414 * Logging of the EKF inputs (INS_EKF2_LOG_INPUTS), to replay the EKF offline with
415 * sw/tools/ekf2_replay. Every sample given to the EKF, every update and the parameters
416 * are logged as EKF2_INPUT messages, in the order the EKF gets them. The EKF only starts
417 * once the log is open (or after INS_EKF2_LOG_WAIT without log), so the replay sees
418 * everything the EKF saw.
419 */
425
426#if INS_EKF2_LOG_INPUTS
427#include "pprzlink/messages.h"
428
430#ifndef INS_EKF2_LOG_WAIT
431#define INS_EKF2_LOG_WAIT 60
432#endif
433
435#ifndef INS_EKF2_LOG_OUTPUT_DIV
436#define INS_EKF2_LOG_OUTPUT_DIV 10
437#endif
438
439/* SD log (also on Linux and in NPS): the flight recorder log by default (as IMU_LOG_HIGHSPEED) */
443#ifndef INS_EKF2_LOG_DEVICE
444#define INS_EKF2_LOG_DEVICE flightrecorder_sdlog
445#if !FLIGHTRECORDER_SDLOG
446#error "INS_EKF2_LOG_INPUTS needs the flight_recorder module (also in the nps target) or INS_EKF2_LOG_DEVICE"
447#endif
448#endif
449#define EKF2_LOG_TP (&pprzlog_tp.trans_tx)
450#define EKF2_LOG_DEV (&(INS_EKF2_LOG_DEVICE).device)
451static bool ekf2_log_ready(void) { return (INS_EKF2_LOG_DEVICE).file != NULL && *(INS_EKF2_LOG_DEVICE).file != -1; }
452
454static enum { EKF2_LOG_WAITING, EKF2_LOG_ON, EKF2_LOG_OFF } ekf2_log_state = EKF2_LOG_WAITING;
455static struct { double lat, lon; float alt; bool set; } ekf2_log_origin;
456
457static void ekf2_log(uint8_t type, uint64_t time_us, uint8_t nb_ints, int32_t *ints, uint8_t nb_values, float *values)
458{
459 if (ekf2_log_state != EKF2_LOG_ON) {
460 return;
461 }
462 uint32_t t = (uint32_t)time_us;
463 int32_t no_int = 0;
464 float no_value = 0;
465 pprz_msg_send_EKF2_INPUT(EKF2_LOG_TP, EKF2_LOG_DEV, AC_ID, &t, &type,
466 nb_ints, nb_ints ? ints : &no_int, nb_values, nb_values ? values : &no_value);
467}
468
470static void ekf2_log_params(uint64_t time_us)
471{
472 for (int i = 0; i < ins_ekf2_params_nb; i += 32) {
473 float values[32];
474 uint8_t n = 0;
475 for (int j = i; j < ins_ekf2_params_nb && n < 32; j++) {
476 values[n++] = ins_ekf2_params[j].get(*ekf_params);
477 }
478 int32_t first = i;
479 ekf2_log(EKF2_IN_PARAMS, time_us, 1, &first, n, values);
480 }
481}
482
484static void ekf2_log_param_changed(uint64_t time_us, const char *name)
485{
486 for (int i = 0; i < ins_ekf2_params_nb; i++) {
487 if (!strcmp(ins_ekf2_params[i].name, name)) {
488 int32_t index = i;
489 float value = ins_ekf2_params[i].get(*ekf_params);
490 ekf2_log(EKF2_IN_PARAMS, time_us, 1, &index, 1, &value);
491 return;
492 }
493 }
494}
495
496static void ekf2_log_origin_set(uint64_t time_us, double lat, double lon, float alt)
497{
498 if (ekf2_log_state == EKF2_LOG_WAITING) {
499 ekf2_log_origin.lat = lat;
500 ekf2_log_origin.lon = lon;
501 ekf2_log_origin.alt = alt;
502 ekf2_log_origin.set = true;
503 return;
504 }
505 int32_t ints[2] = {(int32_t)lround(lat * 1e7), (int32_t)lround(lon * 1e7)};
506 ekf2_log(EKF2_IN_ORIGIN, time_us, 2, ints, 1, &alt);
507}
508
510static void ekf2_log_check_start(void)
511{
512 if (ekf2_log_state != EKF2_LOG_WAITING) {
513 return;
514 }
515 if (ekf2_log_ready()) {
516 ekf2_log_state = EKF2_LOG_ON;
518 ekf2_log_params(now);
519 float config[6] = {INS_EKF2_TAU_POS, INS_EKF2_TAU_VEL, INS_EKF2_GPS_YAW_RST,
521 ekf2_log(EKF2_IN_CONFIG, now, 0, NULL, 6, config);
522 if (ekf2_log_origin.set) {
523 ekf2_log_origin_set(now, ekf2_log_origin.lat, ekf2_log_origin.lon, ekf2_log_origin.alt);
524 }
525 } else if (get_sys_time_float() > INS_EKF2_LOG_WAIT) {
526 ekf2_log_state = EKF2_LOG_OFF;
527 }
528}
529
531static inline bool ekf2_input_enabled(void) { return ekf2_log_state != EKF2_LOG_WAITING; }
532
533static void ekf2_log_output(uint64_t time_us)
534{
535#if INS_EKF2_LOG_OUTPUT_DIV
536 static uint16_t count = 0;
537 if (++count < INS_EKF2_LOG_OUTPUT_DIV) {
538 return;
539 }
540 count = 0;
541 const Vector3f pos{ekf.getPosition()};
542 const Vector3f vel{ekf.getVelocity()};
543 const Quatf q{ekf.getQuaternion()};
544 float values[10] = {pos(0), pos(1), pos(2), vel(0), vel(1), vel(2), q(0), q(1), q(2), q(3)};
545 ekf2_log(EKF2_IN_OUTPUT, time_us, 0, NULL, 10, values);
546#else
547 (void)time_us;
548#endif
549}
550
551#else /* INS_EKF2_LOG_INPUTS */
552#define ekf2_log(...) {}
553#define ekf2_log_param_changed(...) {}
554#define ekf2_log_origin_set(...) {}
555#define ekf2_log_check_start() {}
556#define ekf2_log_output(...) {}
557static inline bool ekf2_input_enabled(void) { return true; }
558#endif /* INS_EKF2_LOG_INPUTS */
559
560#if PERIODIC_TELEMETRY
562
563static void send_ins(struct transport_tx *trans, struct link_device *dev)
564{
565 struct NedCoor_i pos, speed, accel;
566
567 // Get it from the EKF
568 const Vector3f pos_f{ekf.getPosition()};
569 const Vector3f speed_f{ekf.getVelocity()};
570
571 // Convert to integer
572 pos.x = POS_BFP_OF_REAL(pos_f(0));
573 pos.y = POS_BFP_OF_REAL(pos_f(1));
574 pos.z = POS_BFP_OF_REAL(pos_f(2));
575 speed.x = SPEED_BFP_OF_REAL(speed_f(0));
576 speed.y = SPEED_BFP_OF_REAL(speed_f(1));
577 speed.z = SPEED_BFP_OF_REAL(speed_f(2));
578 accel.x = ACCEL_BFP_OF_REAL(ekf2.accel_ned.x);
579 accel.y = ACCEL_BFP_OF_REAL(ekf2.accel_ned.y);
580 accel.z = ACCEL_BFP_OF_REAL(ekf2.accel_ned.z);
581
582 // Send the message
583 pprz_msg_send_INS(trans, dev, AC_ID,
584 &pos.x, &pos.y, &pos.z,
585 &speed.x, &speed.y, &speed.z,
586 &accel.x, &accel.y, &accel.z);
587}
588
589static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
590{
591 float baro_z = 0.0f;
592 int32_t pos_z, speed_z, accel_z;
593
594 // Get it from the EKF
595 const Vector3f pos_f{ekf.getPosition()};
596 const Vector3f speed_f{ekf.getVelocity()};
597
598 // Convert to integer
599 pos_z = POS_BFP_OF_REAL(pos_f(2));
600 speed_z = SPEED_BFP_OF_REAL(speed_f(2));
601 accel_z = ACCEL_BFP_OF_REAL(ekf2.accel_ned.z);
602
603 // Send the message
604 pprz_msg_send_INS_Z(trans, dev, AC_ID,
605 &baro_z, &pos_z, &speed_z, &accel_z);
606}
607
608static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
609{
610 float qfe = 101325.0; //TODO: this is qnh not qfe?
611 if (ekf2.ltp_stamp > 0)
612 pprz_msg_send_INS_REF(trans, dev, AC_ID,
613 &ekf2.ltp_def.ecef.x, &ekf2.ltp_def.ecef.y, &ekf2.ltp_def.ecef.z,
614 &ekf2.ltp_def.lla.lat, &ekf2.ltp_def.lla.lon, &ekf2.ltp_def.lla.alt,
615 &ekf2.ltp_def.hmsl, &qfe);
616}
617
619static float test_ratio(float ratio)
620{
621 return PX4_ISFINITE(ratio) ? ratio : 0.f;
622}
623
624static void send_ins_ekf2(struct transport_tx *trans, struct link_device *dev)
625{
626 uint32_t control_mode = ekf.control_status().value; // lower 32 bits of the status flags
627 uint16_t filter_fault_status = ekf.fault_status().value;
628 uint16_t gps_check_status = gnss_checks.getStrictFailFlags();
629 uint16_t soln_status = ekf.get_ekf_soln_status();
630
631 // Innovation test ratios, a bit is set in innov_test_status when the test fails (ratio > 1)
632 float mag = test_ratio(ekf.getHeadingInnovationTestRatio());
633 float vel = test_ratio(math::max(test_ratio(ekf.getHorizontalVelocityInnovationTestRatio()),
634 test_ratio(ekf.getVerticalVelocityInnovationTestRatio())));
635 float pos = test_ratio(ekf.getHorizontalPositionInnovationTestRatio());
636 float hgt = test_ratio(ekf.getVerticalPositionInnovationTestRatio());
637 float tas = test_ratio(ekf.getAirspeedInnovationTestRatio());
638 float hagl = test_ratio(ekf.getHeightAboveGroundInnovationTestRatio());
639 float beta = test_ratio(ekf.getSyntheticSideslipInnovationTestRatio());
640 float flow = test_ratio(math::max(test_ratio(ekf.aid_src_optical_flow().test_ratio[0]),
641 test_ratio(ekf.aid_src_optical_flow().test_ratio[1])));
642 const float ratios[] = {mag, vel, pos, hgt, tas, hagl, flow, beta};
643 uint16_t innov_test_status = 0;
644 for (uint8_t i = 0; i < sizeof(ratios) / sizeof(ratios[0]); i++) {
645 if (ratios[i] > 1.f) {
646 innov_test_status |= (1 << i);
647 }
648 }
649
650 float mag_decl = 0.0f;
651 ekf.get_mag_decl_deg(mag_decl);
652
653 uint8_t terrain_valid = ekf.isTerrainEstimateValid() ? 1 : 0;
654 uint8_t dead_reckoning = ekf.control_status_flags().inertial_dead_reckoning ? 1 : 0;
655
656 pprz_msg_send_INS_EKF2(trans, dev, AC_ID,
657 &control_mode, &filter_fault_status, &gps_check_status, &soln_status,
658 &innov_test_status, &mag, &vel, &pos, &hgt, &tas, &hagl, &flow, &beta,
659 &mag_decl, &terrain_valid, &dead_reckoning);
660}
661
662static void send_ins_ekf2_ext(struct transport_tx *trans, struct link_device *dev)
663{
664 // GNSS drift on the ground, as computed by the GNSS checks
665 float gps_drift[3] = {
666 gnss_checks.horizontal_position_drift_rate_m_s(),
667 gnss_checks.vertical_position_drift_rate_m_s(),
668 gnss_checks.filtered_horizontal_velocity_m_s()
669 };
670 uint8_t gps_blocked = !PX4_ISFINITE(gps_drift[0]);
671
672 // Upper 32 bits of the control status (the lower ones are in INS_EKF2)
673 uint32_t control_mode_upper = (uint32_t)(ekf.control_status().value >> 32);
674
675 // Estimated accuracies
676 float pos_acc_h, pos_acc_v, vel_acc_h, vel_acc_v;
677 ekf.get_ekf_gpos_accuracy(&pos_acc_h, &pos_acc_v);
678 ekf.get_ekf_vel_accuracy(&vel_acc_h, &vel_acc_v);
679
680 // Height above ground and height sensor biases
681 float hagl = ekf.isTerrainEstimateValid() ? ekf.getHagl() : NAN;
682 float baro_bias = ekf.getBaroBiasEstimatorStatus().bias;
683 float gnss_hgt_bias = ekf.getGpsHgtBiasEstimatorStatus().bias;
684
685 // EKF-GSF emergency yaw estimate (independent of the magnetometer)
686 float yaw_gsf = NAN, yaw_gsf_var = NAN;
687 float gsf_yaw[N_MODELS_EKFGSF], gsf_innov_vn[N_MODELS_EKFGSF], gsf_innov_ve[N_MODELS_EKFGSF], gsf_weight[N_MODELS_EKFGSF];
688 if (!ekf.getDataEKFGSF(&yaw_gsf, &yaw_gsf_var, gsf_yaw, gsf_innov_vn, gsf_innov_ve, gsf_weight)) {
689 yaw_gsf = NAN;
690 yaw_gsf_var = NAN;
691 }
692 float yaw_gsf_std = (PX4_ISFINITE(yaw_gsf_var) && yaw_gsf_var >= 0.f) ? sqrtf(yaw_gsf_var) : NAN;
693
694 // Magnetometer checks against the world magnetic model
695 float mag_inc, mag_inc_ref, mag_strength, mag_strength_ref;
696 ekf.get_mag_checks(mag_inc, mag_inc_ref, mag_strength, mag_strength_ref);
697
698 pprz_msg_send_INS_EKF2_EXT(trans, dev, AC_ID,
699 &gps_drift[0], &gps_drift[1], &gps_drift[2], &gps_blocked,
700 &control_mode_upper,
701 &pos_acc_h, &pos_acc_v, &vel_acc_h, &vel_acc_v,
702 &hagl, &baro_bias, &gnss_hgt_bias,
703 &yaw_gsf, &yaw_gsf_std,
704 &mag_inc, &mag_inc_ref, &mag_strength, &mag_strength_ref);
705}
706
707static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
708{
709 uint8_t ahrs_ekf2_id = AHRS_COMP_ID_EKF2;
710 const auto &flags = ekf.control_status_flags();
711 uint32_t filter_fault_status = ekf.fault_status().value;
712 uint16_t filter_fault_status_16 = filter_fault_status; //FIXME
713 uint8_t mde = 0;
714
715 // Check the alignment and if GPS is fused
716 if (flags.tilt_align && flags.yaw_align && (flags.gnss_pos || flags.ev_pos)) {
717 mde = 3;
718 } else if (flags.tilt_align && flags.yaw_align) {
719 mde = 4;
720 } else {
721 mde = 2;
722 }
723
724 // Check if there is a covariance error
725 if (filter_fault_status) {
726 mde = 6;
727 }
728
729 pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_ekf2_id, &mde, &filter_fault_status_16);
730}
731
732static void send_wind_info_ret(struct transport_tx *trans, struct link_device *dev)
733{
734 Vector2f wind = ekf.getWindVelocity();
735 const Vector3f vel = ekf.getVelocity();
736 // True airspeed from the velocity and the wind estimate
737 float tas = Vector3f(vel(0) - wind(0), vel(1) - wind(1), vel(2)).norm();
738 uint8_t flags = 0x5;
739 float f_zero = 0;
740
741 pprz_msg_send_WIND_INFO_RET(trans, dev, AC_ID, &flags, &wind(1), &wind(0), &f_zero, &tas);
742}
743
744static void send_ahrs_bias(struct transport_tx *trans, struct link_device *dev)
745{
746 Vector3f accel_bias = ekf.getAccelBias();
747 Vector3f gyro_bias = ekf.getGyroBias();
748 Vector3f mag_bias = ekf.getMagBias();
749
750 pprz_msg_send_AHRS_BIAS(trans, dev, AC_ID, &accel_bias(0), &accel_bias(1), &accel_bias(2),
751 &gyro_bias(0), &gyro_bias(1), &gyro_bias(2), &mag_bias(0), &mag_bias(1), &mag_bias(2));
752}
753
754static void send_ahrs_quat(struct transport_tx *trans, struct link_device *dev)
755{
756 struct Int32Quat ltp_to_body_quat;
757 const Quatf att_q{ekf.getQuaternion()};
758 ltp_to_body_quat.qi = QUAT1_BFP_OF_REAL(att_q(0));
759 ltp_to_body_quat.qx = QUAT1_BFP_OF_REAL(att_q(1));
760 ltp_to_body_quat.qy = QUAT1_BFP_OF_REAL(att_q(2));
761 ltp_to_body_quat.qz = QUAT1_BFP_OF_REAL(att_q(3));
762 struct Int32Quat *quat = stateGetNedToBodyQuat_i();
763 float foo = 0.f;
764 uint8_t ahrs_id = 1; // generic
765 pprz_msg_send_AHRS_QUAT_INT(trans, dev, AC_ID,
766 &foo,
767 &ltp_to_body_quat.qi,
768 &ltp_to_body_quat.qx,
769 &ltp_to_body_quat.qy,
770 &ltp_to_body_quat.qz,
771 &(quat->qi),
772 &(quat->qx),
773 &(quat->qy),
774 &(quat->qz),
775 &ahrs_id);
776}
777
778
779static void send_external_pose_down(struct transport_tx *trans, struct link_device *dev)
780{
781 if (sample_ev.time_us == 0) {
782 return;
783 }
784 float sample_temp_ev[11];
785 sample_temp_ev[0] = (float) sample_ev.time_us;
786 sample_temp_ev[1] = sample_ev.pos(0) ;
787 sample_temp_ev[2] = sample_ev.pos(1) ;
788 sample_temp_ev[3] = sample_ev.pos(2) ;
789 sample_temp_ev[4] = sample_ev.vel(0) ;
790 sample_temp_ev[5] = sample_ev.vel(1) ;
791 sample_temp_ev[6] = sample_ev.vel(2) ;
792 sample_temp_ev[7] = sample_ev.quat(0);
793 sample_temp_ev[8] = sample_ev.quat(1);
794 sample_temp_ev[9] = sample_ev.quat(2);
795 sample_temp_ev[10] = sample_ev.quat(3);
796 pprz_msg_send_EXTERNAL_POSE_DOWN(trans, dev, AC_ID,
797 &sample_temp_ev[0],
798 &sample_temp_ev[1],
799 &sample_temp_ev[2],
800 &sample_temp_ev[3],
801 &sample_temp_ev[4],
802 &sample_temp_ev[5],
803 &sample_temp_ev[6],
804 &sample_temp_ev[7],
805 &sample_temp_ev[8],
806 &sample_temp_ev[9],
807 &sample_temp_ev[10]);
808}
809#endif
810
811/*
812 * EKF events and status changes as text (INFO_MSG) to the GCS and the flight recorder
813 */
814#ifndef INS_EKF2_EVENT_MSGS
815#define INS_EKF2_EVENT_MSGS TRUE
816#endif
817
820#ifndef INS_EKF2_EVENTS_ALL
821#define INS_EKF2_EVENTS_ALL FALSE
822#endif
824
826#ifndef INS_EKF2_EVENT_HOLDOFF
827#define INS_EKF2_EVENT_HOLDOFF 5.f
828#endif
829
830#if INS_EKF2_EVENT_MSGS
832#if FLIGHTRECORDER_SDLOG
835#endif
836
838struct ekf2_status_text {
839 uint8_t bit;
840 const char *on;
841 const char *off;
842 bool important;
843};
844
846static const struct ekf2_status_text ekf2_control_texts[] = {
847 {0, "tilt aligned", NULL, true},
848 {1, "yaw aligned", "yaw alignment lost", true},
849 {2, "GNSS pos fusion on", "GNSS pos fusion off", false},
850 {44, "GNSS vel fusion on", "GNSS vel fusion off", false},
851 {11, "GNSS hgt fusion on", "GNSS hgt fusion off", false},
852 {22, "GNSS yaw fusion on", "GNSS yaw fusion off", false},
853 {9, "baro hgt fusion on", "baro hgt fusion off", false},
854 {10, "range hgt fusion on", "range hgt fusion off", false},
855 {3, "flow fusion on", "flow fusion off", false},
856 {12, "EV pos fusion on", "EV pos fusion off", false},
857 {24, "EV vel fusion on", "EV vel fusion off", false},
858 {13, "EV yaw fusion on", "EV yaw fusion off", false},
859 {14, "EV hgt fusion on", "EV hgt fusion off", false},
860 {4, "mag hdg fusion on", "mag hdg fusion off", false},
861 {5, "mag 3D fusion on", "mag 3D fusion off", false},
862 {19, "airspeed fusion on", "airspeed fusion off", false},
863 {15, "sideslip fusion on", "sideslip fusion off", false},
864 {16, "mag field disturbed", "mag field ok", false},
865 {18, "mag fault", "mag fault cleared", true},
866 {45, "GNSS fault", "GNSS fault cleared", true},
867 {47, "GNSS hgt fault", "GNSS hgt fault cleared", true},
868 {27, "GNSS yaw fault", "GNSS yaw fault cleared", true},
869 {43, "baro fault", "baro fault cleared", true},
870 {28, "range fault", "range fault cleared", true},
871 {36, "EV yaw fault", "EV yaw fault cleared", true},
872 {29, "dead reckoning", "dead reckoning ended", true},
873 {30, "wind dead reckoning", "wind dead reckoning ended", true},
874};
875
877static const struct ekf2_status_text ekf2_fault_texts[] = {
878 {0, "mag X fusion error", NULL, true},
879 {1, "mag Y fusion error", NULL, true},
880 {2, "mag Z fusion error", NULL, true},
881 {3, "heading fusion error", NULL, true},
882 {4, "mag decl fusion error", NULL, true},
883 {5, "airspeed fusion error", NULL, true},
884 {6, "sideslip fusion error", NULL, true},
885 {7, "flow X fusion error", NULL, true},
886 {8, "flow Y fusion error", NULL, true},
887 {10, "bad vertical accel", NULL, true},
888 {11, "accel clipping", NULL, true},
889};
890
893static const char *const ekf2_info_texts[] = {
894 "GNSS checks passed", "vel reset to GNSS", "vel reset to flow", "vel reset to EV", "vel reset to zero",
895 "pos reset to last known", "pos reset to GNSS", "pos reset to EV", NULL,
896 NULL, NULL, NULL, "yaw aligned to IMU/GNSS",
897 "hgt reset to baro", "hgt reset to GNSS", "hgt reset to range", "hgt reset to EV",
898 "pos reset to ext obs", "wind reset to ext obs",
899};
900
902#define EKF2_INFO_IMPORTANT ((1UL << 0) | (1UL << 12))
903
904#define EKF2_CONTROL_TEXTS_NB (sizeof(ekf2_control_texts) / sizeof(ekf2_control_texts[0]))
905#define EKF2_FAULT_TEXTS_NB (sizeof(ekf2_fault_texts) / sizeof(ekf2_fault_texts[0]))
906#define EKF2_INFO_TEXTS_NB (sizeof(ekf2_info_texts) / sizeof(ekf2_info_texts[0]))
907
908static struct {
909 uint64_t control;
910 uint32_t fault;
911 float control_time[EKF2_CONTROL_TEXTS_NB];
912 float fault_time[EKF2_FAULT_TEXTS_NB];
913 float info_time[EKF2_INFO_TEXTS_NB];
914 float last_check;
915 struct {
916 char text[120];
917 uint8_t len;
918 } gcs, log;
919} ekf2_events;
920
921static void ekf2_event_send(void)
922{
923#if defined SITL
924 if (ekf2_events.log.len > 0) {
925 printf("%s\n", ekf2_events.log.text);
926 }
927#endif
928#if DOWNLINK
929 if (ekf2_events.gcs.len > 0) {
930 DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, ekf2_events.gcs.len, ekf2_events.gcs.text);
931 }
932#endif
933#if FLIGHTRECORDER_SDLOG
934 if (ekf2_events.log.len > 0 && flightRecorderLogFile != -1) {
935 pprz_msg_send_INFO_MSG(&pprzlog_tp.trans_tx, &flightrecorder_sdlog.device, AC_ID, ekf2_events.log.len, ekf2_events.log.text);
936 }
937#endif
938 ekf2_events.gcs.len = 0;
939 ekf2_events.log.len = 0;
940}
941
943static void ekf2_event_append(char *msg, uint8_t *len, size_t size, const char *text)
944{
945 int n = snprintf(msg + *len, size - *len, *len == 0 ? "EKF2: %s" : ", %s", text);
946 if (n > 0) {
947 *len = Min(*len + n, (int)size - 1);
948 }
949}
950
953static void ekf2_event_add(const char *text, bool important)
954{
955 size_t tlen = strlen(text);
956 if ((ekf2_events.log.len > 0 && ekf2_events.log.len + 2 + tlen >= sizeof(ekf2_events.log.text)) ||
957 (ekf2_events.gcs.len > 0 && ekf2_events.gcs.len + 2 + tlen >= sizeof(ekf2_events.gcs.text))) {
958 ekf2_event_send();
959 }
960 ekf2_event_append(ekf2_events.log.text, &ekf2_events.log.len, sizeof(ekf2_events.log.text), text);
961 if (important || ins_ekf2_events_all) {
962 ekf2_event_append(ekf2_events.gcs.text, &ekf2_events.gcs.len, sizeof(ekf2_events.gcs.text), text);
963 }
964}
965
967template<typename T>
968static void ekf2_event_bits(const struct ekf2_status_text *texts, uint8_t nb, float *times, T &reported, T current, float now)
969{
970 for (uint8_t i = 0; i < nb; i++) {
971 T mask = ((T)1) << texts[i].bit;
972 if ((current & mask) == (reported & mask) || (times[i] > 0.f && now - times[i] < INS_EKF2_EVENT_HOLDOFF)) {
973 continue;
974 }
975 const char *text = (current & mask) ? texts[i].on : texts[i].off;
976 if (text != NULL) {
977 ekf2_event_add(text, texts[i].important);
978 times[i] = now;
979 }
980 reported = (reported & ~mask) | (current & mask);
981 }
982}
983
985static void ins_ekf2_check_events(void)
986{
987 float now = get_sys_time_float();
988 if (now - ekf2_events.last_check < 0.5f) {
989 return;
990 }
991 ekf2_events.last_check = now;
992
993 // Information events (e.g. the resets repeated without aiding), at most once per INS_EKF2_EVENT_HOLDOFF per event
994 uint32_t info = ekf.information_event_status().value;
995 ekf.clear_information_events();
996 for (uint8_t i = 0; i < EKF2_INFO_TEXTS_NB; i++) {
997 float *time = &ekf2_events.info_time[i];
998 if ((info & (1UL << i)) && ekf2_info_texts[i] != NULL && (*time <= 0.f || now - *time >= INS_EKF2_EVENT_HOLDOFF)) {
999 ekf2_event_add(ekf2_info_texts[i], (EKF2_INFO_IMPORTANT & (1UL << i)) != 0);
1000 *time = now;
1001 }
1002 }
1003 ekf2_event_bits<uint64_t>(ekf2_control_texts, EKF2_CONTROL_TEXTS_NB, ekf2_events.control_time,
1004 ekf2_events.control, ekf.control_status().value, now);
1005 ekf2_event_bits<uint32_t>(ekf2_fault_texts, EKF2_FAULT_TEXTS_NB, ekf2_events.fault_time,
1006 ekf2_events.fault, ekf.fault_status().value, now);
1007 ekf2_event_send();
1008}
1009#else
1010#define ins_ekf2_check_events() {}
1011#endif /* INS_EKF2_EVENT_MSGS */
1012
1013/*
1014 * Preflight check: the EKF is aligned, without faults, and uses the configured position source
1015 */
1016#if PREFLIGHT_CHECKS
1018
1020#ifndef INS_EKF2_PREFLIGHT_POS
1021#define INS_EKF2_PREFLIGHT_POS TRUE
1022#endif
1023
1026#ifndef INS_EKF2_PREFLIGHT_INNOV_TAU
1027#define INS_EKF2_PREFLIGHT_INNOV_TAU 1.f
1028#endif
1029
1030static struct preflight_check_t ins_ekf2_pfc;
1031
1033#define EKF2_PREFLIGHT_INNOV_NB 4
1034static const char *const ekf2_preflight_innov_names[EKF2_PREFLIGHT_INNOV_NB] = {"heading", "velocity", "position", "height"};
1035static struct {
1036 float ratio[EKF2_PREFLIGHT_INNOV_NB];
1037 float last_time;
1038} ekf2_preflight;
1039
1041static void ins_ekf2_preflight_filter(void)
1042{
1043 float now = get_sys_time_float();
1044 float dt = now - ekf2_preflight.last_time;
1045 ekf2_preflight.last_time = now;
1046 const float ratio[EKF2_PREFLIGHT_INNOV_NB] = {
1047 ekf.getHeadingInnovationTestRatio(),
1048 math::max(ekf.getHorizontalVelocityInnovationTestRatio(), ekf.getVerticalVelocityInnovationTestRatio()),
1049 ekf.getHorizontalPositionInnovationTestRatio(),
1050 ekf.getVerticalPositionInnovationTestRatio()
1051 };
1052 float alpha = math::constrain(dt / (INS_EKF2_PREFLIGHT_INNOV_TAU + dt), 0.f, 1.f);
1053 for (uint8_t i = 0; i < EKF2_PREFLIGHT_INNOV_NB; i++) {
1054 if (!PX4_ISFINITE(ratio[i])) {
1055 ekf2_preflight.ratio[i] = NAN;
1056 } else if (!PX4_ISFINITE(ekf2_preflight.ratio[i])) {
1057 ekf2_preflight.ratio[i] = ratio[i];
1058 } else {
1059 ekf2_preflight.ratio[i] += alpha * (ratio[i] - ekf2_preflight.ratio[i]);
1060 }
1061 }
1062}
1063
1064static void ins_ekf2_preflight(struct preflight_result_t *result)
1065{
1066 const auto &flags = ekf.control_status_flags();
1067 uint16_t fail_cnt = result->fail_cnt;
1068
1069 if (!flags.tilt_align) {
1070 preflight_error(result, "EKF2 attitude not aligned");
1071 }
1072 if (!flags.yaw_align) {
1073 preflight_error(result, "EKF2 heading not aligned");
1074 }
1075 if (ekf.fault_status().value != 0) {
1076 preflight_error(result, "EKF2 numerical fault 0x%x", (unsigned int)ekf.fault_status().value);
1077 }
1078 if (flags.mag_fault || flags.gnss_fault || flags.baro_fault || flags.gnss_hgt_fault || flags.gnss_yaw_fault) {
1079 preflight_error(result, "EKF2 sensor fault (mag %d, GNSS %d, baro %d, GNSS hgt %d, GNSS yaw %d)",
1080 flags.mag_fault, flags.gnss_fault, flags.baro_fault, flags.gnss_hgt_fault, flags.gnss_yaw_fault);
1081 }
1082 if (flags.inertial_dead_reckoning) {
1083 preflight_error(result, "EKF2 dead reckoning");
1084 }
1085
1086#if INS_EKF2_PREFLIGHT_POS
1087 const int32_t gnss_pos = static_cast<int32_t>(GnssCtrl::HPOS) | static_cast<int32_t>(GnssCtrl::VEL);
1088 if ((ekf_params->ekf2_gps_ctrl & gnss_pos) && !(flags.gnss_pos || flags.gnss_vel)) {
1089 static const char *const names[] = {"nsats", "pdop", "hacc", "vacc", "sacc", "hdrift", "vdrift", "hspeed",
1090 "vspeed", "spoofed", "fix", "jammed"
1091 };
1092 char failed[80] = "";
1093 uint16_t flags_fail = gnss_checks.getStrictFailFlags();
1094 for (uint8_t i = 0; i < sizeof(names) / sizeof(names[0]); i++) {
1095 if (flags_fail & (1 << i)) {
1096 size_t l = strlen(failed);
1097 snprintf(failed + l, sizeof(failed) - l, "%s%s", l ? "," : "", names[i]);
1098 }
1099 }
1100 preflight_error(result, "EKF2 not using GNSS (failed: %s)", failed[0] ? failed : "no data");
1101 }
1102 const int32_t ev_pos = static_cast<int32_t>(EvCtrl::HPOS) | static_cast<int32_t>(EvCtrl::VEL);
1103 if ((ekf_params->ekf2_ev_ctrl & ev_pos) && !(flags.ev_pos || flags.ev_vel)) {
1104 preflight_error(result, "EKF2 not using external vision");
1105 }
1106#endif
1107
1108 // Filtered innovation test ratios above 1 mean the measurements are rejected
1109 for (uint8_t i = 0; i < EKF2_PREFLIGHT_INNOV_NB; i++) {
1110 if (PX4_ISFINITE(ekf2_preflight.ratio[i]) && ekf2_preflight.ratio[i] > 1.f) {
1111 preflight_error(result, "EKF2 %s innovation too high (%.1f)", ekf2_preflight_innov_names[i], ekf2_preflight.ratio[i]);
1112 }
1113 }
1114
1115 if (flags.mag_field_disturbed) {
1116 preflight_warning(result, "EKF2 mag field disturbed");
1117 }
1118 if (result->fail_cnt == fail_cnt) {
1119 preflight_success(result, "EKF2 ok");
1120 }
1121}
1122#else
1123#define ins_ekf2_preflight_filter() {}
1124#endif /* PREFLIGHT_CHECKS */
1125
1126/* Initialize the EKF */
1128{
1129 /* Get the ekf parameters, all with the PX4 defaults or the airframe values */
1130 ekf_params = ekf.getParamHandle();
1131 ins_ekf2_set_px4_params(ekf_params);
1132
1133 /* The observation buffers must also hold the GNSS delay, which is not an EKF parameter */
1134 ekf_params->ekf2_delay_max = math::max((float)INS_EKF2_DELAY_MAX, (float)INS_EKF2_GPS_DELAY);
1135
1136 /* Output predictor, as in the PX4 EKF2 module */
1137 ekf.output_predictor().set_imu_offset(ekf_params->imu_pos_body);
1138 ekf.output_predictor().set_pos_correction_tc(INS_EKF2_TAU_POS);
1139 ekf.output_predictor().set_vel_correction_tc(INS_EKF2_TAU_VEL);
1140 ekf.set_gnss_yaw_reset_wait(INS_EKF2_GPS_YAW_RST * 1000000);
1141
1142 /* Enable all sensors, which ones are used follows from the parameters above */
1143 FusionControl *fc = ekf.getFusionControlHandle();
1144 fc->gps.enabled = true;
1145 fc->of.enabled = true;
1146 fc->ev.enabled = true;
1147 fc->baro.enabled = true;
1148 fc->rng.enabled = true;
1149 fc->mag.enabled = true;
1150 fc->aspd.enabled = true;
1151 ekf.updateParameters();
1152
1153 /* GNSS checks before the GNSS data is used, as the PX4 GNSS_* parameters */
1154 GnssChecks::Params checks_params{};
1155 checks_params.check_mask = INS_EKF2_GNSS_CHECK;
1156 checks_params.req_nsats = INS_EKF2_GNSS_REQ_NSATS;
1157 checks_params.req_pdop = INS_EKF2_GNSS_REQ_PDOP;
1158 checks_params.req_eph = INS_EKF2_GNSS_REQ_EPH;
1159 checks_params.req_epv = INS_EKF2_GNSS_REQ_EPV;
1160 checks_params.req_sacc = INS_EKF2_GNSS_REQ_SACC;
1161 checks_params.req_hdrift = INS_EKF2_GNSS_REQ_HDRIFT;
1162 checks_params.req_vdrift = INS_EKF2_GNSS_REQ_VDRIFT;
1163 checks_params.req_fix = INS_EKF2_GNSS_REQ_FIX;
1164 checks_params.min_health_time_us = INS_EKF2_GNSS_REQ_TIME * 1000000;
1165 gnss_checks.setParams(checks_params);
1166
1167 /* Initialize struct */
1168 ekf2.ltp_stamp = 0;
1169 ekf2.flow_stamp = 0;
1170 ekf2.gyro_valid = false;
1171 ekf2.accel_valid = false;
1172 ekf2.got_imu_data = false;
1173 ekf2.quat_reset_counter = 0;
1174 ekf2.temp = 20.0f; // Default temperature of 20 degrees celcius
1175 ekf2.in_air = false;
1176 ekf2.armed = false;
1177 ekf2.fixed_wing = false;
1178 ekf2.air_density = PPRZ_ISA_AIR_DENSITY;
1179 /* Field strength of the world magnetic model at the flight plan origin, for a magnetometer without
1180 * the strength where it was calibrated (strength in IMU_MAG_CALIB) */
1181 ekf2.mag_scale = 0.4f;
1182#if USE_INS_NAV_INIT
1183 float strength = get_mag_strength_gauss(NAV_LAT0 * 1e-7f, NAV_LON0 * 1e-7f);
1184 if (PX4_ISFINITE(strength) && strength > 0.1f && strength < 1.f) {
1185 ekf2.mag_scale = strength;
1186 }
1187#endif
1188 ekf2.system_flags_time = 0;
1189 FLOAT_VECT3_ZERO(ekf2.accel_ned);
1190
1191 /* Initialize the range sensor limits */
1192 ekf.set_rangefinder_limits(INS_EKF2_RNG_MIN_DIST, INS_EKF2_RNG_MAX_DIST);
1193
1194 /* Initialize the flow sensor limits */
1196
1197 // Don't send external vision data by default
1198 sample_ev.time_us = 0;
1199
1200 /* Initialize the origin from flight plan */
1201#if USE_INS_NAV_INIT
1202 ekf2_log_origin_set(ekf2_time_us(get_sys_time_usec()), NAV_LAT0 * 1e-7, NAV_LON0 * 1e-7, (NAV_ALT0) * 1e-3);
1203 if (ekf.setEkfGlobalOrigin(NAV_LAT0 * 1e-7, NAV_LON0 * 1e-7, (NAV_ALT0) * 1e-3)) { // EKF2 works HMSL
1204 struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
1205 llh_nav0.lat = NAV_LAT0;
1206 llh_nav0.lon = NAV_LON0;
1207 /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
1208 llh_nav0.alt = NAV_ALT0 + NAV_MSL0; // in millimeters above WGS84 reference ellipsoid
1209
1210 ltp_def_from_lla_i(&ekf2.ltp_def, &llh_nav0);
1211 ekf2.ltp_def.hmsl = NAV_ALT0;
1212 stateSetLocalOrigin_i(MODULE_INS_EKF2_ID, &ekf2.ltp_def);
1213
1214 /* update local ENU coordinates of global waypoints */
1216
1217 ekf2.ltp_stamp = 1;
1218 }
1219#endif
1220
1221#if PERIODIC_TELEMETRY
1227 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
1230 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_QUAT_INT, send_ahrs_quat);
1232#endif
1233
1234 /*
1235 * Subscribe to scaled IMU measurements and attach callbacks
1236 */
1237 AbiBindMsgBARO_ABS(INS_EKF2_BARO_ID, &baro_ev, baro_cb);
1238 AbiBindMsgTEMPERATURE(INS_EKF2_TEMPERATURE_ID, &temperature_ev, temperature_cb);
1239 AbiBindMsgAGL(INS_EKF2_AGL_ID, &agl_ev, agl_cb);
1240 AbiBindMsgIMU_GYRO_INT(INS_EKF2_GYRO_ID, &gyro_int_ev, gyro_int_cb);
1241 AbiBindMsgIMU_ACCEL_INT(INS_EKF2_ACCEL_ID, &accel_int_ev, accel_int_cb);
1242 AbiBindMsgIMU_MAG(INS_EKF2_MAG_ID, &mag_ev, mag_cb);
1243 AbiBindMsgGPS(INS_EKF2_GPS_ID, &gps_ev, gps_cb);
1244 AbiBindMsgRELPOS(INS_EKF2_RELPOS_ID, &relpos_ev, relpos_cb);
1245 AbiBindMsgOPTICAL_FLOW(INS_EKF2_OF_ID, &optical_flow_ev, optical_flow_cb);
1246 AbiBindMsgAIRSPEED(INS_EKF2_AIRSPEED_ID, &airspeed_ev, airspeed_cb);
1247 AbiBindMsgINS_RESET(ABI_BROADCAST, &reset_ev, reset_cb);
1248
1249#if PREFLIGHT_CHECKS
1250 for (uint8_t i = 0; i < EKF2_PREFLIGHT_INNOV_NB; i++) {
1251 ekf2_preflight.ratio[i] = NAN;
1252 }
1253 preflight_check_register(&ins_ekf2_pfc, ins_ekf2_preflight);
1254#endif
1255}
1256
1257static void reset_ref(void)
1258{
1259#if USE_GPS
1260 if (GpsFixValid()) {
1261 struct LlaCoor_i lla_pos = lla_int_from_gps(&gps);
1262 ekf2_log_origin_set(ekf2_time_us(get_sys_time_usec()), lla_pos.lat * 1e-7, lla_pos.lon * 1e-7, gps.hmsl * 1e-3);
1263 if (ekf.setEkfGlobalOrigin(lla_pos.lat * 1e-7, lla_pos.lon * 1e-7, gps.hmsl * 1e-3)) {
1264 ltp_def_from_lla_i(&ekf2.ltp_def, &lla_pos);
1265 ekf2.ltp_def.hmsl = gps.hmsl;
1266 stateSetLocalOrigin_i(MODULE_INS_EKF2_ID, &ekf2.ltp_def);
1267 }
1268 }
1269#endif
1270}
1271
1272static void reset_vertical_ref(void)
1273{
1274#if USE_GPS
1275 if (GpsFixValid()) {
1276 struct LlaCoor_i lla_pos = lla_int_from_gps(&gps);
1277 struct LlaCoor_i lla = {
1278 .lat = stateGetLlaOrigin_i().lat,
1279 .lon = stateGetLlaOrigin_i().lon,
1280 .alt = lla_pos.alt
1281 };
1282 ekf2_log_origin_set(ekf2_time_us(get_sys_time_usec()), lla.lat * 1e-7, lla.lon * 1e-7, gps.hmsl * 1e-3);
1283 if (ekf.setEkfGlobalOrigin(lla.lat * 1e-7, lla.lon * 1e-7, gps.hmsl * 1e-3)) {
1284 ltp_def_from_lla_i(&ekf2.ltp_def, &lla);
1285 ekf2.ltp_def.hmsl = gps.hmsl;
1286 stateSetLocalOrigin_i(MODULE_INS_EKF2_ID, &ekf2.ltp_def);
1287 }
1288 }
1289#endif
1290}
1291
1292static void reset_cb(uint8_t sender_id UNUSED, uint8_t flag)
1293{
1294 switch (flag) {
1295 case INS_RESET_REF:
1296 reset_ref();
1297 break;
1300 break;
1301 default:
1302 // unsupported cases
1303 break;
1304 }
1305}
1306
1308static struct {
1309 Vector3f gyro_avg;
1310 Vector3f accel_avg;
1311 float gyro_metric;
1312 float accel_metric;
1313 float still_time;
1314 bool init;
1315 volatile bool still;
1316 bool at_rest;
1318
1319static void ins_ekf2_at_rest_update(const imuSample &imu, const Vector3f &gyro_bias)
1320{
1321 if (!(imu.delta_ang_dt > 0.f) || !(imu.delta_vel_dt > 0.f)) {
1322 return;
1323 }
1324 const float dt = imu.delta_ang_dt;
1325 const Vector3f gyro = imu.delta_ang / imu.delta_ang_dt;
1326 const Vector3f accel = imu.delta_vel / imu.delta_vel_dt;
1327 const float gyro_thr = INS_EKF2_AT_REST_GYRO * (float)M_PI / 180.f;
1328 if (!ekf2_rest.init) {
1329 ekf2_rest.gyro_avg = gyro;
1330 ekf2_rest.accel_avg = accel;
1331 ekf2_rest.gyro_metric = 10.f * gyro_thr;
1332 ekf2_rest.accel_metric = 10.f * INS_EKF2_AT_REST_ACCEL;
1333 ekf2_rest.init = true;
1334 return;
1335 }
1336 const float alpha_avg = fminf(dt / 2.f, 1.f);
1337 const float alpha = fminf(dt / 0.5f, 1.f);
1338 ekf2_rest.gyro_avg += (gyro - ekf2_rest.gyro_avg) * alpha_avg;
1339 ekf2_rest.accel_avg += (accel - ekf2_rest.accel_avg) * alpha_avg;
1340 ekf2_rest.gyro_metric += ((gyro - ekf2_rest.gyro_avg).norm() - ekf2_rest.gyro_metric) * alpha;
1341 ekf2_rest.accel_metric += ((accel - ekf2_rest.accel_avg).norm() - ekf2_rest.accel_metric) * alpha;
1342 const float rate = (ekf2_rest.gyro_avg - gyro_bias).norm();
1343 if (ekf2_rest.gyro_metric < gyro_thr && ekf2_rest.accel_metric < INS_EKF2_AT_REST_ACCEL
1344 && rate < INS_EKF2_AT_REST_RATE * (float)M_PI / 180.f) {
1345 ekf2_rest.still_time = fminf(ekf2_rest.still_time + dt, INS_EKF2_AT_REST_TIME);
1346 } else {
1347 ekf2_rest.still_time = 0.f;
1348 }
1349 ekf2_rest.still = (ekf2_rest.still_time >= INS_EKF2_AT_REST_TIME);
1350}
1351
1355{
1356 bool in_air = autopilot_in_flight();
1357 bool armed = autopilot_get_motors_on();
1358 // airspeed and sideslip fusion, no ground effect, airspeed for the yaw estimator: always for the fixedwing
1359 // firmware, in forward flight for hybrids flown with the rotorcraft firmware (e.g. rotwing_state)
1360 bool fixed_wing = stateIsFixedWingFlight();
1361 bool at_rest = INS_EKF2_AT_REST_DETECTION && !armed && !in_air && ekf2_rest.still;
1363 if (in_air == ekf2.in_air && armed == ekf2.armed && fixed_wing == ekf2.fixed_wing && at_rest == ekf2_rest.at_rest
1364 && now < ekf2.system_flags_time + 1000000) {
1365 return;
1366 }
1367
1368 systemFlagUpdate flags{};
1369 flags.time_us = now;
1370 flags.armed = armed;
1371 flags.in_air = in_air;
1372 flags.at_rest = at_rest;
1373 flags.is_fixed_wing = fixed_wing;
1374 {
1375 int32_t bits = (flags.armed << 0) | (flags.at_rest << 1) | (flags.in_air << 2) | (flags.is_fixed_wing << 3)
1376 | (flags.gnd_effect << 4) | (flags.constant_pos << 5) | (flags.in_transition << 6);
1377 ekf2_log(EKF2_IN_FLAGS, flags.time_us, 1, &bits, 0, NULL);
1378 }
1379 ekf.setSystemFlagData(flags);
1380
1381 ekf2.in_air = in_air;
1382 ekf2.armed = armed;
1383 ekf2.fixed_wing = fixed_wing;
1384 ekf2_rest.at_rest = at_rest;
1385 ekf2.system_flags_time = now;
1386}
1387
1393static struct {
1394 float eas;
1395 uint32_t stamp;
1396 volatile bool new_data;
1398
1399static void airspeed_cb(uint8_t sender_id, float eas)
1400{
1401 if (sender_id == AIRSPEED_WE_QUAD_ID || !PX4_ISFINITE(eas) || eas < 0.f) {
1402 return;
1403 }
1404 ekf2_airspeed.eas = eas;
1406 ekf2_airspeed.new_data = true;
1407}
1408
1411{
1412 if (!ekf2_airspeed.new_data) {
1413 return;
1414 }
1415 const float eas = ekf2_airspeed.eas;
1416 const uint32_t stamp = ekf2_airspeed.stamp;
1417 ekf2_airspeed.new_data = false;
1418 if (ekf2.air_density <= 0.f) {
1419 return;
1420 }
1421 airspeedSample sample{};
1422 sample.time_us = ekf2_time_us(stamp);
1423 sample.eas2tas = sqrtf(PPRZ_ISA_AIR_DENSITY / ekf2.air_density);
1424 sample.true_airspeed = eas * sample.eas2tas;
1425 if (!ekf2_input_enabled()) {
1426 return;
1427 }
1428 {
1429 float values[2] = {sample.true_airspeed, sample.eas2tas};
1430 ekf2_log(EKF2_IN_AIRSPEED, sample.time_us, 0, NULL, 2, values);
1431 }
1432 ekf.setAirspeedData(sample);
1433}
1434
1435/* Update the INS state */
1437{
1439 if (!ekf2_input_enabled()) {
1440 return;
1441 }
1444
1445 /* Update the EKF */
1446 if (ekf2.got_imu_data) {
1447 // Update the EKF but ignore the response and also copy the faster intermediate filter
1448 uint64_t update_time = ekf2_time_us(get_sys_time_usec());
1449 ekf2_log(EKF2_IN_UPDATE, update_time, 0, NULL, 0, NULL);
1450 ekf.update();
1451 ekf2_log_output(update_time);
1454
1455 /* Get the accelerations in NED frame (average since the last update) */
1456 const Vector3f vel_deriv_f{ekf.getVelocityDerivative()};
1457 ekf.resetVelocityDerivativeAccumulation();
1458
1459 // Only publish position after successful alignment
1460 if (ekf.control_status_flags().tilt_align) {
1461 /* Get the position */
1462 const Vector3f pos_f{ekf.getPosition()};
1463 struct NedCoor_f pos;
1464 pos.x = pos_f(0);
1465 pos.y = pos_f(1);
1466 pos.z = pos_f(2);
1467
1468 // Publish to the state
1469 stateSetPositionNed_f(MODULE_INS_EKF2_ID, &pos);
1470
1471 /* Get the velocity in NED frame */
1472 const Vector3f vel_f{ekf.getVelocity()};
1473 struct NedCoor_f speed;
1474 speed.x = vel_f(0);
1475 speed.y = vel_f(1);
1476 speed.z = vel_f(2);
1477
1478 // Publish to state
1479 stateSetSpeedNed_f(MODULE_INS_EKF2_ID, &speed);
1480
1481 ekf2.accel_ned.x = vel_deriv_f(0);
1482 ekf2.accel_ned.y = vel_deriv_f(1);
1483 ekf2.accel_ned.z = vel_deriv_f(2);
1484
1485 // Publish to state
1486 struct NedCoor_f accel;
1487 accel.x = ekf2.accel_ned.x;
1488 accel.y = ekf2.accel_ned.y;
1489 accel.z = ekf2.accel_ned.z;
1490 stateSetAccelNed_f(MODULE_INS_EKF2_ID, &accel);
1491
1492 /* Get local origin */
1493 // Position of local NED origin in GPS / WGS84 frame
1494 double ekf_origin_lat, ekf_origin_lon;
1495 float ref_alt;
1496 struct LlaCoor_i lla_ref;
1497 uint64_t origin_time;
1498
1499 // Only update the origin when the state estimator has updated the origin
1500 if (ekf.global_origin_valid()) {
1501 ekf.getEkfGlobalOrigin(origin_time, ekf_origin_lat, ekf_origin_lon, ref_alt);
1502 } else {
1503 origin_time = 0;
1504 }
1505 if (origin_time > ekf2.ltp_stamp) {
1506#if INS_EKF2_FORCE_NAV_REF && USE_INS_NAV_INIT
1507 ekf2_log_origin_set(ekf2_time_us(get_sys_time_usec()), NAV_LAT0 * 1e-7, NAV_LON0 * 1e-7, (NAV_ALT0) * 1e-3);
1508 if (ekf.setEkfGlobalOrigin(NAV_LAT0 * 1e-7, NAV_LON0 * 1e-7, (NAV_ALT0) * 1e-3)) {
1509 struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
1510 llh_nav0.lat = NAV_LAT0;
1511 llh_nav0.lon = NAV_LON0;
1512 llh_nav0.alt = NAV_ALT0 + NAV_MSL0; // in millimeters above WGS84 reference ellipsoid
1513
1514 ltp_def_from_lla_i(&ekf2.ltp_def, &llh_nav0);
1515 ekf2.ltp_def.hmsl = NAV_ALT0;
1516 stateSetLocalOrigin_i(MODULE_INS_EKF2_ID, &ekf2.ltp_def);
1517 }
1518#else
1519 lla_ref.lat = ekf_origin_lat * 1e7; // WGS-84 lat
1520 lla_ref.lon = ekf_origin_lon * 1e7; // WGS-84 lon
1521 lla_ref.alt = ref_alt * 1e3 + wgs84_ellipsoid_to_geoid_i(lla_ref.lat, lla_ref.lon); // in millimeters above WGS84 reference ellipsoid (ref_alt is in HMSL)
1522 ltp_def_from_lla_i(&ekf2.ltp_def, &lla_ref);
1523 ekf2.ltp_def.hmsl = ref_alt * 1e3;
1524 stateSetLocalOrigin_i(MODULE_INS_EKF2_ID, &ekf2.ltp_def);
1525#endif
1526 /* update local ENU coordinates of global waypoints */
1528
1529 ekf2.ltp_stamp = origin_time;
1530 }
1531 }
1532 }
1533
1534#if defined SITL && USE_NPS
1535 if (nps_bypass_ins) {
1537 }
1538#endif
1539
1540 ekf2.got_imu_data = false;
1541}
1542
1544{
1545 if (DL_EXTERNAL_POSE_ac_id(buf) != AC_ID) { return; } // not for this aircraft
1546
1547 sample_ev.time_us = ekf2_time_us(get_sys_time_usec()); //FIXME
1548 sample_ev.pos(0) = DL_EXTERNAL_POSE_enu_y(buf);
1549 sample_ev.pos(1) = DL_EXTERNAL_POSE_enu_x(buf);
1550 sample_ev.pos(2) = -DL_EXTERNAL_POSE_enu_z(buf);
1551 sample_ev.vel(0) = DL_EXTERNAL_POSE_enu_yd(buf);
1552 sample_ev.vel(1) = DL_EXTERNAL_POSE_enu_xd(buf);
1553 sample_ev.vel(2) = -DL_EXTERNAL_POSE_enu_zd(buf);
1554 sample_ev.quat(0) = DL_EXTERNAL_POSE_body_qi(buf);
1555 sample_ev.quat(1) = DL_EXTERNAL_POSE_body_qy(buf);
1556 sample_ev.quat(2) = DL_EXTERNAL_POSE_body_qx(buf);
1557 sample_ev.quat(3) = -DL_EXTERNAL_POSE_body_qz(buf);
1558
1559#ifdef INS_EXT_VISION_ROTATION
1560 // Rotate the quaternion
1561 struct FloatQuat body_q = {sample_ev.quat(0), sample_ev.quat(1), sample_ev.quat(2), sample_ev.quat(3)};
1562 struct FloatQuat rot_q;
1563 float_quat_comp(&rot_q, &body_q, &ins_ext_vision_rot);
1564 sample_ev.quat(0) = rot_q.qi;
1565 sample_ev.quat(1) = rot_q.qx;
1566 sample_ev.quat(2) = rot_q.qy;
1567 sample_ev.quat(3) = rot_q.qz;
1568#endif
1569
1570 // The external pose is given in a north aligned frame (converted from ENU to NED above)
1571 sample_ev.position_var.setAll(INS_EKF2_EVP_NOISE * INS_EKF2_EVP_NOISE);
1572 sample_ev.velocity_var.setAll(INS_EKF2_EVV_NOISE * INS_EKF2_EVV_NOISE);
1573 sample_ev.orientation_var.setAll(INS_EKF2_EVA_NOISE * INS_EKF2_EVA_NOISE);
1574 sample_ev.pos_frame = PositionFrame::LOCAL_FRAME_NED;
1575 sample_ev.vel_frame = VelocityFrame::LOCAL_FRAME_NED;
1576 sample_ev.quality = 100;
1577
1578 if (!ekf2_input_enabled()) {
1579 return;
1580 }
1581 {
1582 int32_t ints[4] = {(int32_t)sample_ev.pos_frame, (int32_t)sample_ev.vel_frame, sample_ev.quality, sample_ev.reset_counter};
1583 float values[19] = {sample_ev.pos(0), sample_ev.pos(1), sample_ev.pos(2), sample_ev.vel(0), sample_ev.vel(1), sample_ev.vel(2),
1584 sample_ev.quat(0), sample_ev.quat(1), sample_ev.quat(2), sample_ev.quat(3),
1585 sample_ev.position_var(0), sample_ev.position_var(1), sample_ev.position_var(2),
1586 sample_ev.velocity_var(0), sample_ev.velocity_var(1), sample_ev.velocity_var(2),
1587 sample_ev.orientation_var(0), sample_ev.orientation_var(1), sample_ev.orientation_var(2)};
1588 ekf2_log(EKF2_IN_EV, sample_ev.time_us, 4, ints, 19, values);
1589 }
1590 ekf.setExtVisionData(sample_ev);
1591}
1592
1593void ins_ekf2_parse_EXTERNAL_POSE_SMALL(uint8_t __attribute__((unused)) *buf)
1594{
1595
1596}
1597
1602{
1603 imuSample imu_sample = {};
1604 imu_sample.time_us = time_us;
1605 imu_sample.delta_ang_dt = ekf2.gyro_dt * 1.e-6f;
1606 imu_sample.delta_ang = Vector3f{ekf2.delta_gyro.p, ekf2.delta_gyro.q, ekf2.delta_gyro.r};
1607 imu_sample.delta_vel_dt = ekf2.accel_dt * 1.e-6f;
1608 imu_sample.delta_vel = Vector3f{ekf2.delta_accel.x, ekf2.delta_accel.y, ekf2.delta_accel.z};
1609 if (!ekf2_input_enabled()) {
1610 return;
1611 }
1612 {
1613 int32_t ints[2] = {(int32_t)ekf2.gyro_dt, (int32_t)ekf2.accel_dt};
1614 float values[6] = {imu_sample.delta_ang(0), imu_sample.delta_ang(1), imu_sample.delta_ang(2),
1615 imu_sample.delta_vel(0), imu_sample.delta_vel(1), imu_sample.delta_vel(2)};
1616 ekf2_log(EKF2_IN_IMU, imu_sample.time_us, 2, ints, 6, values);
1617 }
1618 ins_ekf2_at_rest_update(imu_sample, ekf.getGyroBias());
1619 ekf.setIMUData(imu_sample);
1620
1621 if (ekf.attitude_valid()) {
1622 // Calculate the quaternion
1623 struct FloatQuat ltp_to_body_quat;
1624 const Quatf att_q{ekf.getQuaternion()};
1625 ltp_to_body_quat.qi = att_q(0);
1626 ltp_to_body_quat.qx = att_q(1);
1627 ltp_to_body_quat.qy = att_q(2);
1628 ltp_to_body_quat.qz = att_q(3);
1629
1630 // Publish it to the state
1631 stateSetNedToBodyQuat_f(MODULE_INS_EKF2_ID, &ltp_to_body_quat);
1632
1633 /* Check the quaternion reset state */
1634 float delta_q_reset[4];
1635 uint8_t quat_reset_counter;
1636 ekf.get_quat_reset(delta_q_reset, &quat_reset_counter);
1637
1638#ifndef NO_RESET_UPDATE_SETPOINT_HEADING
1639 // FIXME is this hard reset of control setpoint really needed ? is it the right place ?
1640 if (ekf2.quat_reset_counter < quat_reset_counter) {
1641 float psi = matrix::Eulerf(matrix::Quatf(delta_q_reset)).psi();
1642 guidance_h.sp.heading += psi;
1643 guidance_h.rc_sp.heading += psi;
1644 nav.heading += psi;
1645 //guidance_h_read_rc(autopilot_in_flight());
1647 ekf2.quat_reset_counter = quat_reset_counter;
1648 }
1649#endif
1650
1651 /* Get in-run gyro bias */
1652 struct FloatRates body_rates;
1653 Vector3f gyro_bias{ekf.getGyroBias()};
1654 body_rates.p = (ekf2.delta_gyro.p / (ekf2.gyro_dt * 1.e-6f)) - gyro_bias(0);
1655 body_rates.q = (ekf2.delta_gyro.q / (ekf2.gyro_dt * 1.e-6f)) - gyro_bias(1);
1656 body_rates.r = (ekf2.delta_gyro.r / (ekf2.gyro_dt * 1.e-6f)) - gyro_bias(2);
1657
1658 // Publish it to the state
1659 stateSetBodyRates_f(MODULE_INS_EKF2_ID, &body_rates);
1660
1661 /* Get the in-run acceleration bias */
1662 struct Int32Vect3 accel;
1663 Vector3f accel_bias{ekf.getAccelBias()};
1664 accel.x = ACCEL_BFP_OF_REAL((ekf2.delta_accel.x / (ekf2.accel_dt * 1e-6f)) - accel_bias(0));
1665 accel.y = ACCEL_BFP_OF_REAL((ekf2.delta_accel.y / (ekf2.accel_dt * 1e-6f)) - accel_bias(1));
1666 accel.z = ACCEL_BFP_OF_REAL((ekf2.delta_accel.z / (ekf2.accel_dt * 1e-6f)) - accel_bias(2));
1667
1668 // Publish it to the state
1669 stateSetAccelBody_i(MODULE_INS_EKF2_ID, &accel);
1670 }
1671
1672 ekf2.gyro_valid = false;
1673 ekf2.accel_valid = false;
1674 ekf2.got_imu_data = true;
1675}
1676
1677/* Update INS based on Baro information */
1678static void baro_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp, float pressure)
1679{
1680 if (!ekf2_input_enabled()) {
1681 return;
1682 }
1683 // Calculate the air density
1684 float rho = pprz_isa_density_of_pressure(pressure, ekf2.temp);
1685 ekf.set_air_density(rho);
1686 ekf2.air_density = rho;
1687
1688 // Calculate the height above mean sea level based on pressure
1689 baroSample sample{};
1690 sample.time_us = ekf2_time_us(stamp);
1691 sample.hgt = pprz_isa_height_of_pressure_full(pressure, PPRZ_ISA_SEA_LEVEL_PRESSURE); // the EKF estimates the baro offset
1692 {
1693 float values[2] = {sample.hgt, rho};
1694 ekf2_log(EKF2_IN_BARO, sample.time_us, 0, NULL, 2, values);
1695 }
1696 ekf.setBaroData(sample);
1697}
1698
1699/* Save the latest temperature measurement for air density calculations */
1700static void temperature_cb(uint8_t __attribute__((unused)) sender_id, float temp)
1701{
1702 ekf2.temp = temp;
1703}
1704
1705/* Update INS based on AGL information */
1706static void agl_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp, float distance)
1707{
1708 if (!ekf2_input_enabled()) {
1709 return;
1710 }
1711 estimator::sensor::rangeSample sample{};
1712 sample.time_us = ekf2_time_us(stamp);
1713 sample.rng = distance;
1714 sample.quality = -1; // unknown signal quality
1715
1716 {
1717 int32_t quality = sample.quality;
1718 ekf2_log(EKF2_IN_RANGE, sample.time_us, 1, &quality, 1, &sample.rng);
1719 }
1720 ekf.setRangeData(sample);
1721}
1722
1723/* Update INS based on Gyro information */
1724static void gyro_int_cb(uint8_t __attribute__((unused)) sender_id,
1725 uint32_t stamp, struct FloatRates *delta_gyro, uint16_t dt)
1726{
1727 // Copy and save the gyro data
1728 RATES_COPY(ekf2.delta_gyro, *delta_gyro);
1729 ekf2.gyro_dt = dt;
1730 ekf2.gyro_valid = true;
1731
1732 /* When Gyro and accelerometer are valid enter it into the EKF */
1733 if (ekf2.gyro_valid && ekf2.accel_valid) {
1735 }
1736}
1737
1738/* Update INS based on Accelerometer information */
1739static void accel_int_cb(uint8_t sender_id __attribute__((unused)),
1740 uint32_t stamp, struct FloatVect3 *delta_accel, uint16_t dt)
1741{
1742 // Copy and save the gyro data
1743 VECT3_COPY(ekf2.delta_accel, *delta_accel);
1744 ekf2.accel_dt = dt;
1745 ekf2.accel_valid = true;
1746
1747 /* When Gyro and accelerometer are valid enter it into the EKF */
1748 if (ekf2.gyro_valid && ekf2.accel_valid) {
1750 }
1751}
1752
1753/* Update INS based on Magnetometer information */
1754static void mag_cb(uint8_t sender_id,
1755 uint32_t stamp,
1756 struct Int32Vect3 *mag)
1757{
1758 if (!ekf2_input_enabled()) {
1759 return;
1760 }
1761 struct FloatVect3 mag_gauss;
1762
1763 // The calibrated magnetometer is 1 where it was calibrated: scale it to the field strength there
1764 // (IMU_MAG_CALIB strength, written by the calibration tool), else to the field strength of the
1765 // world magnetic model at the flight plan origin. The scale is fixed (a change while the EKF runs
1766 // is learned as a magnetometer bias and gives a heading error)
1767 struct imu_mag_t *imu_mag = imu_get_mag(sender_id, false);
1768 const float scale = (imu_mag != NULL && imu_mag->strength > 0.1f && imu_mag->strength < 1.f) ? imu_mag->strength : ekf2.mag_scale;
1769 MAGS_FLOAT_OF_BFP(mag_gauss, *mag);
1770 mag_gauss.x *= scale;
1771 mag_gauss.y *= scale;
1772 mag_gauss.z *= scale;
1773
1774 // Publish information to the EKF
1775 magSample sample{};
1776 sample.time_us = ekf2_time_us(stamp);
1777 sample.mag = Vector3f{mag_gauss.x, mag_gauss.y, mag_gauss.z};
1778 {
1779 float values[3] = {mag_gauss.x, mag_gauss.y, mag_gauss.z};
1780 ekf2_log(EKF2_IN_MAG, sample.time_us, 0, NULL, 3, values);
1781 }
1782
1783 ekf.setMagData(sample);
1784 ekf2.got_imu_data = true;
1785}
1786
1787/* Update INS based on GPS information */
1788static void gps_cb(uint8_t sender_id __attribute__((unused)),
1789 uint32_t stamp,
1790 struct GpsState *gps_s)
1791{
1792 if (!ekf2_input_enabled()) {
1793 return;
1794 }
1795 // The velocity is converted from ECEF when the receiver gives no NED velocity. Receivers
1796 // without any velocity (e.g. Furuno, NMEA, MTK) are used for the position only, as before.
1797 bool has_vel = bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT) ||
1798 bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_ECEF_BIT);
1799 int32_t gps_ctrl = has_vel ? (int32_t)(INS_EKF2_GPS_CTRL) : ((INS_EKF2_GPS_CTRL) & ~EKF2_GPS_CTRL_VEL);
1800 if (ekf_params->ekf2_gps_ctrl != gps_ctrl) {
1801 ekf_params->ekf2_gps_ctrl = gps_ctrl;
1802 ekf2_log_param_changed(ekf2_time_us(stamp), "GPS_CTRL");
1803 }
1804
1805 struct LlaCoor_i lla_pos = lla_int_from_gps(gps_s);
1806 struct NedCoor_f ned_vel = ned_vel_float_from_gps(gps_s);
1807
1808 // Run the GNSS checks
1809 gnssChecksSample checks_sample{};
1810 checks_sample.time_us = ekf2_time_us(stamp);
1811 checks_sample.lat = lla_pos.lat * 1e-7;
1812 checks_sample.lon = lla_pos.lon * 1e-7;
1813 checks_sample.alt = gps_s->hmsl * 1e-3f; // EKF2 works with HMSL
1814 checks_sample.vel = Vector3f{ned_vel.x, ned_vel.y, ned_vel.z};
1815 checks_sample.hacc = gps_s->hacc / 100.0f;
1816 checks_sample.vacc = gps_s->vacc / 100.0f;
1817 checks_sample.sacc = gps_s->sacc / 100.0f;
1818 checks_sample.fix_type = gps_s->fix;
1819 checks_sample.nsats = gps_s->num_sv;
1820 checks_sample.pdop = gps_s->pdop / 100.0f;
1821 const bool at_rest = ekf.control_status_flags().vehicle_at_rest;
1822 gnss_checks.run(checks_sample, ekf2.armed, ekf2.in_air, at_rest);
1823
1824 // The measurement was taken INS_EKF2_GPS_DELAY before it arrived
1825 gnssSample sample{};
1826 sample.time_us = ekf2_delayed_time_us(stamp, INS_EKF2_GPS_DELAY);
1827 sample.lat = checks_sample.lat;
1828 sample.lon = checks_sample.lon;
1829 sample.alt = checks_sample.alt;
1830 sample.vel = checks_sample.vel;
1831 sample.hacc = checks_sample.hacc;
1832 sample.vacc = checks_sample.vacc;
1833 sample.sacc = checks_sample.sacc;
1834 sample.fix_type = checks_sample.fix_type;
1835 sample.nsats = checks_sample.nsats;
1836 sample.pdop = checks_sample.pdop;
1837 sample.pos_body = Vector3f{INS_EKF2_GPS_POS_X, INS_EKF2_GPS_POS_Y, INS_EKF2_GPS_POS_Z};
1838 sample.usable = gnss_checks.passed();
1839 sample.sacc_passes_strict = !(gnss_checks.getStrictFailFlags() & vehicle_gnss_s::CHECK_SACC);
1840 {
1841 // Flags: the sample (bits 0-3), and to find why a sample isn't usable, the state of the GNSS checks (bits 4-5),
1842 // their inputs (bits 6-8), and the checks the sample failed at the strict and relaxed thresholds (GNSS_CHECK_*)
1843 int32_t flags = (sample.usable << 0) | (sample.sacc_passes_strict << 1) | (sample.spoofed << 2) | (sample.jammed << 3)
1844 | (gnss_checks.strict() << 4) | (gnss_checks.passedStrict() << 5)
1845 | (at_rest << 6) | (ekf2.armed << 7) | (ekf2.in_air << 8);
1846 int32_t ints[7] = {lla_pos.lat, lla_pos.lon, sample.fix_type, sample.nsats, flags,
1847 gnss_checks.getStrictFailFlags(), gnss_checks.getRelaxedFailFlags()};
1848 float values[11] = {sample.alt, sample.vel(0), sample.vel(1), sample.vel(2), sample.hacc, sample.vacc, sample.sacc,
1849 sample.pdop, sample.pos_body(0), sample.pos_body(1), sample.pos_body(2)};
1850 ekf2_log(EKF2_IN_GNSS, sample.time_us, 7, ints, 11, values);
1851 }
1852 ekf.setGpsData(sample);
1853
1854}
1855
1856/* Update the heading from the relative position (dual antenna RTK GNSS) */
1857static void relpos_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp, struct RelPosNED *relpos)
1858{
1859 if (!ekf2_input_enabled()) {
1860 return;
1861 }
1862 // Verify if we received a valid heading
1863 if (
1864#ifdef INS_EKF2_RELHEADING_REF_ID
1865 relpos->reference_id != INS_EKF2_RELHEADING_REF_ID ||
1866#endif
1867 !PX4_ISFINITE(relpos->heading)
1868 ) {
1869 return;
1870 }
1871
1872#ifdef INS_EKF2_GPS_YAW_OFFSET
1873 // The heading is measured from antenna 1 to antenna 2, the offset is the yaw of that baseline in the body frame
1874 gnssYawSample yaw_sample{};
1875 yaw_sample.time_us = ekf2_delayed_time_us(stamp, INS_EKF2_GPS_DELAY);
1876 yaw_sample.yaw = wrap_pi(relpos->heading - RadOfDeg(INS_EKF2_GPS_YAW_OFFSET));
1877 yaw_sample.yaw_acc = (PX4_ISFINITE(relpos->heading_acc) && relpos->heading_acc > 0.f) ? relpos->heading_acc : NAN;
1878 yaw_sample.yaw_offset = RadOfDeg(INS_EKF2_GPS_YAW_OFFSET);
1879 yaw_sample.usable = true;
1880 {
1881 int32_t usable = yaw_sample.usable;
1882 float values[3] = {yaw_sample.yaw, yaw_sample.yaw_acc, yaw_sample.yaw_offset};
1883 ekf2_log(EKF2_IN_GNSS_YAW, yaw_sample.time_us, 1, &usable, 3, values);
1884 }
1885 ekf.setGnssYawData(yaw_sample);
1886#else
1887 (void)stamp;
1888#endif
1889}
1890
1891/* Update INS based on Optical Flow information */
1892static void optical_flow_cb(uint8_t sender_id __attribute__((unused)),
1893 uint32_t stamp,
1894 int32_t flow_x,
1895 int32_t flow_y,
1896 int32_t flow_der_x __attribute__((unused)),
1897 int32_t flow_der_y __attribute__((unused)),
1898 float quality,
1899 float size_divergence __attribute__((unused)))
1900{
1901 if (!ekf2_input_enabled()) {
1902 return;
1903 }
1904 // Wait for two measurements in order to know the integration time
1905 if (ekf2.flow_stamp <= 0) {
1906 ekf2.flow_stamp = stamp;
1907 return;
1908 }
1909 uint32_t dt = stamp - ekf2.flow_stamp;
1910 ekf2.flow_stamp = stamp;
1911
1912 /* Flow rates of the image about the X and Y body axes (rad/s), RH rotation is positive
1913 NOTE: pure rotations should result in same flow_x and gyro_roll and same flow_y and gyro_pitch */
1914 flowSample sample{};
1915 sample.time_us = ekf2_delayed_time_us(stamp, 0) - dt / 2; // middle of the integration interval
1916 sample.flow_rate = Vector2f{(float)RadOfDeg(flow_y), -(float)RadOfDeg(flow_x)};
1917 sample.gyro_rate = Vector3f{NAN, NAN, NAN}; // the EKF uses its own gyro measurements
1918 sample.quality = quality; // quality indicator between 0 and 255
1919
1920 {
1921 int32_t quality = sample.quality;
1922 float values[5] = {sample.flow_rate(0), sample.flow_rate(1), sample.gyro_rate(0), sample.gyro_rate(1), sample.gyro_rate(2)};
1923 ekf2_log(EKF2_IN_FLOW, sample.time_us, 1, &quality, 5, values);
1924 }
1925 // Update the optical flow data based on the callback
1926 ekf.setOpticalFlowData(sample);
1927}
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
#define ABI_BROADCAST
Broadcast address.
Definition abi_common.h:80
#define AIRSPEED_WE_QUAD_ID
#define AHRS_COMP_ID_EKF2
Definition ahrs.h:47
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static abi_event mag_ev
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
static abi_event gps_ev
bool autopilot_get_motors_on(void)
get motors status
Definition autopilot.c:303
bool autopilot_in_flight(void)
get in_flight flag
Definition autopilot.c:351
Core autopilot interface common to all firmwares.
#define UNUSED(x)
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
static float last_time
static struct uart_periph * dev
#define Min(x, y)
Definition esc_dshot.c:110
struct NedCoor_f ned_vel_float_from_gps(struct GpsState *gps_s)
Get GPS ned velocity (float) Converted on the fly if not available.
Definition gps.c:558
struct GpsState gps
global GPS state
Definition gps.c:74
struct LlaCoor_i lla_int_from_gps(struct GpsState *gps_s)
Get GPS lla (integer) Converted on the fly if not available.
Definition gps.c:478
int32_t hmsl
height above mean sea level (MSL) in mm
Definition gps.h:97
float heading
Relative heading to the reference station in radians.
Definition gps.h:137
uint32_t sacc
speed accuracy in cm/s
Definition gps.h:106
#define GPS_VALID_VEL_ECEF_BIT
Definition gps.h:51
#define GPS_VALID_VEL_NED_BIT
Definition gps.h:52
float heading_acc
Heading accuracy in radians.
Definition gps.h:141
uint32_t hacc
horizontal accuracy in cm
Definition gps.h:104
uint16_t pdop
position dilution of precision scaled by 100
Definition gps.h:108
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition gps.h:91
uint32_t vacc
vertical accuracy in cm
Definition gps.h:105
#define GpsFixValid()
Definition gps.h:171
uint16_t reference_id
Reference station identification.
Definition gps.h:132
uint8_t num_sv
number of sat in fix
Definition gps.h:109
uint8_t fix
status of fix
Definition gps.h:110
data structure for GPS information
Definition gps.h:90
float q
in rad/s
float p
in rad/s
float r
in rad/s
#define FLOAT_VECT3_ZERO(_v)
void float_quat_comp(struct FloatQuat *a2c, const struct FloatQuat *a2b, const struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
Roation quaternion.
angular rates
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
#define RATES_COPY(_a, _b)
#define VECT3_COPY(_a, _b)
#define QUAT1_BFP_OF_REAL(_qf)
#define ACCEL_BFP_OF_REAL(_af)
#define POS_BFP_OF_REAL(_af)
#define SPEED_BFP_OF_REAL(_af)
Rotation quaternion.
int32_t lat
in degrees*1e7
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t z
Down.
int32_t y
East.
int32_t lon
in degrees*1e7
int32_t x
North.
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
vector in Latitude, Longitude and Altitude
vector in North East Down coordinates
static int32_t wgs84_ellipsoid_to_geoid_i(int32_t lat, int32_t lon)
Get WGS84 ellipsoid/geoid separation.
static float pprz_isa_density_of_pressure(float pressure, float temp)
Get the air density (rho) from a given pressure and temperature.
Definition pprz_isa.h:193
#define PPRZ_ISA_AIR_DENSITY
standard air density in kg/m^3
Definition pprz_isa.h:64
#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 void stateSetAccelNed_f(uint16_t id, struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition state.h:1166
static void stateSetAccelBody_i(uint16_t id, struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition state.h:1186
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition state.h:1295
static void stateSetNedToBodyQuat_f(uint16_t id, struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition state.h:1272
static bool stateIsFixedWingFlight(void)
Flying as a fixed wing (always for the fixedwing firmware, in forward flight for hybrids).
Definition state.h:1653
static void stateSetPositionNed_f(uint16_t id, struct NedCoor_f *ned_pos)
Set position from local NED coordinates (float).
Definition state.h:729
static void stateSetLocalOrigin_i(uint16_t id, struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition state.h:530
struct LlaCoor_i stateGetLlaOrigin_i(void)
Get the LLA position of the frame origin (int).
Definition state.c:130
static void stateSetBodyRates_f(uint16_t id, struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition state.h:1371
static void stateSetSpeedNed_f(uint16_t id, struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition state.h:958
struct Imu imu
global IMU state
Definition imu.c:465
struct imu_mag_t * imu_get_mag(uint8_t sender_id, bool create)
Find or create the mag in the imu structure.
Definition imu.c:1217
Inertial Measurement Unit interface.
float strength
Earth field strength where it was calibrated (gauss), the scaled values are 1 there; 0 if unknown.
Definition imu.h:114
#define INS_RESET_VERTICAL_REF
Definition ins.h:37
#define INS_RESET_REF
flags for INS reset
Definition ins.h:36
static void reset_cb(uint8_t sender_id, uint8_t flag)
static abi_event reset_ev
abi_event baro_ev
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
#define ekf2_log_check_start()
Definition ins_ekf2.cpp:555
static void reset_vertical_ref(void)
#define EKF2_GPS_CTRL_VEL
3D velocity
Definition ins_ekf2.cpp:112
#define ekf2_log(...)
Definition ins_ekf2.cpp:552
#define INS_EKF2_GNSS_REQ_EPH
Indoor configuration with only external vision (e.g.
Definition ins_ekf2.cpp:166
static void relpos_cb(uint8_t sender_id, uint32_t stamp, struct RelPosNED *relpos)
static GnssChecks gnss_checks
GNSS checks before the GNSS data is used.
Definition ins_ekf2.cpp:378
static void ins_ekf2_at_rest_update(const imuSample &imu, const Vector3f &gyro_bias)
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
#define INS_EKF2_RNG_MAX_DIST
Maximum distance the range finder can measure (m).
Definition ins_ekf2.cpp:248
static abi_event airspeed_ev
Definition ins_ekf2.cpp:356
#define INS_EKF2_EVENT_HOLDOFF
Minimum time between two messages about the same status, so a flickering status can't flood the link ...
Definition ins_ekf2.cpp:827
void ins_ekf2_update(void)
#define INS_EKF2_AT_REST_ACCEL
accelerometer threshold (m/s2), noise at rest about 0.04
Definition ins_ekf2.cpp:194
#define INS_EKF2_GPS_ID
Definition ins_ekf2.cpp:304
static void reset_cb(uint8_t sender_id, uint8_t flag)
static void ins_ekf2_airspeed_update(void)
Give the last airspeed to the EKF, in the thread of the EKF.
static void send_ins(struct transport_tx *trans, struct link_device *dev)
Definition ins_ekf2.cpp:563
#define ekf2_log_output(...)
Definition ins_ekf2.cpp:556
#define INS_EKF2_GPS_POS_Y
Definition ins_ekf2.cpp:327
static void send_ins_ekf2(struct transport_tx *trans, struct link_device *dev)
Definition ins_ekf2.cpp:624
#define INS_EKF2_GPS_DELAY
GNSS measurement delay (ms), as the PX4 SENS_GNSS0_DELAY parameter.
Definition ins_ekf2.cpp:236
static void send_ins_ekf2_ext(struct transport_tx *trans, struct link_device *dev)
Definition ins_ekf2.cpp:662
static abi_event temperature_ev
Definition ins_ekf2.cpp:348
static void optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x, int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence)
#define INS_EKF2_TEMPERATURE_ID
default temperature sensor to use in INS
Definition ins_ekf2.cpp:274
static void reset_ref(void)
#define INS_EKF2_RELPOS_ID
Definition ins_ekf2.cpp:310
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
static void accel_int_cb(uint8_t sender_id, uint32_t stamp, struct FloatVect3 *delta_accel, uint16_t dt)
#define INS_EKF2_AIRSPEED_ID
default airspeed sensor to use in INS (equivalent airspeed, only fused above INS_EKF2_ARSP_THR): any ...
Definition ins_ekf2.cpp:262
#define INS_EKF2_GPS_POS_Z
Definition ins_ekf2.cpp:332
static abi_event gyro_int_ev
Definition ins_ekf2.cpp:350
static abi_event relpos_ev
Definition ins_ekf2.cpp:354
bool ins_ekf2_events_all
Definition ins_ekf2.cpp:823
static struct @251215145125312176322261332011232027323240311102 ekf2_rest
At rest detection from the IMU samples given to the EKF.
#define INS_EKF2_ACCEL_ID
Definition ins_ekf2.cpp:292
#define INS_EKF2_GYRO_ID
Definition ins_ekf2.cpp:286
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
Definition ins_ekf2.cpp:608
static void temperature_cb(uint8_t sender_id, float temp)
static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance)
#define INS_EKF2_GPS_YAW_NOISE
Definition ins_ekf2.cpp:338
#define INS_EKF2_GNSS_REQ_SACC
Definition ins_ekf2.cpp:169
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
Definition ins_ekf2.cpp:589
static parameters * ekf_params
The EKF parameters.
Definition ins_ekf2.cpp:377
static void ins_ekf2_update_system_flags(void)
Give the arming, in-air and at rest state to the EKF when it changes, and every second (the EKF ignor...
void ins_ekf2_init(void)
static bool ekf2_input_enabled(void)
Definition ins_ekf2.cpp:557
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
Definition ins_ekf2.cpp:707
#define ins_ekf2_check_events()
#define ekf2_log_param_changed(...)
Definition ins_ekf2.cpp:553
static float test_ratio(float ratio)
Innovation test ratio, 0 when not available.
Definition ins_ekf2.cpp:619
static struct @056174147002310323301350044105370077147041341005 ekf2_airspeed
Airspeed (equivalent airspeed) to the EKF as true airspeed.
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
static uint64_t ekf2_time_us(uint32_t stamp)
Convert a 32 bit timestamp (us) of an ABI message to the 64 bit time of the EKF, so the EKF time keep...
Definition ins_ekf2.cpp:388
static void ins_ekf2_publish_attitude(uint64_t time_us)
Publish the attitude and get the new state Directly called after a succeslfull gyro+accel reading.
#define ekf2_log_origin_set(...)
Definition ins_ekf2.cpp:554
static void send_wind_info_ret(struct transport_tx *trans, struct link_device *dev)
Definition ins_ekf2.cpp:732
#define INS_EKF2_GNSS_CHECK
GNSS checks used, PX4 enables all except jamming (2047).
Definition ins_ekf2.cpp:105
static Ekf ekf
EKF class itself.
Definition ins_ekf2.cpp:376
static uint64_t ekf2_delayed_time_us(uint32_t stamp, uint32_t delay_ms)
Time of a measurement that was taken delay_ms before it arrived.
Definition ins_ekf2.cpp:407
#define INS_EKF2_AT_REST_TIME
time below the thresholds before at rest (s)
Definition ins_ekf2.cpp:200
struct ekf2_t ekf2
Local EKF2 status structure.
Definition ins_ekf2.cpp:379
void ins_ekf2_parse_EXTERNAL_POSE(uint8_t *buf)
#define INS_EKF2_AT_REST_RATE
average rotation rate threshold (deg/s), above the gyro bias before it is estimated
Definition ins_ekf2.cpp:197
void ins_ekf2_parse_EXTERNAL_POSE_SMALL(uint8_t *buf)
#define INS_EKF2_MAG_ID
Definition ins_ekf2.cpp:298
static void send_ahrs_quat(struct transport_tx *trans, struct link_device *dev)
Definition ins_ekf2.cpp:754
#define INS_EKF2_HGT_REF
Barometer as height reference (PX4 uses GNSS), as the previous version: the heights don't depend on t...
Definition ins_ekf2.cpp:175
#define INS_EKF2_AGL_ID
default AGL sensor to use in INS
Definition ins_ekf2.cpp:280
#define INS_EKF2_OF_ID
Definition ins_ekf2.cpp:316
static void gyro_int_cb(uint8_t sender_id, uint32_t stamp, struct FloatRates *delta_gyro, uint16_t dt)
ekf2_input_type
Definition ins_ekf2.cpp:420
@ EKF2_IN_FLOW
Definition ins_ekf2.cpp:422
@ EKF2_IN_RANGE
Definition ins_ekf2.cpp:422
@ EKF2_IN_PARAMS
Definition ins_ekf2.cpp:423
@ EKF2_IN_GNSS_YAW
Definition ins_ekf2.cpp:421
@ EKF2_IN_UPDATE
Definition ins_ekf2.cpp:421
@ EKF2_IN_GNSS
Definition ins_ekf2.cpp:421
@ EKF2_IN_ORIGIN
Definition ins_ekf2.cpp:422
@ EKF2_IN_MAG
Definition ins_ekf2.cpp:421
@ EKF2_IN_IMU
Definition ins_ekf2.cpp:421
@ EKF2_IN_FLAGS
Definition ins_ekf2.cpp:422
@ EKF2_IN_OUTPUT
Definition ins_ekf2.cpp:423
@ EKF2_IN_BARO
Definition ins_ekf2.cpp:421
@ EKF2_IN_AIRSPEED
Definition ins_ekf2.cpp:422
@ EKF2_IN_EV
Definition ins_ekf2.cpp:422
@ EKF2_IN_CONFIG
Definition ins_ekf2.cpp:423
#define INS_EKF2_AT_REST_GYRO
gyro threshold (deg/s), noise at rest about 0.15
Definition ins_ekf2.cpp:191
static abi_event accel_int_ev
Definition ins_ekf2.cpp:351
#define INS_EKF2_EVENTS_ALL
Send all the EKF events to the GCS, not only the important ones (alignment, dead reckoning,...
Definition ins_ekf2.cpp:821
static struct extVisionSample sample_ev
External vision sample.
Definition ins_ekf2.cpp:380
#define INS_EKF2_BARO_ID
default barometer to use in INS
Definition ins_ekf2.cpp:268
static void send_external_pose_down(struct transport_tx *trans, struct link_device *dev)
Definition ins_ekf2.cpp:779
#define INS_EKF2_GPS_POS_X
Definition ins_ekf2.cpp:322
static void airspeed_cb(uint8_t sender_id, float eas)
#define INS_EKF2_RNG_MIN_DIST
Minimum distance the range finder can measure (m).
Definition ins_ekf2.cpp:242
#define INS_EKF2_AT_REST_DETECTION
At rest detection, as the PX4 land detector (at rest when landed and not moving for 1 s): the EKF the...
Definition ins_ekf2.cpp:188
static void send_ahrs_bias(struct transport_tx *trans, struct link_device *dev)
Definition ins_ekf2.cpp:744
#define ins_ekf2_preflight_filter()
#define INS_EKF2_OF_MAX_RATE
Maximum flow rate the optical flow sensor can measure (rad/s).
Definition ins_ekf2.cpp:254
INS based in the EKF2 of PX4.
float parameters[22]
Definition ins_flow.c:249
uint16_t foo
Definition main_demo5.c:58
void waypoints_localize_all(void)
update local ENU coordinates of global waypoints
Definition waypoints.c:376
bool init
Definition nav_gls.c:57
bool nps_bypass_ins
void sim_overwrite_ins(void)
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
static abi_event optical_flow_ev
static abi_event agl_ev
The altitude ABI event.
float z
in meters
float x
in meters
float y
in meters
vector in North East Down coordinates Units: meters
WGS-84 Geoid Heights.
Paparazzi atmospheric pressure conversion utilities.
struct pprzlog_transport pprzlog_tp
PPRZLOG transport structure.
Definition pprzlog_tp.c:29
Initialize pprzlog transport.
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.
struct HorizontalGuidance guidance_h
Definition guidance_h.c:45
struct RotorcraftNavigation nav
Definition navigation.c:51
void stabilization_attitude_enter(void)
Attitude control enter function.
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
float alpha
Definition textons.c:133
static const float pos_z[]
static const float scale[]
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned long long uint64_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.