Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
ins_ekf2.cpp File Reference

INS based in the EKF2 of PX4. More...

#include "modules/ins/ins_ekf2.h"
#include "modules/nav/waypoints.h"
#include "modules/core/abi.h"
#include "modules/imu/imu.h"
#include "stabilization/stabilization_attitude.h"
#include "generated/airframe.h"
#include "generated/flight_plan.h"
#include "EKF/ekf.h"
#include "lib/gnss/gnss_checks.hpp"
#include "lib/world_magnetic_model/geo_mag_declination.h"
#include "math/pprz_isa.h"
#include "math/pprz_geodetic_wgs84.h"
#include "mcu_periph/sys_time.h"
#include "autopilot.h"
#include "generated/ekf2_params.h"
#include "modules/datalink/telemetry.h"
Include dependency graph for ins_ekf2.cpp:

Go to the source code of this file.

Macros

#define USE_INS_NAV_INIT   TRUE
 For SITL and NPS we need special includes.
#define GNSS_CHECK_NSATS   (1<<0)
 GNSS checks (INS_EKF2_GNSS_CHECK), bits of the PX4 GNSS_CHECK parameter.
#define GNSS_CHECK_PDOP   (1<<1)
 position dilution of precision (INS_EKF2_GNSS_REQ_PDOP)
#define GNSS_CHECK_EPH   (1<<2)
 horizontal position accuracy (INS_EKF2_GNSS_REQ_EPH)
#define GNSS_CHECK_EPV   (1<<3)
 vertical position accuracy (INS_EKF2_GNSS_REQ_EPV)
#define GNSS_CHECK_SACC   (1<<4)
 speed accuracy (INS_EKF2_GNSS_REQ_SACC)
#define GNSS_CHECK_HDRIFT   (1<<5)
 horizontal position drift at rest on the ground (INS_EKF2_GNSS_REQ_HDRIFT)
#define GNSS_CHECK_VDRIFT   (1<<6)
 vertical position drift at rest on the ground (INS_EKF2_GNSS_REQ_VDRIFT)
#define GNSS_CHECK_HSPEED   (1<<7)
 horizontal speed at rest on the ground (INS_EKF2_GNSS_REQ_HDRIFT)
#define GNSS_CHECK_VSPEED   (1<<8)
 vertical speed at rest on the ground (INS_EKF2_GNSS_REQ_VDRIFT)
#define GNSS_CHECK_SPOOFED   (1<<9)
 receiver reports spoofing
#define GNSS_CHECK_FIX   (1<<10)
 fix type (INS_EKF2_GNSS_REQ_FIX)
#define GNSS_CHECK_JAMMED   (1<<11)
 receiver reports jamming
#define INS_EKF2_GNSS_CHECK   (GNSS_CHECK_NSATS | GNSS_CHECK_EPH | GNSS_CHECK_SACC | GNSS_CHECK_FIX)
 GNSS checks used, PX4 enables all except jamming (2047).
#define EKF2_GPS_CTRL_HPOS   1
 Values of INS_EKF2_GPS_CTRL (bitmask).
#define EKF2_GPS_CTRL_VPOS   2
 vertical position
#define EKF2_GPS_CTRL_VEL   4
 3D velocity
#define EKF2_GPS_CTRL_YAW   8
 dual antenna heading
#define EKF2_EV_CTRL_HPOS   1
 Values of INS_EKF2_EV_CTRL (bitmask).
#define EKF2_EV_CTRL_VPOS   2
 vertical position
#define EKF2_EV_CTRL_VEL   4
 3D velocity
#define EKF2_EV_CTRL_YAW   8
 yaw
#define EKF2_HGT_REF_BARO   0
 Values of INS_EKF2_HGT_REF.
#define EKF2_HGT_REF_GNSS   1
#define EKF2_HGT_REF_RANGE   2
#define EKF2_HGT_REF_EV   3
#define EKF2_MAG_TYPE_AUTO   0
 Values of INS_EKF2_MAG_TYPE.
#define EKF2_MAG_TYPE_HEADING   1
 heading only
#define EKF2_MAG_TYPE_NONE   5
 not used
#define EKF2_MAG_TYPE_INIT   6
 only to initialize the heading
