Paparazzi UAS  v6.3_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "stabilization/stabilization_attitude.h"
#include "generated/airframe.h"
#include "generated/flight_plan.h"
#include "EKF/ekf.h"
#include "math/pprz_isa.h"
#include "math/pprz_geodetic_wgs84.h"
#include "mcu_periph/sys_time.h"
#include "autopilot.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. More...
 
#define INS_EKF2_FUSION_MODE   (MASK_USE_GPS)
 Special configuration for Optitrack. More...
 
#define INS_EKF2_VDIST_SENSOR_TYPE   VDIST_SENSOR_BARO
 The EKF2 primary vertical distance sensor type. More...
 
#define INS_EKF2_GPS_CHECK_MASK   21
 The EKF2 GPS checks before initialization. More...
 
#define INS_EKF2_SONAR_MIN_RANGE   0.001
 Default AGL sensor minimum range. More...
 
#define INS_EKF2_SONAR_MAX_RANGE   4
 Default AGL sensor maximum range. More...
 
#define INS_EKF2_RANGE_MAIN_AGL   1
 If enabled uses radar sensor as primary AGL source, if possible. More...
 
#define INS_EKF2_BARO_ID   ABI_BROADCAST
 default barometer to use in INS More...
 
#define INS_EKF2_TEMPERATURE_ID   ABI_BROADCAST
 default temperature sensor to use in INS More...
 
#define INS_EKF2_AGL_ID   ABI_BROADCAST
 default AGL sensor to use in INS More...
 
#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_OF_ID   ABI_BROADCAST
 
#define INS_EKF2_IMU_POS_X   0
 
#define INS_EKF2_IMU_POS_Y   0
 
#define INS_EKF2_IMU_POS_Z   0
 
#define INS_EKF2_GPS_POS_X   0
 
#define INS_EKF2_GPS_POS_Y   0
 
#define INS_EKF2_GPS_POS_Z   0
 
#define INS_EKF2_FLOW_SENSOR_DELAY   15
 
#define INS_EKF2_MIN_FLOW_QUALITY   100
 
#define INS_EKF2_MAX_FLOW_RATE   200
 
#define INS_EKF2_FLOW_POS_X   0
 
#define INS_EKF2_FLOW_POS_Y   0
 
#define INS_EKF2_FLOW_POS_Z   0
 
#define INS_EKF2_FLOW_NOISE   0.03
 
#define INS_EKF2_FLOW_NOISE_QMIN   0.05
 
#define INS_EKF2_FLOW_INNOV_GATE   4
 
#define INS_EKF2_EVP_NOISE   0.02f
 
#define INS_EKF2_EVV_NOISE   0.1f
 
#define INS_EKF2_EVA_NOISE   0.05f
 
#define INS_EKF2_GPS_V_NOISE   0.3f
 
#define INS_EKF2_GPS_P_NOISE   0.5f
 
#define INS_EKF2_BARO_NOISE   3.5f
 

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 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 ins_ekf2_publish_attitude (uint32_t stamp)
 Publish the attitude and get the new state Directly called after a succeslfull gyro+accel reading. More...
 
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 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)
 
void ins_ekf2_init (void)
 
void ins_ekf2_update (void)
 
void ins_ekf2_change_param (int32_t unk)
 
void ins_ekf2_remove_gps (int32_t mode)
 
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 optical_flow_ev
 
static Ekf ekf
 EKF class itself. More...
 
static parameters * ekf_params
 The EKF parameters. More...
 
struct ekf2_t ekf2
 Local EKF2 status structure. More...
 

Detailed Description

INS based in the EKF2 of PX4.

Definition in file ins_ekf2.cpp.

Macro Definition Documentation

◆ INS_EKF2_ACCEL_ID

#define INS_EKF2_ACCEL_ID   ABI_BROADCAST

Definition at line 128 of file ins_ekf2.cpp.

◆ INS_EKF2_AGL_ID

#define INS_EKF2_AGL_ID   ABI_BROADCAST

default AGL sensor to use in INS

Definition at line 116 of file ins_ekf2.cpp.

◆ INS_EKF2_BARO_ID

#define INS_EKF2_BARO_ID   ABI_BROADCAST

default barometer to use in INS

Definition at line 103 of file ins_ekf2.cpp.

◆ INS_EKF2_BARO_NOISE

#define INS_EKF2_BARO_NOISE   3.5f

Definition at line 272 of file ins_ekf2.cpp.

◆ INS_EKF2_EVA_NOISE

#define INS_EKF2_EVA_NOISE   0.05f

Definition at line 254 of file ins_ekf2.cpp.

◆ INS_EKF2_EVP_NOISE

#define INS_EKF2_EVP_NOISE   0.02f

Definition at line 242 of file ins_ekf2.cpp.

