ins_ekf2.cpp File Reference

#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 "EKF/ekf.h"
#include "math/pprz_isa.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.

Data Structures
struct	ekf2_t

Macros
#define	INS_EKF2_FUSION_MODE   (MASK_USE_GPS)
	For SITL and NPS we need special includes. 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_AGL_ID   ABI_BROADCAST
	default AGL sensor to use in INS 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_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_FLOW_SENSOR_DELAY   15
#define	INS_EKF2_MIN_FLOW_QUALITY   100
#define	INS_EKF2_MAX_FLOW_RATE   200
#define	INS_EKF2_FLOW_OFFSET_X   0
#define	INS_EKF2_FLOW_OFFSET_Y   0
#define	INS_EKF2_FLOW_OFFSET_Z   0
#define	INS_EKF2_FLOW_NOISE   0.03
#define	INS_EKF2_FLOW_NOISE_QMIN   0.05
#define	INS_EKF2_FLOW_INNOV_GATE   4

Functions
static void	agl_cb (uint8_t sender_id, uint32_t stamp, float distance)
static void	baro_cb (uint8_t sender_id, uint32_t stamp, float pressure)
static void	gyro_cb (uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
static void	accel_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
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	body_to_imu_cb (uint8_t sender_id, struct FloatQuat *q_b2i_f)
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_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)

Variables
static abi_event	agl_ev
static abi_event	baro_ev
static abi_event	gyro_ev
static abi_event	accel_ev
static abi_event	mag_ev
static abi_event	gps_ev
static abi_event	body_to_imu_ev
static abi_event	optical_flow_ev
struct gps_message	gps_msg = {}
struct flow_message	flow_msg = {}
static Ekf	ekf
	EKF class itself. More...
static parameters *	ekf_params
	The EKF parameters. More...
struct ekf2_t	ekf2
	Local EKF2 status structure. More...
static uint8_t	ahrs_ekf2_id = AHRS_COMP_ID_EKF2
	Component ID for EKF. More...
struct ekf2_parameters_t	ekf2_params

Detailed Description

INS based in the EKF2 of PX4

Definition in file ins_ekf2.cpp.

---

Data Structure Documentation

ekf2_t

struct ekf2_t

Definition at line 211 of file ins_ekf2.cpp.

Collaboration diagram for ekf2_t:

Data Fields
FloatVect3	accel
uint32_t	accel_dt
uint32_t	accel_stamp
bool	accel_valid
struct OrientationReps	body_to_imu
uint32_t	flow_dt
float	flow_innov
float	flow_quality
uint32_t	flow_stamp
float	flow_x
float	flow_y
bool	got_imu_data
FloatRates	gyro
uint32_t	gyro_dt
float	gyro_pitch
float	gyro_roll
uint32_t	gyro_stamp
bool	gyro_valid
float	gyro_yaw
struct LtpDef_i	ltp_def
uint64_t	ltp_stamp
float	offset_x
float	offset_y
float	offset_z
uint8_t	quat_reset_counter

Macro Definition Documentation

INS_EKF2_ACCEL_ID

#define INS_EKF2_ACCEL_ID   ABI_BROADCAST

Definition at line 110 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 70 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 97 of file ins_ekf2.cpp.

INS_EKF2_FLOW_INNOV_GATE

#define INS_EKF2_FLOW_INNOV_GATE   4

Definition at line 182 of file ins_ekf2.cpp.

INS_EKF2_FLOW_NOISE

#define INS_EKF2_FLOW_NOISE   0.03

Definition at line 170 of file ins_ekf2.cpp.

INS_EKF2_FLOW_NOISE_QMIN

#define INS_EKF2_FLOW_NOISE_QMIN   0.05

Definition at line 176 of file ins_ekf2.cpp.

INS_EKF2_FLOW_OFFSET_X

#define INS_EKF2_FLOW_OFFSET_X   0

Definition at line 152 of file ins_ekf2.cpp.

