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.
#define	INS_EKF2_MAX_REL_LENGTH_ERROR   0.2
	Maximum allowed error in distance between dual GPS antennae.
#define	INS_EKF2_FUSION_MODE   (MASK_USE_GPS)
	Special configuration for Optitrack.
#define	INS_EKF2_MAG_FUSION_TYPE   MAG_FUSE_TYPE_AUTO
	The EKF2 magnetometer fusion type.
#define	INS_EKF2_VDIST_SENSOR_TYPE   VDIST_SENSOR_BARO
	The EKF2 primary vertical distance sensor type.
#define	INS_EKF2_GPS_CHECK_MASK   21
	The EKF2 GPS checks before initialization.
#define	INS_EKF2_SONAR_MIN_RANGE   0.001
	Default AGL sensor minimum range.
#define	INS_EKF2_SONAR_MAX_RANGE   4
	Default AGL sensor maximum range.
#define	INS_EKF2_RANGE_MAIN_AGL   1
	If enabled uses radar sensor as primary AGL source, if possible.
#define	INS_EKF2_BARO_ID   ABI_BROADCAST
	default barometer to use in INS
#define	INS_EKF2_TEMPERATURE_ID   ABI_BROADCAST
	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_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
#define	INS_EKF2_RELHEADING_ERR   0.2

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	reset_cb (uint8_t sender_id, uint8_t flag)
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.
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)
static void	reset_ref (void)
static void	reset_vertical_ref (void)
void	ins_ekf2_update (void)
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	relpos_ev
static abi_event	optical_flow_ev
static abi_event	reset_ev
static Ekf	ekf
	EKF class itself.
static parameters *	ekf_params
	The EKF parameters.
struct ekf2_t	ekf2
	Local EKF2 status structure.
static struct extVisionSample	sample_ev
	External vision sample.

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 142 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 130 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 117 of file ins_ekf2.cpp.

INS_EKF2_BARO_NOISE

#define INS_EKF2_BARO_NOISE   3.5f

Definition at line 292 of file ins_ekf2.cpp.

INS_EKF2_EVA_NOISE

#define INS_EKF2_EVA_NOISE   0.05f

Definition at line 274 of file ins_ekf2.cpp.

INS_EKF2_EVP_NOISE

#define INS_EKF2_EVP_NOISE   0.02f

Definition at line 262 of file ins_ekf2.cpp.

INS_EKF2_EVV_NOISE

#define INS_EKF2_EVV_NOISE   0.1f

Definition at line 268 of file ins_ekf2.cpp.

INS_EKF2_FLOW_INNOV_GATE

#define INS_EKF2_FLOW_INNOV_GATE   4

Definition at line 256 of file ins_ekf2.cpp.

INS_EKF2_FLOW_NOISE

#define INS_EKF2_FLOW_NOISE   0.03

Definition at line 244 of file ins_ekf2.cpp.

INS_EKF2_FLOW_NOISE_QMIN

#define INS_EKF2_FLOW_NOISE_QMIN   0.05

Definition at line 250 of file ins_ekf2.cpp.

INS_EKF2_FLOW_POS_X

#define INS_EKF2_FLOW_POS_X   0

Definition at line 226 of file ins_ekf2.cpp.

INS_EKF2_FLOW_POS_Y

#define INS_EKF2_FLOW_POS_Y   0

Definition at line 232 of file ins_ekf2.cpp.

INS_EKF2_FLOW_POS_Z

#define INS_EKF2_FLOW_POS_Z   0

Definition at line 238 of file ins_ekf2.cpp.

INS_EKF2_FLOW_SENSOR_DELAY

#define INS_EKF2_FLOW_SENSOR_DELAY   15

Definition at line 208 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 72 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 90 of file ins_ekf2.cpp.

INS_EKF2_GPS_ID

#define INS_EKF2_GPS_ID   GPS_MULTI_ID

Definition at line 154 of file ins_ekf2.cpp.

INS_EKF2_GPS_P_NOISE