◆ INS_EKF2_EVV_NOISE

#define INS_EKF2_EVV_NOISE   0.1f

Definition at line 248 of file ins_ekf2.cpp.

◆ INS_EKF2_FLOW_INNOV_GATE

#define INS_EKF2_FLOW_INNOV_GATE   4

Definition at line 236 of file ins_ekf2.cpp.

◆ INS_EKF2_FLOW_NOISE

#define INS_EKF2_FLOW_NOISE   0.03

Definition at line 224 of file ins_ekf2.cpp.

◆ INS_EKF2_FLOW_NOISE_QMIN

#define INS_EKF2_FLOW_NOISE_QMIN   0.05

Definition at line 230 of file ins_ekf2.cpp.

◆ INS_EKF2_FLOW_POS_X

#define INS_EKF2_FLOW_POS_X   0

Definition at line 206 of file ins_ekf2.cpp.

◆ INS_EKF2_FLOW_POS_Y

#define INS_EKF2_FLOW_POS_Y   0

Definition at line 212 of file ins_ekf2.cpp.

◆ INS_EKF2_FLOW_POS_Z

#define INS_EKF2_FLOW_POS_Z   0

Definition at line 218 of file ins_ekf2.cpp.

◆ INS_EKF2_FLOW_SENSOR_DELAY

#define INS_EKF2_FLOW_SENSOR_DELAY   15

Definition at line 188 of file ins_ekf2.cpp.

◆ INS_EKF2_FUSION_MODE

#define INS_EKF2_FUSION_MODE   (MASK_USE_GPS)

Special configuration for Optitrack.

The EKF2 fusion mode setting

Definition at line 64 of file ins_ekf2.cpp.

◆ INS_EKF2_GPS_CHECK_MASK

#define INS_EKF2_GPS_CHECK_MASK   21

The EKF2 GPS checks before initialization.

Definition at line 76 of file ins_ekf2.cpp.

◆ INS_EKF2_GPS_ID

#define INS_EKF2_GPS_ID   GPS_MULTI_ID

Definition at line 140 of file ins_ekf2.cpp.

◆ INS_EKF2_GPS_P_NOISE

#define INS_EKF2_GPS_P_NOISE   0.5f

Definition at line 266 of file ins_ekf2.cpp.

◆ INS_EKF2_GPS_POS_X

#define INS_EKF2_GPS_POS_X   0

Definition at line 170 of file ins_ekf2.cpp.

◆ INS_EKF2_GPS_POS_Y

#define INS_EKF2_GPS_POS_Y   0

Definition at line 176 of file ins_ekf2.cpp.

◆ INS_EKF2_GPS_POS_Z

#define INS_EKF2_GPS_POS_Z   0

Definition at line 182 of file ins_ekf2.cpp.

◆ INS_EKF2_GPS_V_NOISE

#define INS_EKF2_GPS_V_NOISE   0.3f

Definition at line 260 of file ins_ekf2.cpp.

◆ INS_EKF2_GYRO_ID

#define INS_EKF2_GYRO_ID   ABI_BROADCAST

Definition at line 122 of file ins_ekf2.cpp.

◆ INS_EKF2_IMU_POS_X

#define INS_EKF2_IMU_POS_X   0

Definition at line 152 of file ins_ekf2.cpp.

◆ INS_EKF2_IMU_POS_Y

#define INS_EKF2_IMU_POS_Y   0

Definition at line 158 of file ins_ekf2.cpp.

◆ INS_EKF2_IMU_POS_Z

#define INS_EKF2_IMU_POS_Z   0

Definition at line 164 of file ins_ekf2.cpp.

◆ INS_EKF2_MAG_ID

#define INS_EKF2_MAG_ID   ABI_BROADCAST

Definition at line 134 of file ins_ekf2.cpp.

◆ INS_EKF2_MAX_FLOW_RATE

#define INS_EKF2_MAX_FLOW_RATE   200

Definition at line 200 of file ins_ekf2.cpp.

◆ INS_EKF2_MIN_FLOW_QUALITY

#define INS_EKF2_MIN_FLOW_QUALITY   100

Definition at line 194 of file ins_ekf2.cpp.

◆ INS_EKF2_OF_ID

#define INS_EKF2_OF_ID   ABI_BROADCAST

Definition at line 146 of file ins_ekf2.cpp.

◆ INS_EKF2_RANGE_MAIN_AGL

#define INS_EKF2_RANGE_MAIN_AGL   1

If enabled uses radar sensor as primary AGL source, if possible.

Definition at line 94 of file ins_ekf2.cpp.

◆ INS_EKF2_SONAR_MAX_RANGE

#define INS_EKF2_SONAR_MAX_RANGE   4

Default AGL sensor maximum range.

Definition at line 88 of file ins_ekf2.cpp.