#define INS_EKF2_GNSS_REQ_EPH   5.0f
 Indoor configuration with only external vision (e.g.
#define INS_EKF2_GNSS_REQ_SACC   1.0f
#define INS_EKF2_HGT_REF   EKF2_HGT_REF_BARO
 Barometer as height reference (PX4 uses GNSS), as the previous version: the heights don't depend on the ground altitude of the flight plan.
#define INS_EKF2_AT_REST_DETECTION   TRUE
 At rest detection, as the PX4 land detector (at rest when landed and not moving for 1 s): the EKF then runs the GNSS drift checks on the ground, fuses zero velocity without other velocity aiding (so a drifting GNSS, e.g.
#define INS_EKF2_AT_REST_GYRO   0.3f
 gyro threshold (deg/s), noise at rest about 0.15
#define INS_EKF2_AT_REST_ACCEL   0.1f
 accelerometer threshold (m/s2), noise at rest about 0.04
#define INS_EKF2_AT_REST_RATE   1.f
 average rotation rate threshold (deg/s), above the gyro bias before it is estimated
#define INS_EKF2_AT_REST_TIME   1.f
 time below the thresholds before at rest (s)
#define INS_EKF2_RNG_FOG   1.f
#define INS_EKF2_GPS_DELAY   110
 GNSS measurement delay (ms), as the PX4 SENS_GNSS0_DELAY parameter.
#define INS_EKF2_RNG_MIN_DIST   0.001f
 Minimum distance the range finder can measure (m).
#define INS_EKF2_RNG_MAX_DIST   4.f
 Maximum distance the range finder can measure (m).
#define INS_EKF2_OF_MAX_RATE   200.f
 Maximum flow rate the optical flow sensor can measure (rad/s).
#define INS_EKF2_AIRSPEED_ID   AIR_DATA_SENDER_ID
 default airspeed sensor to use in INS (equivalent airspeed, only fused above INS_EKF2_ARSP_THR): any sensor, or the airspeed computed by air_data from a differential pressure sensor.
#define INS_EKF2_BARO_ID   AIR_DATA_SENDER_ID
 default barometer to use in INS
#define INS_EKF2_TEMPERATURE_ID   AIR_DATA_SENDER_ID
 default temperature sensor to use in INS
#define INS_EKF2_AGL_ID   ABI_BROADCAST
 default AGL sensor to use in INS
#define INS_EKF2_GYRO_ID   ABI_BROADCAST
#define INS_EKF2_ACCEL_ID   ABI_BROADCAST
#define INS_EKF2_MAG_ID   ABI_BROADCAST
#define INS_EKF2_GPS_ID   GPS_MULTI_ID
#define INS_EKF2_RELPOS_ID   ABI_BROADCAST
#define INS_EKF2_OF_ID   ABI_BROADCAST
#define INS_EKF2_GPS_POS_X   0
#define INS_EKF2_GPS_POS_Y   0
#define INS_EKF2_GPS_POS_Z   0
#define INS_EKF2_GPS_YAW_NOISE   0.1f
#define ekf2_log(...)
#define ekf2_log_param_changed(...)
#define ekf2_log_origin_set(...)
#define ekf2_log_check_start()
#define ekf2_log_output(...)
#define INS_EKF2_EVENT_MSGS   TRUE
#define INS_EKF2_EVENTS_ALL   FALSE
 Send all the EKF events to the GCS, not only the important ones (alignment, dead reckoning, faults).
#define INS_EKF2_EVENT_HOLDOFF   5.f
 Minimum time between two messages about the same status, so a flickering status can't flood the link [s].
#define ins_ekf2_check_events()
#define ins_ekf2_preflight_filter()

Enumerations

enum  ekf2_input_type {
  EKF2_IN_IMU , EKF2_IN_UPDATE , EKF2_IN_MAG , EKF2_IN_BARO ,
  EKF2_IN_GNSS , EKF2_IN_GNSS_YAW , EKF2_IN_AIRSPEED , EKF2_IN_RANGE ,
  EKF2_IN_FLOW , EKF2_IN_EV , EKF2_IN_FLAGS , EKF2_IN_ORIGIN ,
  EKF2_IN_PARAMS , EKF2_IN_CONFIG , EKF2_IN_OUTPUT
}

Functions

static void baro_cb (uint8_t sender_id, uint32_t stamp, float pressure)
static void temperature_cb (uint8_t sender_id, float temp)
static void agl_cb (uint8_t sender_id, uint32_t stamp, float distance)
static void gyro_int_cb (uint8_t sender_id, uint32_t stamp, struct FloatRates *delta_gyro, uint16_t dt)
static void accel_int_cb (uint8_t sender_id, uint32_t stamp, struct FloatVect3 *delta_accel, uint16_t dt)
static void mag_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
static void gps_cb (uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static void relpos_cb (uint8_t sender_id, uint32_t stamp, struct RelPosNED *relpos)
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)
static void airspeed_cb (uint8_t sender_id, float eas)
static void reset_cb (uint8_t sender_id, uint8_t flag)
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.
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 keeps increasing when the 32 bit time wraps (after 71 minutes).
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.
static bool ekf2_input_enabled (void)
static void send_ins (struct transport_tx *trans, struct link_device *dev)
static void send_ins_z (struct transport_tx *trans, struct link_device *dev)
static void send_ins_ref (struct transport_tx *trans, struct link_device *dev)
static float test_ratio (float ratio)
 Innovation test ratio, 0 when not available.
static void send_ins_ekf2 (struct transport_tx *trans, struct link_device *dev)
static void send_ins_ekf2_ext (struct transport_tx *trans, struct link_device *dev)
static void send_filter_status (struct transport_tx *trans, struct link_device *dev)
static void send_wind_info_ret (struct transport_tx *trans, struct link_device *dev)
static void send_ahrs_bias (struct transport_tx *trans, struct link_device *dev)
static void send_ahrs_quat (struct transport_tx *trans, struct link_device *dev)
static void send_external_pose_down (struct transport_tx *trans, struct link_device *dev)
void ins_ekf2_init (void)
static void reset_ref (void)
static void reset_vertical_ref (void)
static void ins_ekf2_at_rest_update (const imuSample &imu, const Vector3f &gyro_bias)
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 ignores them until it is initialised by the first IMU data).
static void ins_ekf2_airspeed_update (void)
 Give the last airspeed to the EKF, in the thread of the EKF.
void ins_ekf2_update (void)
void ins_ekf2_parse_EXTERNAL_POSE (uint8_t *buf)
void ins_ekf2_parse_EXTERNAL_POSE_SMALL (uint8_t *buf)

Variables

static abi_event baro_ev
static abi_event temperature_ev
static abi_event agl_ev
static abi_event gyro_int_ev
static abi_event accel_int_ev
static abi_event mag_ev
static abi_event gps_ev
static abi_event relpos_ev
static abi_event optical_flow_ev
static abi_event airspeed_ev
static abi_event reset_ev
static Ekf ekf
 EKF class itself.
static parameters * ekf_params
 The EKF parameters.
static GnssChecks gnss_checks
 GNSS checks before the GNSS data is used.
struct ekf2_t ekf2
 Local EKF2 status structure.
static struct extVisionSample sample_ev
 External vision sample.
bool ins_ekf2_events_all = INS_EKF2_EVENTS_ALL
struct { 
Vector3f gyro_avg
 2 s average of the gyro (rad/s)
Vector3f accel_avg
 2 s average of the accelerometer (m/s2)
float gyro_metric
 Low-passed deviation of the gyro from its average (rad/s).
float accel_metric
 Low-passed deviation of the accelerometer from its average (m/s2).
float still_time
 Time below the thresholds (s).
bool init
volatile bool still
 Below the thresholds for INS_EKF2_AT_REST_TIME.
bool at_rest
 Last at rest flag given to the EKF.
} ekf2_rest
 At rest detection from the IMU samples given to the EKF.
struct { 
float eas
 Equivalent airspeed (m/s).
uint32_t stamp
 Time of the measurement (us).
volatile bool new_data
 Set last by the callback, cleared once given to the EKF.
} ekf2_airspeed
 Airspeed (equivalent airspeed) to the EKF as true airspeed.

Detailed Description

INS based in the EKF2 of PX4.

The EKF2 and the PX4 libraries it needs are copied in sw/ext/px4_ekf2 (see sync_px4_ekf2.py there). The GNSS checks, which PX4 runs in its sensors module, are done here before the GNSS data is given to the EKF.

Definition in file ins_ekf2.cpp.

Macro Definition Documentation

◆ EKF2_EV_CTRL_HPOS

#define EKF2_EV_CTRL_HPOS   1

Values of INS_EKF2_EV_CTRL (bitmask).

horizontal position

Definition at line 116 of file ins_ekf2.cpp.

◆ EKF2_EV_CTRL_VEL

#define EKF2_EV_CTRL_VEL   4

3D velocity

Definition at line 118 of file ins_ekf2.cpp.

◆ EKF2_EV_CTRL_VPOS

#define EKF2_EV_CTRL_VPOS   2

vertical position

Definition at line 117 of file ins_ekf2.cpp.

◆ EKF2_EV_CTRL_YAW

#define EKF2_EV_CTRL_YAW   8

yaw

Definition at line 119 of file ins_ekf2.cpp.

◆ EKF2_GPS_CTRL_HPOS

#define EKF2_GPS_CTRL_HPOS   1

Values of INS_EKF2_GPS_CTRL (bitmask).

horizontal position

Definition at line 110 of file ins_ekf2.cpp.

◆ EKF2_GPS_CTRL_VEL

#define EKF2_GPS_CTRL_VEL   4

3D velocity

Definition at line 112 of file ins_ekf2.cpp.

Referenced by gps_cb().

◆ EKF2_GPS_CTRL_VPOS

#define EKF2_GPS_CTRL_VPOS   2

vertical position

Definition at line 111 of file ins_ekf2.cpp.

◆ EKF2_GPS_CTRL_YAW

#define EKF2_GPS_CTRL_YAW   8

dual antenna heading

Definition at line 113 of file ins_ekf2.cpp.

◆ EKF2_HGT_REF_BARO

#define EKF2_HGT_REF_BARO   0

Values of INS_EKF2_HGT_REF.

Definition at line 122 of file ins_ekf2.cpp.

◆ EKF2_HGT_REF_EV

#define EKF2_HGT_REF_EV   3

Definition at line 125 of file ins_ekf2.cpp.

◆ EKF2_HGT_REF_GNSS

#define EKF2_HGT_REF_GNSS   1

Definition at line 123 of file ins_ekf2.cpp.

◆ EKF2_HGT_REF_RANGE

#define EKF2_HGT_REF_RANGE   2

Definition at line 124 of file ins_ekf2.cpp.

◆ ekf2_log

◆ ekf2_log_check_start

#define ekf2_log_check_start ( )
Value:
{}

Definition at line 555 of file ins_ekf2.cpp.

Referenced by ins_ekf2_update().

◆ ekf2_log_origin_set

#define ekf2_log_origin_set ( ...)
Value:
{}

Definition at line 554 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init(), ins_ekf2_update(), reset_ref(), and reset_vertical_ref().

◆ ekf2_log_output

#define ekf2_log_output ( ...)
Value:
{}

Definition at line 556 of file ins_ekf2.cpp.

Referenced by ins_ekf2_update().

◆ ekf2_log_param_changed

#define ekf2_log_param_changed ( ...)
Value:
{}

Definition at line 553 of file ins_ekf2.cpp.

Referenced by gps_cb().

◆ EKF2_MAG_TYPE_AUTO

#define EKF2_MAG_TYPE_AUTO   0

Values of INS_EKF2_MAG_TYPE.

heading on the ground, 3D in the air

Definition at line 128 of file ins_ekf2.cpp.

◆ EKF2_MAG_TYPE_HEADING

#define EKF2_MAG_TYPE_HEADING   1

heading only

Definition at line 129 of file ins_ekf2.cpp.

◆ EKF2_MAG_TYPE_INIT

#define EKF2_MAG_TYPE_INIT   6

only to initialize the heading

Definition at line 131 of file ins_ekf2.cpp.

◆ EKF2_MAG_TYPE_NONE

#define EKF2_MAG_TYPE_NONE   5

not used

Definition at line 130 of file ins_ekf2.cpp.

◆ GNSS_CHECK_EPH

#define GNSS_CHECK_EPH   (1<<2)

horizontal position accuracy (INS_EKF2_GNSS_REQ_EPH)

Definition at line 92 of file ins_ekf2.cpp.

◆ GNSS_CHECK_EPV

#define GNSS_CHECK_EPV   (1<<3)

vertical position accuracy (INS_EKF2_GNSS_REQ_EPV)

Definition at line 93 of file ins_ekf2.cpp.

◆ GNSS_CHECK_FIX

#define GNSS_CHECK_FIX   (1<<10)

fix type (INS_EKF2_GNSS_REQ_FIX)

Definition at line 100 of file ins_ekf2.cpp.

◆ GNSS_CHECK_HDRIFT

#define GNSS_CHECK_HDRIFT   (1<<5)

horizontal position drift at rest on the ground (INS_EKF2_GNSS_REQ_HDRIFT)

Definition at line 95 of file ins_ekf2.cpp.

◆ GNSS_CHECK_HSPEED

#define GNSS_CHECK_HSPEED   (1<<7)

horizontal speed at rest on the ground (INS_EKF2_GNSS_REQ_HDRIFT)

Definition at line 97 of file ins_ekf2.cpp.

◆ GNSS_CHECK_JAMMED

#define GNSS_CHECK_JAMMED   (1<<11)

receiver reports jamming

Definition at line 101 of file ins_ekf2.cpp.

◆ GNSS_CHECK_NSATS

#define GNSS_CHECK_NSATS   (1<<0)

GNSS checks (INS_EKF2_GNSS_CHECK), bits of the PX4 GNSS_CHECK parameter.

number of satellites (INS_EKF2_GNSS_REQ_NSATS)

Definition at line 90 of file ins_ekf2.cpp.

◆ GNSS_CHECK_PDOP

#define GNSS_CHECK_PDOP   (1<<1)

position dilution of precision (INS_EKF2_GNSS_REQ_PDOP)

Definition at line 91 of file ins_ekf2.cpp.

◆ GNSS_CHECK_SACC

#define GNSS_CHECK_SACC   (1<<4)

speed accuracy (INS_EKF2_GNSS_REQ_SACC)

Definition at line 94 of file ins_ekf2.cpp.

◆ GNSS_CHECK_SPOOFED

#define GNSS_CHECK_SPOOFED   (1<<9)

receiver reports spoofing

Definition at line 99 of file ins_ekf2.cpp.

◆ GNSS_CHECK_VDRIFT

#define GNSS_CHECK_VDRIFT   (1<<6)

vertical position drift at rest on the ground (INS_EKF2_GNSS_REQ_VDRIFT)

Definition at line 96 of file ins_ekf2.cpp.

◆ GNSS_CHECK_VSPEED

#define GNSS_CHECK_VSPEED   (1<<8)

vertical speed at rest on the ground (INS_EKF2_GNSS_REQ_VDRIFT)

Definition at line 98 of file ins_ekf2.cpp.

◆ INS_EKF2_ACCEL_ID

#define INS_EKF2_ACCEL_ID   ABI_BROADCAST

Definition at line 292 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ INS_EKF2_AGL_ID

#define INS_EKF2_AGL_ID   ABI_BROADCAST

default AGL sensor to use in INS

Definition at line 280 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ INS_EKF2_AIRSPEED_ID

#define INS_EKF2_AIRSPEED_ID   AIR_DATA_SENDER_ID

default airspeed sensor to use in INS (equivalent airspeed, only fused above INS_EKF2_ARSP_THR): any sensor, or the airspeed computed by air_data from a differential pressure sensor.

The airspeed estimated from the wind (wind_estimation_quadrotor) is never used.

Definition at line 262 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ INS_EKF2_AT_REST_ACCEL

#define INS_EKF2_AT_REST_ACCEL   0.1f

accelerometer threshold (m/s2), noise at rest about 0.04

Definition at line 194 of file ins_ekf2.cpp.

Referenced by ins_ekf2_at_rest_update().

◆ INS_EKF2_AT_REST_DETECTION

#define INS_EKF2_AT_REST_DETECTION   TRUE

At rest detection, as the PX4 land detector (at rest when landed and not moving for 1 s): the EKF then runs the GNSS drift checks on the ground, fuses zero velocity without other velocity aiding (so a drifting GNSS, e.g.

indoors, doesn't tilt it) and fuses the gyro as its bias. At rest when disarmed, on the ground, the gyro and accelerometer deviation from their 2 s average (low-passed over 0.5 s) stay below the thresholds and the average rotation rate (without the gyro bias) below INS_EKF2_AT_REST_RATE, for INS_EKF2_AT_REST_TIME. Stricter than the PX4 vibration metric, which doesn't see the vehicle being turned by hand (and would then fuse that rotation as gyro bias). Disable it (FALSE) on a moving platform (e.g. a ship or car deck), where the IMU can look still while moving

Definition at line 188 of file ins_ekf2.cpp.

Referenced by ins_ekf2_update_system_flags().

◆ INS_EKF2_AT_REST_GYRO

#define INS_EKF2_AT_REST_GYRO   0.3f

gyro threshold (deg/s), noise at rest about 0.15

Definition at line 191 of file ins_ekf2.cpp.

Referenced by ins_ekf2_at_rest_update().

◆ INS_EKF2_AT_REST_RATE

#define INS_EKF2_AT_REST_RATE   1.f

average rotation rate threshold (deg/s), above the gyro bias before it is estimated

Definition at line 197 of file ins_ekf2.cpp.

Referenced by ins_ekf2_at_rest_update().

◆ INS_EKF2_AT_REST_TIME

#define INS_EKF2_AT_REST_TIME   1.f

time below the thresholds before at rest (s)

Definition at line 200 of file ins_ekf2.cpp.

Referenced by ins_ekf2_at_rest_update().

◆ INS_EKF2_BARO_ID

#define INS_EKF2_BARO_ID   AIR_DATA_SENDER_ID

default barometer to use in INS

Definition at line 268 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ ins_ekf2_check_events

#define ins_ekf2_check_events ( )
Value:
{}

Definition at line 1010 of file ins_ekf2.cpp.

Referenced by ins_ekf2_update().

◆ INS_EKF2_EVENT_HOLDOFF

#define INS_EKF2_EVENT_HOLDOFF   5.f

Minimum time between two messages about the same status, so a flickering status can't flood the link [s].

Definition at line 827 of file ins_ekf2.cpp.

◆ INS_EKF2_EVENT_MSGS

#define INS_EKF2_EVENT_MSGS   TRUE

Definition at line 815 of file ins_ekf2.cpp.

◆ INS_EKF2_EVENTS_ALL

#define INS_EKF2_EVENTS_ALL   FALSE

Send all the EKF events to the GCS, not only the important ones (alignment, dead reckoning, faults).

All events are always written to the flight recorder. Can be changed from the GCS.

Definition at line 821 of file ins_ekf2.cpp.

◆ INS_EKF2_GNSS_CHECK

#define INS_EKF2_GNSS_CHECK   (GNSS_CHECK_NSATS | GNSS_CHECK_EPH | GNSS_CHECK_SACC | GNSS_CHECK_FIX)

GNSS checks used, PX4 enables all except jamming (2047).

Definition at line 105 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ INS_EKF2_GNSS_REQ_EPH

#define INS_EKF2_GNSS_REQ_EPH   5.0f

Indoor configuration with only external vision (e.g.

Optitrack): position, velocity, yaw and height from external vision, no GNSS, magnetometer and barometer, and the external vision noise and delay of the previous EKF2 version. Each value can still be changed in the airframe. GNSS accuracy thresholds of the previous version (PX4: EPH 3 m, speed accuracy 0.5 m/s), reached by the internal receivers of e.g. the Bebop2

Definition at line 166 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ INS_EKF2_GNSS_REQ_SACC

#define INS_EKF2_GNSS_REQ_SACC   1.0f

Definition at line 169 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ INS_EKF2_GPS_DELAY

#define INS_EKF2_GPS_DELAY   110

GNSS measurement delay (ms), as the PX4 SENS_GNSS0_DELAY parameter.

Definition at line 236 of file ins_ekf2.cpp.

Referenced by gps_cb(), ins_ekf2_init(), and relpos_cb().

◆ INS_EKF2_GPS_ID

#define INS_EKF2_GPS_ID   GPS_MULTI_ID

Definition at line 304 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ INS_EKF2_GPS_POS_X

#define INS_EKF2_GPS_POS_X   0

Definition at line 322 of file ins_ekf2.cpp.

Referenced by gps_cb().

◆ INS_EKF2_GPS_POS_Y

#define INS_EKF2_GPS_POS_Y   0

Definition at line 327 of file ins_ekf2.cpp.

Referenced by gps_cb().

◆ INS_EKF2_GPS_POS_Z

#define INS_EKF2_GPS_POS_Z   0

Definition at line 332 of file ins_ekf2.cpp.

Referenced by gps_cb().

◆ INS_EKF2_GPS_YAW_NOISE

#define INS_EKF2_GPS_YAW_NOISE   0.1f

Definition at line 338 of file ins_ekf2.cpp.

◆ INS_EKF2_GYRO_ID

#define INS_EKF2_GYRO_ID   ABI_BROADCAST

Definition at line 286 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ INS_EKF2_HGT_REF

#define INS_EKF2_HGT_REF   EKF2_HGT_REF_BARO

Barometer as height reference (PX4 uses GNSS), as the previous version: the heights don't depend on the ground altitude of the flight plan.

Definition at line 175 of file ins_ekf2.cpp.

◆ INS_EKF2_MAG_ID

#define INS_EKF2_MAG_ID   ABI_BROADCAST

Definition at line 298 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ INS_EKF2_OF_ID

#define INS_EKF2_OF_ID   ABI_BROADCAST

Definition at line 316 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ INS_EKF2_OF_MAX_RATE

#define INS_EKF2_OF_MAX_RATE   200.f

Maximum flow rate the optical flow sensor can measure (rad/s).

Definition at line 254 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ ins_ekf2_preflight_filter

#define ins_ekf2_preflight_filter ( )
Value:
{}

Definition at line 1123 of file ins_ekf2.cpp.

Referenced by ins_ekf2_update().

◆ INS_EKF2_RELPOS_ID

#define INS_EKF2_RELPOS_ID   ABI_BROADCAST

Definition at line 310 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ INS_EKF2_RNG_FOG

#define INS_EKF2_RNG_FOG   1.f

Definition at line 219 of file ins_ekf2.cpp.

◆ INS_EKF2_RNG_MAX_DIST

#define INS_EKF2_RNG_MAX_DIST   4.f

Maximum distance the range finder can measure (m).

Definition at line 248 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ INS_EKF2_RNG_MIN_DIST

#define INS_EKF2_RNG_MIN_DIST   0.001f

Minimum distance the range finder can measure (m).

Definition at line 242 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ INS_EKF2_TEMPERATURE_ID

#define INS_EKF2_TEMPERATURE_ID   AIR_DATA_SENDER_ID

default temperature sensor to use in INS

Definition at line 274 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ USE_INS_NAV_INIT

#define USE_INS_NAV_INIT   TRUE

For SITL and NPS we need special includes.

INS reference from flight plan, true by default

Definition at line 77 of file ins_ekf2.cpp.

Enumeration Type Documentation

◆ ekf2_input_type

Enumerator
EKF2_IN_IMU 
EKF2_IN_UPDATE 
EKF2_IN_MAG 
EKF2_IN_BARO 
EKF2_IN_GNSS 
EKF2_IN_GNSS_YAW 
EKF2_IN_AIRSPEED 
EKF2_IN_RANGE 
EKF2_IN_FLOW 
EKF2_IN_EV 
EKF2_IN_FLAGS 
EKF2_IN_ORIGIN 
EKF2_IN_PARAMS 
EKF2_IN_CONFIG 
EKF2_IN_OUTPUT 

Definition at line 420 of file ins_ekf2.cpp.

Function Documentation

◆ accel_int_cb()

void accel_int_cb ( uint8_t sender_id,
uint32_t stamp,
struct FloatVect3 * delta_accel,
uint16_t dt )
static

Definition at line 1739 of file ins_ekf2.cpp.

References ekf2, ekf2_time_us(), ins_ekf2_publish_attitude(), and VECT3_COPY.

Referenced by ins_ekf2_init().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ agl_cb()

void agl_cb ( uint8_t sender_id,
uint32_t stamp,
float distance )
static

Definition at line 1706 of file ins_ekf2.cpp.

References ekf, EKF2_IN_RANGE, ekf2_input_enabled(), ekf2_log, and ekf2_time_us().

Referenced by agl_dist_init(), ins_ekf2_init(), ins_int_init(), and opticflow_pmw3901_init().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ airspeed_cb()

void airspeed_cb ( uint8_t sender_id,
float eas )
static

Definition at line 1399 of file ins_ekf2.cpp.

References AIRSPEED_WE_QUAD_ID, ekf2_airspeed, and get_sys_time_usec().

Referenced by ins_ekf2_init(), and ins_mekf_wind_wrapper_init().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ baro_cb()

void baro_cb ( uint8_t sender_id,
uint32_t stamp,
float pressure )
static

◆ ekf2_delayed_time_us()

uint64_t ekf2_delayed_time_us ( uint32_t stamp,
uint32_t delay_ms )
static

Time of a measurement that was taken delay_ms before it arrived.

Definition at line 407 of file ins_ekf2.cpp.

References ekf2_time_us().

Referenced by gps_cb(), optical_flow_cb(), and relpos_cb().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ ekf2_input_enabled()

bool ekf2_input_enabled ( void )
inlinestatic

◆ ekf2_time_us()

uint64_t ekf2_time_us ( uint32_t stamp)
static

Convert a 32 bit timestamp (us) of an ABI message to the 64 bit time of the EKF, so the EKF time keeps increasing when the 32 bit time wraps (after 71 minutes).

Only a step back of more than half the range is a wrap: a time read just before a later one stored by another caller is a little older, and taking it for a wrap would put the EKF time 71 minutes ahead.

Definition at line 388 of file ins_ekf2.cpp.

References get_sys_time_usec().

Referenced by accel_int_cb(), agl_cb(), baro_cb(), ekf2_delayed_time_us(), gps_cb(), gyro_int_cb(), ins_ekf2_airspeed_update(), ins_ekf2_init(), ins_ekf2_parse_EXTERNAL_POSE(), ins_ekf2_update(), ins_ekf2_update_system_flags(), mag_cb(), reset_ref(), and reset_vertical_ref().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ gps_cb()

◆ gyro_int_cb()

void gyro_int_cb ( uint8_t sender_id,
uint32_t stamp,
struct FloatRates * delta_gyro,
uint16_t dt )
static

Definition at line 1724 of file ins_ekf2.cpp.

References ekf2, ekf2_time_us(), ins_ekf2_publish_attitude(), and RATES_COPY.

Referenced by ins_ekf2_init().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ ins_ekf2_airspeed_update()

void ins_ekf2_airspeed_update ( void )
static

Give the last airspeed to the EKF, in the thread of the EKF.

Definition at line 1410 of file ins_ekf2.cpp.

References ekf, ekf2, ekf2_airspeed, EKF2_IN_AIRSPEED, ekf2_input_enabled(), ekf2_log, ekf2_time_us(), and PPRZ_ISA_AIR_DENSITY.

Referenced by ins_ekf2_update().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ ins_ekf2_at_rest_update()

void ins_ekf2_at_rest_update ( const imuSample & imu,
const Vector3f & gyro_bias )
static

Definition at line 1319 of file ins_ekf2.cpp.

References alpha, ekf2_rest, imu, INS_EKF2_AT_REST_ACCEL, INS_EKF2_AT_REST_GYRO, INS_EKF2_AT_REST_RATE, and INS_EKF2_AT_REST_TIME.

Referenced by ins_ekf2_publish_attitude().

Here is the caller graph for this function:

◆ ins_ekf2_init()

◆ ins_ekf2_parse_EXTERNAL_POSE()

void ins_ekf2_parse_EXTERNAL_POSE ( uint8_t * buf)

Definition at line 1543 of file ins_ekf2.cpp.

References ekf, EKF2_IN_EV, ekf2_input_enabled(), ekf2_log, ekf2_time_us(), float_quat_comp(), get_sys_time_usec(), FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, and sample_ev.

Here is the call graph for this function:

◆ ins_ekf2_parse_EXTERNAL_POSE_SMALL()

void ins_ekf2_parse_EXTERNAL_POSE_SMALL ( uint8_t * buf)

Definition at line 1593 of file ins_ekf2.cpp.

◆ ins_ekf2_publish_attitude()

void ins_ekf2_publish_attitude ( uint64_t time_us)
static

Publish the attitude and get the new state Directly called after a succeslfull gyro+accel reading.

Definition at line 1601 of file ins_ekf2.cpp.

References ACCEL_BFP_OF_REAL, ekf, ekf2, EKF2_IN_IMU, ekf2_input_enabled(), ekf2_log, guidance_h, ins_ekf2_at_rest_update(), nav, FloatRates::p, FloatRates::q, FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatRates::r, stabilization_attitude_enter(), stateSetAccelBody_i(), stateSetBodyRates_f(), stateSetNedToBodyQuat_f(), Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.

Referenced by accel_int_cb(), and gyro_int_cb().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ ins_ekf2_update()

◆ ins_ekf2_update_system_flags()

void ins_ekf2_update_system_flags ( void )
static

Give the arming, in-air and at rest state to the EKF when it changes, and every second (the EKF ignores them until it is initialised by the first IMU data).

Definition at line 1354 of file ins_ekf2.cpp.

References autopilot_get_motors_on(), autopilot_in_flight(), ekf, ekf2, EKF2_IN_FLAGS, ekf2_log, ekf2_rest, ekf2_time_us(), get_sys_time_usec(), INS_EKF2_AT_REST_DETECTION, and stateIsFixedWingFlight().

Referenced by ins_ekf2_update().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ mag_cb()

void mag_cb ( uint8_t sender_id,
uint32_t stamp,
struct Int32Vect3 * mag )
static

Definition at line 1754 of file ins_ekf2.cpp.

References ekf, ekf2, EKF2_IN_MAG, ekf2_input_enabled(), ekf2_log, ekf2_time_us(), imu_get_mag(), MAGS_FLOAT_OF_BFP, scale, imu_mag_t::strength, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Here is the call graph for this function:

◆ optical_flow_cb()

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 )
static

Definition at line 1892 of file ins_ekf2.cpp.

References ekf, ekf2, ekf2_delayed_time_us(), EKF2_IN_FLOW, ekf2_input_enabled(), and ekf2_log.

Referenced by ins_ekf2_init().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ relpos_cb()

void relpos_cb ( uint8_t sender_id,
uint32_t stamp,
struct RelPosNED * relpos )
static

Definition at line 1857 of file ins_ekf2.cpp.

References ekf, ekf2_delayed_time_us(), EKF2_IN_GNSS_YAW, ekf2_input_enabled(), ekf2_log, RelPosNED::heading, RelPosNED::heading_acc, INS_EKF2_GPS_DELAY, and RelPosNED::reference_id.

Referenced by ins_ekf2_init().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ reset_cb()

void reset_cb ( uint8_t sender_id UNUSED,
uint8_t flag )
static

Definition at line 1292 of file ins_ekf2.cpp.

References INS_RESET_REF, INS_RESET_VERTICAL_REF, reset_ref(), reset_vertical_ref(), and UNUSED.

Here is the call graph for this function:

◆ reset_ref()

void reset_ref ( void )
static

Definition at line 1257 of file ins_ekf2.cpp.

References ekf, ekf2, ekf2_log_origin_set, ekf2_time_us(), get_sys_time_usec(), gps, GpsFixValid, LlaCoor_i::lat, lla_int_from_gps(), LlaCoor_i::lon, ltp_def_from_lla_i(), and stateSetLocalOrigin_i().

Referenced by reset_cb().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ reset_vertical_ref()

void reset_vertical_ref ( void )
static

Definition at line 1272 of file ins_ekf2.cpp.

References LlaCoor_i::alt, ekf, ekf2, ekf2_log_origin_set, ekf2_time_us(), get_sys_time_usec(), gps, GpsFixValid, LlaCoor_i::lat, lla_int_from_gps(), LlaCoor_i::lon, ltp_def_from_lla_i(), stateGetLlaOrigin_i(), and stateSetLocalOrigin_i().

Referenced by reset_cb().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ send_ahrs_bias()

void send_ahrs_bias ( struct transport_tx * trans,
struct link_device * dev )
static

Definition at line 744 of file ins_ekf2.cpp.

References dev, and ekf.

Referenced by ins_ekf2_init(), and ins_ext_pose_init().

Here is the caller graph for this function:

◆ send_ahrs_quat()

void send_ahrs_quat ( struct transport_tx * trans,
struct link_device * dev )
static

Definition at line 754 of file ins_ekf2.cpp.

References dev, ekf, foo, Int32Quat::qi, QUAT1_BFP_OF_REAL, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, and stateGetNedToBodyQuat_i().

Referenced by ins_ekf2_init().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ send_external_pose_down()

void send_external_pose_down ( struct transport_tx * trans,
struct link_device * dev )
static

Definition at line 779 of file ins_ekf2.cpp.

References dev, and sample_ev.

Referenced by ins_ekf2_init(), and ins_ext_pose_init().

Here is the caller graph for this function:

◆ send_filter_status()

void send_filter_status ( struct transport_tx * trans,
struct link_device * dev )
static

Definition at line 707 of file ins_ekf2.cpp.

References AHRS_COMP_ID_EKF2, dev, and ekf.

◆ send_ins()

void send_ins ( struct transport_tx * trans,
struct link_device * dev )
static

◆ send_ins_ekf2()

void send_ins_ekf2 ( struct transport_tx * trans,
struct link_device * dev )
static

Definition at line 624 of file ins_ekf2.cpp.

References dev, ekf, gnss_checks, and test_ratio().

Referenced by ins_ekf2_init().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ send_ins_ekf2_ext()

void send_ins_ekf2_ext ( struct transport_tx * trans,
struct link_device * dev )
static

Definition at line 662 of file ins_ekf2.cpp.

References dev, ekf, and gnss_checks.

Referenced by ins_ekf2_init().

Here is the caller graph for this function:

◆ send_ins_ref()

void send_ins_ref ( struct transport_tx * trans,
struct link_device * dev )
static

Definition at line 608 of file ins_ekf2.cpp.

References dev, and ekf2.

Referenced by ins_ekf2_init(), ins_ext_pose_init(), ins_float_invariant_wrapper_init(), ins_flow_init(), ins_gps_passthrough_init(), ins_int_init(), and ins_vectornav_init().

Here is the caller graph for this function:

◆ send_ins_z()

void send_ins_z ( struct transport_tx * trans,
struct link_device * dev )
static

Definition at line 589 of file ins_ekf2.cpp.

References ACCEL_BFP_OF_REAL, dev, ekf, ekf2, POS_BFP_OF_REAL, pos_z, and SPEED_BFP_OF_REAL.

Referenced by ins_ekf2_init(), ins_ext_pose_init(), ins_gps_passthrough_init(), ins_int_init(), and ins_vectornav_init().

Here is the caller graph for this function:

◆ send_wind_info_ret()

void send_wind_info_ret ( struct transport_tx * trans,
struct link_device * dev )
static

Definition at line 732 of file ins_ekf2.cpp.

References dev, and ekf.

Referenced by ins_ekf2_init().

Here is the caller graph for this function:

◆ temperature_cb()

void temperature_cb ( uint8_t sender_id,
float temp )
static

Definition at line 1700 of file ins_ekf2.cpp.

References ekf2.

Referenced by ins_ekf2_init().

Here is the caller graph for this function:

◆ test_ratio()

float test_ratio ( float ratio)
static

Innovation test ratio, 0 when not available.

Definition at line 619 of file ins_ekf2.cpp.

Referenced by send_ins_ekf2().

Here is the caller graph for this function:

Variable Documentation

◆ accel_int_ev

abi_event accel_int_ev
static

Definition at line 351 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ agl_ev

abi_event agl_ev
static

Definition at line 349 of file ins_ekf2.cpp.

◆ airspeed_ev

abi_event airspeed_ev
static

Definition at line 356 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init(), and ins_mekf_wind_wrapper_init().

◆ baro_ev

abi_event baro_ev
static

Definition at line 347 of file ins_ekf2.cpp.

◆ ekf

◆ ekf2

◆ [struct]

struct { ... } ekf2_airspeed

Airspeed (equivalent airspeed) to the EKF as true airspeed.

The airspeed can come from another thread (e.g. a UAVCAN sensor through air_data, in the CAN receive thread): only keep it here, it is given to the EKF from ins_ekf2_update(), as the other samples.

Referenced by airspeed_cb(), and ins_ekf2_airspeed_update().

◆ [struct]

struct { ... } ekf2_rest

At rest detection from the IMU samples given to the EKF.

Referenced by ins_ekf2_at_rest_update(), and ins_ekf2_update_system_flags().

◆ ekf_params

parameters* ekf_params
static

The EKF parameters.

Definition at line 377 of file ins_ekf2.cpp.

Referenced by ekf_aw_wrapper_init(), ekf_aw_wrapper_periodic(), gps_cb(), and ins_ekf2_init().

◆ gnss_checks

GnssChecks gnss_checks
static

GNSS checks before the GNSS data is used.

Definition at line 378 of file ins_ekf2.cpp.

Referenced by gps_cb(), ins_ekf2_init(), send_ins_ekf2(), and send_ins_ekf2_ext().

◆ gps_ev

abi_event gps_ev
static

Definition at line 353 of file ins_ekf2.cpp.

◆ gyro_int_ev

abi_event gyro_int_ev
static

Definition at line 350 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ ins_ekf2_events_all

bool ins_ekf2_events_all = INS_EKF2_EVENTS_ALL

Definition at line 823 of file ins_ekf2.cpp.

◆ mag_ev

abi_event mag_ev
static

Definition at line 352 of file ins_ekf2.cpp.

◆ optical_flow_ev

abi_event optical_flow_ev
static

Definition at line 355 of file ins_ekf2.cpp.

◆ relpos_ev

abi_event relpos_ev
static

Definition at line 354 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ reset_ev

abi_event reset_ev
static

Definition at line 357 of file ins_ekf2.cpp.

◆ sample_ev

struct extVisionSample sample_ev
static

External vision sample.

Definition at line 380 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init(), ins_ekf2_parse_EXTERNAL_POSE(), and send_external_pose_down().

◆ temperature_ev

abi_event temperature_ev
static

Definition at line 348 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().