INS_EKF2_FLOW_OFFSET_Y

#define INS_EKF2_FLOW_OFFSET_Y   0

Definition at line 158 of file ins_ekf2.cpp.

INS_EKF2_FLOW_OFFSET_Z

#define INS_EKF2_FLOW_OFFSET_Z   0

Definition at line 164 of file ins_ekf2.cpp.

INS_EKF2_FLOW_SENSOR_DELAY

#define INS_EKF2_FLOW_SENSOR_DELAY   15

Definition at line 134 of file ins_ekf2.cpp.

INS_EKF2_FUSION_MODE

#define INS_EKF2_FUSION_MODE   (MASK_USE_GPS)

For SITL and NPS we need special includes.

Prevent setting INS reference from flight plan The EKF2 fusion mode setting

Definition at line 52 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 64 of file ins_ekf2.cpp.

INS_EKF2_GPS_ID

#define INS_EKF2_GPS_ID   GPS_MULTI_ID

Definition at line 122 of file ins_ekf2.cpp.

INS_EKF2_GYRO_ID

#define INS_EKF2_GYRO_ID   ABI_BROADCAST

Definition at line 104 of file ins_ekf2.cpp.

INS_EKF2_MAG_ID

#define INS_EKF2_MAG_ID   ABI_BROADCAST

Definition at line 116 of file ins_ekf2.cpp.

INS_EKF2_MAX_FLOW_RATE

#define INS_EKF2_MAX_FLOW_RATE   200

Definition at line 146 of file ins_ekf2.cpp.

INS_EKF2_MIN_FLOW_QUALITY

#define INS_EKF2_MIN_FLOW_QUALITY   100

Definition at line 140 of file ins_ekf2.cpp.

INS_EKF2_OF_ID

#define INS_EKF2_OF_ID   ABI_BROADCAST

Definition at line 128 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 88 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 82 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 76 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 58 of file ins_ekf2.cpp.

Function Documentation

accel_cb()

static void accel_cb ( uint8_t  sender_id, uint32_t  stamp, struct Int32Vect3 *  accel  )

static

Definition at line 655 of file ins_ekf2.cpp.

References ekf2_t::accel, ekf2_t::accel_dt, ekf2_t::accel_stamp, ekf2_t::accel_valid, ACCELS_FLOAT_OF_BFP, ekf2_t::body_to_imu, ekf2, float_rmat_transp_vmult(), ekf2_t::gyro_valid, ins_ekf2_publish_attitude(), and orientationGetRMat_f().

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 623 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 609 of file ins_ekf2.cpp.

References ekf, f, pprz_isa_density_of_pressure(), and pprz_isa_height_of_pressure_full().

Referenced by ins_ekf2_init().

Here is the call graph for this function:

Here is the caller graph for this function:

body_to_imu_cb()

static void body_to_imu_cb ( uint8_t  sender_id, struct FloatQuat *  q_b2i_f  )

static

Definition at line 739 of file ins_ekf2.cpp.

References ekf2_t::body_to_imu, ekf2, and orientationSetQuat_f().

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

Definition at line 708 of file ins_ekf2.cpp.

References GpsState::course, ekf, GpsState::fix, gps_msg, GPS_VALID_VEL_NED_BIT, GpsState::gspeed, GpsState::hacc, GpsState::hmsl, LlaCoor_i::lat, GpsState::lla_pos, LlaCoor_i::lon, GpsState::ned_vel, GpsState::num_sv, GpsState::sacc, GpsState::vacc, GpsState::valid_fields, NedCoor_i::x, NedCoor_i::y, and NedCoor_i::z.

Referenced by ins_ekf2_init().

Here is the caller graph for this function:

gyro_cb()

static void gyro_cb ( uint8_t  sender_id, uint32_t  stamp, struct Int32Rates *  gyro  )

static

Definition at line 629 of file ins_ekf2.cpp.