◆ INS_EKF2_SONAR_MIN_RANGE

#define INS_EKF2_SONAR_MIN_RANGE   0.001

Default AGL sensor minimum range.

Definition at line 82 of file ins_ekf2.cpp.

◆ INS_EKF2_TEMPERATURE_ID

#define INS_EKF2_TEMPERATURE_ID   ABI_BROADCAST

default temperature sensor to use in INS

Definition at line 110 of file ins_ekf2.cpp.

◆ INS_EKF2_VDIST_SENSOR_TYPE

#define INS_EKF2_VDIST_SENSOR_TYPE   VDIST_SENSOR_BARO

The EKF2 primary vertical distance sensor type.

Definition at line 70 of file ins_ekf2.cpp.

◆ 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 49 of file ins_ekf2.cpp.

Function Documentation

◆ accel_int_cb()

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

Definition at line 809 of file ins_ekf2.cpp.

References ekf2_t::accel_dt, ekf2_t::accel_valid, ekf2_t::delta_accel, ekf2, ekf2_t::gyro_valid, 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()

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

Definition at line 783 of file ins_ekf2.cpp.

References ekf.

Referenced by ins_ekf2_init().

+ Here is the caller graph for this function:

◆ baro_cb()

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

Definition at line 762 of file ins_ekf2.cpp.

References ekf, ekf2, pprz_isa_density_of_pressure(), pprz_isa_height_of_pressure_full(), ekf2_t::qnh, and ekf2_t::temp.

Referenced by ins_ekf2_init().

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

◆ gps_cb()

static void gps_cb ( uint8_t  sender_id,
uint32_t  stamp,
struct GpsState gps_s 
)
static

◆ gyro_int_cb()

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

Definition at line 794 of file ins_ekf2.cpp.

References ekf2_t::accel_valid, ekf2_t::delta_gyro, ekf2, ekf2_t::gyro_dt, ekf2_t::gyro_valid, 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_change_param()

void ins_ekf2_change_param ( int32_t  unk)

Definition at line 646 of file ins_ekf2.cpp.

References ekf2, ekf_params, and ekf2_t::mag_fusion_type.

◆ ins_ekf2_init()

void ins_ekf2_init ( void  )

Definition at line 461 of file ins_ekf2.cpp.

References accel_int_cb(), accel_int_ev, ekf2_t::accel_valid, agl_cb(), agl_ev, LlaCoor_i::alt, baro_cb(), baro_ev, DefaultPeriodic, ekf, ekf2, ekf_params, ekf2_t::flow_stamp, ekf2_t::got_imu_data, gps_cb(), gps_ev, gyro_int_cb(), gyro_int_ev, ekf2_t::gyro_valid, LtpDef_i::hmsl, INS_EKF2_ACCEL_ID, INS_EKF2_AGL_ID, INS_EKF2_BARO_ID, INS_EKF2_BARO_NOISE, INS_EKF2_FLOW_INNOV_GATE, INS_EKF2_FLOW_NOISE, INS_EKF2_FLOW_NOISE_QMIN, INS_EKF2_FLOW_POS_X, INS_EKF2_FLOW_POS_Y, INS_EKF2_FLOW_POS_Z, INS_EKF2_FLOW_SENSOR_DELAY, INS_EKF2_FUSION_MODE, INS_EKF2_GPS_CHECK_MASK, INS_EKF2_GPS_ID, INS_EKF2_GPS_P_NOISE, INS_EKF2_GPS_POS_X, INS_EKF2_GPS_POS_Y, INS_EKF2_GPS_POS_Z, INS_EKF2_GPS_V_NOISE, INS_EKF2_GYRO_ID, INS_EKF2_IMU_POS_X, INS_EKF2_IMU_POS_Y, INS_EKF2_IMU_POS_Z, INS_EKF2_MAG_ID, INS_EKF2_MAX_FLOW_RATE, INS_EKF2_MIN_FLOW_QUALITY, INS_EKF2_OF_ID, INS_EKF2_RANGE_MAIN_AGL, INS_EKF2_SONAR_MAX_RANGE, INS_EKF2_SONAR_MIN_RANGE, INS_EKF2_TEMPERATURE_ID, INS_EKF2_VDIST_SENSOR_TYPE, LlaCoor_i::lat, LlaCoor_i::lon, ekf2_t::ltp_def, ltp_def_from_lla_i(), ekf2_t::ltp_stamp, mag_cb(), mag_ev, optical_flow_cb(), optical_flow_ev, ekf2_t::qnh, ekf2_t::quat_reset_counter, register_periodic_telemetry(), send_ahrs_bias(), send_filter_status(), send_ins(), send_ins_ekf2(), send_ins_ekf2_ext(), send_ins_ref(), send_ins_z(), send_wind_info_ret(), stateSetLocalOrigin_i(), ekf2_t::temp, temperature_cb(), temperature_ev, and waypoints_localize_all().

