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_MAX_REL_LENGTH_ERROR   0.2
	Maximum allowed error in distance between dual GPS antennae. 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)
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)
void	ins_reset_local_origin (void)
	INS local origin reset. More...
void	ins_reset_altitude_ref (void)
	INS altitude reference reset. More...
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...
static struct extVisionSample	sample_ev
	External vision sample. 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 133 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 121 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 108 of file ins_ekf2.cpp.

INS_EKF2_BARO_NOISE

#define INS_EKF2_BARO_NOISE   3.5f

Definition at line 277 of file ins_ekf2.cpp.

INS_EKF2_EVA_NOISE

#define INS_EKF2_EVA_NOISE   0.05f

Definition at line 259 of file ins_ekf2.cpp.

INS_EKF2_EVP_NOISE

#define INS_EKF2_EVP_NOISE   0.02f

Definition at line 247 of file ins_ekf2.cpp.

INS_EKF2_EVV_NOISE

#define INS_EKF2_EVV_NOISE   0.1f

Definition at line 253 of file ins_ekf2.cpp.

INS_EKF2_FLOW_INNOV_GATE

#define INS_EKF2_FLOW_INNOV_GATE   4

Definition at line 241 of file ins_ekf2.cpp.

INS_EKF2_FLOW_NOISE

#define INS_EKF2_FLOW_NOISE   0.03

Definition at line 229 of file ins_ekf2.cpp.

INS_EKF2_FLOW_NOISE_QMIN

#define INS_EKF2_FLOW_NOISE_QMIN   0.05

Definition at line 235 of file ins_ekf2.cpp.

INS_EKF2_FLOW_POS_X

#define INS_EKF2_FLOW_POS_X   0

Definition at line 211 of file ins_ekf2.cpp.

INS_EKF2_FLOW_POS_Y

#define INS_EKF2_FLOW_POS_Y   0

Definition at line 217 of file ins_ekf2.cpp.

INS_EKF2_FLOW_POS_Z

#define INS_EKF2_FLOW_POS_Z   0

Definition at line 223 of file ins_ekf2.cpp.

INS_EKF2_FLOW_SENSOR_DELAY

#define INS_EKF2_FLOW_SENSOR_DELAY   15

Definition at line 193 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 69 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 81 of file ins_ekf2.cpp.

INS_EKF2_GPS_ID

#define INS_EKF2_GPS_ID   GPS_MULTI_ID

Definition at line 145 of file ins_ekf2.cpp.

INS_EKF2_GPS_P_NOISE

#define INS_EKF2_GPS_P_NOISE   0.5f

Definition at line 271 of file ins_ekf2.cpp.

INS_EKF2_GPS_POS_X

#define INS_EKF2_GPS_POS_X   0

Definition at line 175 of file ins_ekf2.cpp.

INS_EKF2_GPS_POS_Y

#define INS_EKF2_GPS_POS_Y   0

Definition at line 181 of file ins_ekf2.cpp.

INS_EKF2_GPS_POS_Z

#define INS_EKF2_GPS_POS_Z   0

Definition at line 187 of file ins_ekf2.cpp.

INS_EKF2_GPS_V_NOISE

#define INS_EKF2_GPS_V_NOISE   0.3f

Definition at line 265 of file ins_ekf2.cpp.

INS_EKF2_GYRO_ID

#define INS_EKF2_GYRO_ID   ABI_BROADCAST

Definition at line 127 of file ins_ekf2.cpp.

INS_EKF2_IMU_POS_X

#define INS_EKF2_IMU_POS_X   0

Definition at line 157 of file ins_ekf2.cpp.

INS_EKF2_IMU_POS_Y

#define INS_EKF2_IMU_POS_Y   0

Definition at line 163 of file ins_ekf2.cpp.

INS_EKF2_IMU_POS_Z

#define INS_EKF2_IMU_POS_Z   0

Definition at line 169 of file ins_ekf2.cpp.

INS_EKF2_MAG_ID

#define INS_EKF2_MAG_ID   ABI_BROADCAST

Definition at line 139 of file ins_ekf2.cpp.

INS_EKF2_MAX_FLOW_RATE

#define INS_EKF2_MAX_FLOW_RATE   200

Definition at line 205 of file ins_ekf2.cpp.

INS_EKF2_MAX_REL_LENGTH_ERROR

#define INS_EKF2_MAX_REL_LENGTH_ERROR   0.2

Maximum allowed error in distance between dual GPS antennae.

Definition at line 54 of file ins_ekf2.cpp.

INS_EKF2_MIN_FLOW_QUALITY

#define INS_EKF2_MIN_FLOW_QUALITY   100

Definition at line 199 of file ins_ekf2.cpp.

INS_EKF2_OF_ID

#define INS_EKF2_OF_ID   ABI_BROADCAST

Definition at line 151 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 99 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 93 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 87 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 115 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 75 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 917 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 891 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 870 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

Definition at line 956 of file ins_ekf2.cpp.

References GpsRelposNED::carrSoln, GpsState::course, GpsRelposNED::diffSoln, ekf, GpsState::fix, gps_relposned, GPS_VALID_VEL_NED_BIT, GpsState::gspeed, GpsState::hacc, GpsState::hmsl, INS_EKF2_MAX_REL_LENGTH_ERROR, LlaCoor_i::lat, lla_int_from_gps(), LlaCoor_i::lon, ned_vel_float_from_gps(), GpsState::num_sv, GpsState::pdop, GpsRelposNED::relPosHeading, GpsRelposNED::relPosLength, GpsRelposNED::relPosValid, GpsState::sacc, GpsState::vacc, GpsState::valid_fields, NedCoor_f::x, NedCoor_f::y, and NedCoor_f::z.