#define INS_EKF2_GPS_P_NOISE   0.5f

Definition at line 286 of file ins_ekf2.cpp.

INS_EKF2_GPS_POS_X

#define INS_EKF2_GPS_POS_X   0

Definition at line 190 of file ins_ekf2.cpp.

INS_EKF2_GPS_POS_Y

#define INS_EKF2_GPS_POS_Y   0

Definition at line 196 of file ins_ekf2.cpp.

INS_EKF2_GPS_POS_Z

#define INS_EKF2_GPS_POS_Z   0

Definition at line 202 of file ins_ekf2.cpp.

INS_EKF2_GPS_V_NOISE

#define INS_EKF2_GPS_V_NOISE   0.3f

Definition at line 280 of file ins_ekf2.cpp.

INS_EKF2_GYRO_ID

#define INS_EKF2_GYRO_ID   ABI_BROADCAST

Definition at line 136 of file ins_ekf2.cpp.

INS_EKF2_IMU_POS_X

#define INS_EKF2_IMU_POS_X   0

Definition at line 172 of file ins_ekf2.cpp.

INS_EKF2_IMU_POS_Y

#define INS_EKF2_IMU_POS_Y   0

Definition at line 178 of file ins_ekf2.cpp.

INS_EKF2_IMU_POS_Z

#define INS_EKF2_IMU_POS_Z   0

Definition at line 184 of file ins_ekf2.cpp.

INS_EKF2_MAG_FUSION_TYPE

#define INS_EKF2_MAG_FUSION_TYPE   MAG_FUSE_TYPE_AUTO

The EKF2 magnetometer fusion type.

Definition at line 78 of file ins_ekf2.cpp.

INS_EKF2_MAG_ID

#define INS_EKF2_MAG_ID   ABI_BROADCAST

Definition at line 148 of file ins_ekf2.cpp.

INS_EKF2_MAX_FLOW_RATE

#define INS_EKF2_MAX_FLOW_RATE   200

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

INS_EKF2_OF_ID

#define INS_EKF2_OF_ID   ABI_BROADCAST

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

INS_EKF2_RELHEADING_ERR

#define INS_EKF2_RELHEADING_ERR   0.2

Definition at line 298 of file ins_ekf2.cpp.

INS_EKF2_RELPOS_ID

#define INS_EKF2_RELPOS_ID   ABI_BROADCAST

Definition at line 160 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 102 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 96 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 124 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 84 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 953 of file ins_ekf2.cpp.

References ekf2_t::accel_dt, ekf2_t::accel_valid, ekf2_t::delta_accel, ekf2, foo, 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 927 of file ins_ekf2.cpp.

References ekf, and foo.

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

References ekf, ekf2, foo, 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 992 of file ins_ekf2.cpp.

References ekf, ekf2, foo, GPS_VALID_VEL_NED_BIT, LlaCoor_i::lat, lla_int_from_gps(), LlaCoor_i::lon, ned_vel_float_from_gps(), ekf2_t::rel_heading, ekf2_t::rel_heading_valid, 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 938 of file ins_ekf2.cpp.

References ekf2_t::accel_valid, ekf2_t::delta_gyro, ekf2, foo, 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_init()

void ins_ekf2_init

(

void

)

Definition at line 549 of file ins_ekf2.cpp.

References ABI_BROADCAST, accel_int_cb(), accel_int_ev, ekf2_t::accel_valid, agl_cb(), agl_ev, baro_cb(), baro_ev, DefaultPeriodic, ekf, ekf2, ekf_params, ekf2_t::flow_stamp, foo, 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_FUSION_TYPE, 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_RELPOS_ID, INS_EKF2_SONAR_MAX_RANGE, INS_EKF2_SONAR_MIN_RANGE, INS_EKF2_TEMPERATURE_ID, INS_EKF2_VDIST_SENSOR_TYPE, LlaCoor_i::lat, 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(), relpos_cb(), relpos_ev, reset_cb(), reset_ev, 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 798 of file ins_ekf2.cpp.

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

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