+ Here is the call graph for this function:

◆ ins_ekf2_parse_EXTERNAL_POSE()

void ins_ekf2_parse_EXTERNAL_POSE ( uint8_t buf)

Definition at line 660 of file ins_ekf2.cpp.

References ekf, get_sys_time_usec(), INS_EKF2_EVA_NOISE, INS_EKF2_EVP_NOISE, and INS_EKF2_EVV_NOISE.

+ 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 683 of file ins_ekf2.cpp.

◆ ins_ekf2_publish_attitude()

◆ ins_ekf2_remove_gps()

void ins_ekf2_remove_gps ( int32_t  mode)

Definition at line 651 of file ins_ekf2.cpp.

References ekf2, ekf_params, ekf2_t::fusion_mode, INS_EKF2_FUSION_MODE, and mode.

◆ ins_ekf2_update()

◆ mag_cb()

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

Definition at line 824 of file ins_ekf2.cpp.

References ekf, ekf2, ekf2_t::got_imu_data, MAGS_FLOAT_OF_BFP, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ins_ekf2_init().

+ Here is the caller graph for this function:

◆ optical_flow_cb()

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

Definition at line 882 of file ins_ekf2.cpp.

References ekf, ekf2, and ekf2_t::flow_stamp.

Referenced by ins_ekf2_init().

+ Here is the caller graph for this function:

◆ send_ahrs_bias()

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

Definition at line 449 of file ins_ekf2.cpp.

References dev, and ekf.

Referenced by ins_ekf2_init().

+ Here is the caller graph for this function:

◆ send_filter_status()

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

Definition at line 412 of file ins_ekf2.cpp.

References AHRS_COMP_ID_EKF2, dev, and ekf.

Referenced by ins_ekf2_init().

+ Here is the caller graph for this function:

◆ send_ins()

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

Definition at line 308 of file ins_ekf2.cpp.

References ACCEL_BFP_OF_REAL, dev, ekf, POS_BFP_OF_REAL, SPEED_BFP_OF_REAL, NedCoor_i::x, NedCoor_i::y, and NedCoor_i::z.

Referenced by ins_ekf2_init().

+ Here is the caller graph for this function:

◆ send_ins_ekf2()

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

Definition at line 365 of file ins_ekf2.cpp.

References dev, and ekf.

Referenced by ins_ekf2_init().

+ Here is the caller graph for this function:

◆ send_ins_ekf2_ext()

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

Definition at line 398 of file ins_ekf2.cpp.

References dev, and ekf.

Referenced by ins_ekf2_init().

+ Here is the caller graph for this function:

◆ send_ins_ref()

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

Definition at line 355 of file ins_ekf2.cpp.

References LlaCoor_i::alt, dev, LtpDef_i::ecef, ekf2, LtpDef_i::hmsl, LlaCoor_i::lat, LtpDef_i::lla, LlaCoor_i::lon, ekf2_t::ltp_def, ekf2_t::ltp_stamp, EcefCoor_i::x, EcefCoor_i::y, and EcefCoor_i::z.

Referenced by ins_ekf2_init().

+ Here is the caller graph for this function:

◆ send_ins_z()

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

Definition at line 335 of file ins_ekf2.cpp.

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

Referenced by ins_ekf2_init().

+ Here is the caller graph for this function:

◆ send_wind_info_ret()

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

Definition at line 437 of file ins_ekf2.cpp.

References dev, and ekf.

Referenced by ins_ekf2_init().

+ Here is the caller graph for this function:

◆ temperature_cb()

static void temperature_cb ( uint8_t  sender_id,
float  temp 
)
static

Definition at line 777 of file ins_ekf2.cpp.

References ekf2, and ekf2_t::temp.

Referenced by ins_ekf2_init().

+ Here is the caller graph for this function:

Variable Documentation

◆ accel_int_ev

abi_event accel_int_ev
static

Definition at line 281 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ agl_ev

abi_event agl_ev
static

Definition at line 279 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ baro_ev

abi_event baro_ev
static

Definition at line 277 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ ekf

◆ ekf2

◆ ekf_params

parameters* ekf_params
static

The EKF parameters.

Definition at line 302 of file ins_ekf2.cpp.

Referenced by ins_ekf2_change_param(), ins_ekf2_init(), and ins_ekf2_remove_gps().

◆ gps_ev

abi_event gps_ev
static

Definition at line 283 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ gyro_int_ev

abi_event gyro_int_ev
static

Definition at line 280 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ mag_ev

abi_event mag_ev
static

Definition at line 282 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ optical_flow_ev

abi_event optical_flow_ev
static

Definition at line 284 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ temperature_ev

abi_event temperature_ev
static

Definition at line 278 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().