Referenced by ins_ekf2_init().

Here is the call graph for this function:

Here is the caller graph for this function:

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 902 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 748 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 525 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(), sample_ev, send_ahrs_bias(), send_ahrs_quat(), send_external_pose_down(), 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 762 of file ins_ekf2.cpp.

References ekf, float_quat_comp(), get_sys_time_usec(), INS_EKF2_EVA_NOISE, INS_EKF2_EVP_NOISE, INS_EKF2_EVV_NOISE, 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 796 of file ins_ekf2.cpp.

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

References ACCEL_BFP_OF_REAL, ekf2_t::accel_dt, ekf2_t::accel_valid, ekf2_t::delta_accel, ekf2_t::delta_gyro, ekf, ekf2, ekf2_t::got_imu_data, guidance_h, ekf2_t::gyro_dt, ekf2_t::gyro_valid, HorizontalGuidanceSetpoint::heading, HorizontalGuidanceRCInput::heading, RotorcraftNavigation::heading, nav, FloatRates::p, FloatRates::q, FloatQuat::qi, ekf2_t::quat_reset_counter, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatRates::r, HorizontalGuidance::rc_sp, HorizontalGuidance::sp, 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_int_cb(), and gyro_int_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 753 of file ins_ekf2.cpp.

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

ins_ekf2_update()

void ins_ekf2_update

(

void

)

Definition at line 671 of file ins_ekf2.cpp.

References LlaCoor_i::alt, autopilot_in_flight(), ekf, ekf2, ekf2_t::got_imu_data, LtpDef_i::hmsl, 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(), wgs84_ellipsoid_to_geoid_i(), NedCoor_f::x, NedCoor_f::y, and NedCoor_f::z.

Here is the call graph for this function:

ins_reset_altitude_ref()

void ins_reset_altitude_ref

(

void

)

INS altitude reference reset.

Reset only vertical reference to the current altitude. Does nothing if not implemented by specific INS algorithm.

Definition at line 651 of file ins_ekf2.cpp.

References LlaCoor_i::alt, ekf, ekf2, gps, GpsFixValid, LtpDef_i::hmsl, GpsState::hmsl, LlaCoor_i::lat, LtpDef_i::lla, lla_int_from_gps(), LlaCoor_i::lon, ekf2_t::ltp_def, ltp_def_from_lla_i(), State::ned_origin_i, state, and stateSetLocalOrigin_i().

Here is the call graph for this function:

ins_reset_local_origin()

void ins_reset_local_origin

(

void

)

INS local origin reset.

Reset horizontal and vertical reference to the current position. Does nothing if not implemented by specific INS algorithm.

INS local origin reset.

Definition at line 637 of file ins_ekf2.cpp.

References ekf, ekf2, gps, GpsFixValid, LtpDef_i::hmsl, GpsState::hmsl, LlaCoor_i::lat, lla_int_from_gps(), LlaCoor_i::lon, ekf2_t::ltp_def, ltp_def_from_lla_i(), and stateSetLocalOrigin_i().

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 932 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 1000 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 457 of file ins_ekf2.cpp.

References dev, and ekf.

Referenced by ins_ekf2_init().

Here is the caller graph for this function:

send_ahrs_quat()

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

static

Definition at line 467 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()

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

static

Definition at line 492 of file ins_ekf2.cpp.

References dev, and sample_ev.

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 420 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 316 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 373 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 406 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 363 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 343 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 445 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 885 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 290 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

agl_ev

abi_event agl_ev

static

Definition at line 288 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

baro_ev

abi_event baro_ev

static

Definition at line 286 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

ekf

Ekf ekf

static

EKF class itself.

Definition at line 309 of file ins_ekf2.cpp.

Referenced by agl_cb(), baro_cb(), gps_cb(), ins_ekf2_init(), ins_ekf2_parse_EXTERNAL_POSE(), ins_ekf2_publish_attitude(), ins_ekf2_update(), ins_reset_altitude_ref(), ins_reset_local_origin(), mag_cb(), optical_flow_cb(), send_ahrs_bias(), send_ahrs_quat(), send_filter_status(), send_ins(), send_ins_ekf2(), send_ins_ekf2_ext(), send_ins_z(), and send_wind_info_ret().

ekf2

struct ekf2_t ekf2

Local EKF2 status structure.

Definition at line 310 of file ins_ekf2.cpp.

Referenced by accel_int_cb(), baro_cb(), gyro_int_cb(), ins_ekf2_change_param(), ins_ekf2_init(), ins_ekf2_publish_attitude(), ins_ekf2_remove_gps(), ins_ekf2_update(), ins_reset_altitude_ref(), ins_reset_local_origin(), mag_cb(), optical_flow_cb(), send_ins_ref(), and temperature_cb().

ekf_params

parameters* ekf_params

static

The EKF parameters.

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

Referenced by ins_ekf2_init().

gyro_int_ev

abi_event gyro_int_ev

static

Definition at line 289 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

mag_ev

abi_event mag_ev

static

Definition at line 291 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

optical_flow_ev

abi_event optical_flow_ev

static

Definition at line 293 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

sample_ev

struct extVisionSample sample_ev

static

External vision sample.

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

Referenced by ins_ekf2_init().