ins_ekf2_update()

void ins_ekf2_update

(

void

)

Definition at line 712 of file ins_ekf2.cpp.

References autopilot_in_flight(), ekf, ekf2, foo, ekf2_t::got_imu_data, LtpDef_i::hmsl, LlaCoor_i::lat, 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:

mag_cb()

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

static

Definition at line 968 of file ins_ekf2.cpp.

References ekf, ekf2, foo, ekf2_t::got_imu_data, and MAGS_FLOAT_OF_BFP.

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

References ekf, ekf2, ekf2_t::flow_stamp, and foo.

Referenced by ins_ekf2_init().

Here is the caller graph for this function:

relpos_cb()

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

static

Definition at line 1036 of file ins_ekf2.cpp.

References ekf2, foo, INS_EKF2_RELHEADING_ERR, ekf2_t::rel_heading, and ekf2_t::rel_heading_valid.

Referenced by ins_ekf2_init().

Here is the caller graph for this function:

reset_cb()

static void reset_cb ( uint8_t sender_id  UNUSED, uint8_t  flag  )

static

Definition at line 697 of file ins_ekf2.cpp.

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

Referenced by ins_ekf2_init().

Here is the call graph for this function:

Here is the caller graph for this function:

reset_ref()

static void reset_ref ( void  )

static

Definition at line 664 of file ins_ekf2.cpp.

References ekf, ekf2, foo, 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().

Referenced by reset_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

reset_vertical_ref()

static void reset_vertical_ref ( void  )

static

Definition at line 678 of file ins_ekf2.cpp.

References LlaCoor_i::alt, ekf, ekf2, foo, 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(), 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()

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

static

Definition at line 481 of file ins_ekf2.cpp.

References dev, ekf, and foo.

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

References dev, foo, 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 444 of file ins_ekf2.cpp.

References AHRS_COMP_ID_EKF2, dev, ekf, and foo.

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

References ACCEL_BFP_OF_REAL, dev, ekf, foo, 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 397 of file ins_ekf2.cpp.

References dev, ekf, and foo.

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

References dev, ekf, and foo.

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

References LlaCoor_i::alt, dev, LtpDef_i::ecef, ekf2, foo, 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 367 of file ins_ekf2.cpp.

References ACCEL_BFP_OF_REAL, dev, ekf, foo, 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 469 of file ins_ekf2.cpp.

References dev, ekf, and foo.

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

Referenced by ins_ekf2_init().

agl_ev

abi_event agl_ev

static

Definition at line 308 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

baro_ev

abi_event baro_ev

static

Definition at line 306 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

ekf

Ekf ekf

static

EKF class itself.

Definition at line 333 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(), mag_cb(), optical_flow_cb(), reset_ref(), reset_vertical_ref(), 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 335 of file ins_ekf2.cpp.

Referenced by accel_int_cb(), baro_cb(), gps_cb(), gyro_int_cb(), ins_ekf2_init(), ins_ekf2_publish_attitude(), ins_ekf2_remove_gps(), ins_ekf2_update(), mag_cb(), optical_flow_cb(), relpos_cb(), reset_ref(), reset_vertical_ref(), send_ins_ref(), and temperature_cb().

ekf_params

parameters* ekf_params

static

The EKF parameters.

Definition at line 334 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init(), and ins_ekf2_remove_gps().

gps_ev

abi_event gps_ev

static

Definition at line 312 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

gyro_int_ev

abi_event gyro_int_ev

static

Definition at line 309 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

mag_ev

abi_event mag_ev

static

Definition at line 311 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

optical_flow_ev

abi_event optical_flow_ev

static

Definition at line 314 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

relpos_ev

abi_event relpos_ev

static

Definition at line 313 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

reset_ev

abi_event reset_ev

static

Definition at line 315 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

sample_ev

struct extVisionSample sample_ev

static

External vision sample.

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

Referenced by ins_ekf2_init().