References ekf2_t::accel_valid, ekf2_t::body_to_imu, ekf2, float_rmat_transp_ratemult(), ekf2_t::gyro, ekf2_t::gyro_dt, ekf2_t::gyro_stamp, ekf2_t::gyro_valid, ins_ekf2_publish_attitude(), orientationGetRMat_f(), and RATES_FLOAT_OF_BFP.

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

References ekf2_params, ekf_params, and ekf2_parameters_t::mag_fusion_type.

ins_ekf2_init()

void ins_ekf2_init

(

void

)

Definition at line 374 of file ins_ekf2.cpp.

References ABI_BROADCAST, accel_cb(), accel_ev, ekf2_t::accel_stamp, ekf2_t::accel_valid, agl_cb(), agl_ev, baro_cb(), baro_ev, body_to_imu_cb(), body_to_imu_ev, DefaultPeriodic, ekf, ekf2, ekf_params, ekf2_t::flow_stamp, ekf2_t::got_imu_data, gps_cb(), gps_ev, gyro_cb(), gyro_ev, ekf2_t::gyro_stamp, ekf2_t::gyro_valid, INS_EKF2_ACCEL_ID, INS_EKF2_AGL_ID, INS_EKF2_BARO_ID, INS_EKF2_FLOW_INNOV_GATE, INS_EKF2_FLOW_NOISE, INS_EKF2_FLOW_NOISE_QMIN, INS_EKF2_FLOW_OFFSET_X, INS_EKF2_FLOW_OFFSET_Y, INS_EKF2_FLOW_OFFSET_Z, INS_EKF2_FLOW_SENSOR_DELAY, INS_EKF2_FUSION_MODE, INS_EKF2_GPS_CHECK_MASK, INS_EKF2_GPS_ID, INS_EKF2_GYRO_ID, 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_VDIST_SENSOR_TYPE, ekf2_t::ltp_stamp, mag_cb(), mag_ev, ekf2_t::offset_x, ekf2_t::offset_y, ekf2_t::offset_z, optical_flow_cb(), optical_flow_ev, ekf2_t::quat_reset_counter, register_periodic_telemetry(), send_ahrs_bias(), send_filter_status(), send_ins_ekf2(), send_ins_ekf2_ext(), send_ins_ref(), and send_wind_info_ret().

Here is the call graph for this function:

ins_ekf2_publish_attitude()

static void ins_ekf2_publish_attitude ( uint32_t  stamp )

static

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

Definition at line 535 of file ins_ekf2.cpp.

References ekf2_t::accel, ACCEL_BFP_OF_REAL, ekf2_t::accel_dt, ekf2_t::accel_valid, ANGLE_BFP_OF_REAL, autopilot_in_flight(), ekf, ekf2, f, ekf2_t::got_imu_data, guidance_h, guidance_h_read_rc(), ekf2_t::gyro, ekf2_t::gyro_dt, ekf2_t::gyro_valid, HorizontalGuidanceSetpoint::heading, nav_heading, FloatRates::p, FloatEulers::psi, FloatRates::q, FloatQuat::qi, ekf2_t::quat_reset_counter, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatRates::r, HorizontalGuidance::rc_sp, HorizontalGuidance::sp, stab_att_sp_euler, stabilization_attitude_enter(), stateSetAccelBody_i(), stateSetBodyRates_f(), stateSetNedToBodyQuat_f(), FloatVect3::x, Int32Vect3::x, FloatVect3::y, Int32Vect3::y, FloatVect3::z, and Int32Vect3::z.

Referenced by accel_cb(), and gyro_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ins_ekf2_remove_gps()

void ins_ekf2_remove_gps

(

int32_t

mode

)

Definition at line 523 of file ins_ekf2.cpp.

References ekf2_params, ekf_params, ekf2_parameters_t::fusion_mode, INS_EKF2_FUSION_MODE, and mode.

ins_ekf2_update()

void ins_ekf2_update

(

void

)

Definition at line 440 of file ins_ekf2.cpp.

References LlaCoor_i::alt, autopilot_in_flight(), ekf, ekf2, ekf2_t::got_imu_data, LlaCoor_i::lat, LlaCoor_i::lon, ekf2_t::ltp_def, ltp_def_from_lla_i(), ekf2_t::ltp_stamp, nps_bypass_ins, sim_overwrite_ins(), stateSetAccelNed_f(), stateSetLocalOrigin_i(), stateSetPositionNed_f(), stateSetSpeedNed_f(), waypoints_localize_all(), NedCoor_f::x, NedCoor_f::y, and NedCoor_f::z.

Here is the call graph for this function:

mag_cb()

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

static

Definition at line 681 of file ins_ekf2.cpp.

References ekf2_t::body_to_imu, ekf, ekf2, float_rmat_transp_vmult(), ekf2_t::got_imu_data, MAGS_FLOAT_OF_BFP, orientationGetRMat_f(), FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ins_ekf2_init().

Here is the call graph for this function:

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

References ekf, ekf2, ekf2_t::flow_dt, flow_msg, ekf2_t::flow_quality, ekf2_t::flow_stamp, ekf2_t::flow_x, ekf2_t::flow_y, ekf2_t::gyro_pitch, ekf2_t::gyro_roll, and ekf2_t::gyro_yaw.

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

References dev, ekf, and states.

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

References ahrs_ekf2_id, dev, and ekf.

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 274 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 310 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 264 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_wind_info_ret()

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

static

Definition at line 349 of file ins_ekf2.cpp.

References dev, and ekf.

Referenced by ins_ekf2_init().

Here is the caller graph for this function:

Variable Documentation

accel_ev

abi_event accel_ev

static

Definition at line 190 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

agl_ev

abi_event agl_ev

static

Definition at line 187 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

ahrs_ekf2_id

uint8_t ahrs_ekf2_id = AHRS_COMP_ID_EKF2

static

Component ID for EKF.

Definition at line 256 of file ins_ekf2.cpp.

Referenced by send_filter_status().

baro_ev

abi_event baro_ev

static

Definition at line 188 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

body_to_imu_ev

abi_event body_to_imu_ev

static

Definition at line 193 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

ekf

Ekf ekf

static

EKF class itself.

Definition at line 253 of file ins_ekf2.cpp.

Referenced by agl_cb(), baro_cb(), gps_cb(), ins_ekf2_init(), ins_ekf2_publish_attitude(), ins_ekf2_update(), mag_cb(), optical_flow_cb(), send_ahrs_bias(), send_filter_status(), send_ins_ekf2(), send_ins_ekf2_ext(), and send_wind_info_ret().

ekf2

struct ekf2_t ekf2

Local EKF2 status structure.

Definition at line 255 of file ins_ekf2.cpp.

Referenced by accel_cb(), body_to_imu_cb(), gyro_cb(), ins_ekf2_init(), ins_ekf2_publish_attitude(), ins_ekf2_update(), mag_cb(), optical_flow_cb(), and send_ins_ref().

ekf2_params

struct ekf2_parameters_t ekf2_params

Definition at line 259 of file ins_ekf2.cpp.

Referenced by ins_ekf2_change_param(), and ins_ekf2_remove_gps().

ekf_params

parameters* ekf_params

static

The EKF parameters.

Definition at line 254 of file ins_ekf2.cpp.

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

flow_msg

struct flow_message flow_msg = {}

Definition at line 198 of file ins_ekf2.cpp.

Referenced by optical_flow_cb().

gps_ev

abi_event gps_ev

static

Definition at line 192 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

gps_msg

struct gps_message gps_msg = {}

Definition at line 197 of file ins_ekf2.cpp.

Referenced by gps_cb().

gyro_ev

abi_event gyro_ev

static

Definition at line 189 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

mag_ev

abi_event mag_ev

static

Definition at line 191 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

optical_flow_ev

abi_event optical_flow_ev

static

Definition at line 194 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().