Paparazzi specific wrapper to run MEKF-Wind INS filter. More...

#include "modules/ins/ins_mekf_wind_wrapper.h"
#include "modules/ins/ins_mekf_wind.h"
#include "modules/air_data/air_data.h"
#include "modules/ahrs/ahrs_float_utils.h"
#include "modules/ahrs/ahrs_aligner.h"
#include "modules/ins/ins.h"
#include "modules/core/abi.h"
#include "math/pprz_isa.h"
#include "state.h"
#include "generated/airframe.h"
#include "generated/flight_plan.h"
#include "modules/datalink/telemetry.h"
#include "mcu_periph/sys_time.h"

Include dependency graph for ins_mekf_wind_wrapper.c:

Go to the source code of this file.

Macros
#define	INS_MEKF_WIND_FILTER_ID 3

#define	INS_MEKF_WIND_AIRSPEED_ID ABI_BROADCAST
	airspeed (Pitot tube) More...

#define	INS_MEKF_WIND_INCIDENCE_ID ABI_BROADCAST
	incidence angles More...

#define	INS_MEKF_WIND_BARO_ID ABI_BROADCAST
	baro More...

#define	INS_MEKF_WIND_IMU_ID ABI_BROADCAST
	IMU (gyro, accel) More...

#define	INS_MEKF_WIND_MAG_ID ABI_BROADCAST
	magnetometer More...

#define	INS_MEKF_WIND_GPS_ID GPS_MULTI_ID
	ABI binding for gps data. More...

Functions
static void	set_state_from_ins (void)
	update state interface More...

static void	send_euler (struct transport_tx trans, struct link_device dev)
	logging functions More...

static void	send_wind (struct transport_tx trans, struct link_device dev)

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

static void	send_inv_filter (struct transport_tx trans, struct link_device dev)

static void	reset_cb (uint8_t sender_id, uint8_t flag)

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

static void	pressure_diff_cb (uint8_t sender_id, float pdyn)

static void	airspeed_cb (uint8_t sender_id, float airspeed)

static void	incidence_cb (uint8_t sender_id, uint8_t flag, float aoa, float sideslip)

static void	gyro_cb (uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
	Call ins_mekf_wind_propagate on new gyro measurements. More...

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	aligner_cb (uint8_t sender_id, uint32_t stamp, struct Int32Rates lp_gyro, struct Int32Vect3 lp_accel, struct Int32Vect3 *lp_mag)

static void	geo_mag_cb (uint8_t sender_id, struct FloatVect3 *h)

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

void	ins_mekf_wind_wrapper_init (void)
	Init function. More...

static void	reset_ref (void)

static void	reset_vertical_ref (void)

Variables
struct InsMekfWind	ins_mekf_wind

static struct FloatVect3	ins_mekf_wind_accel
	last accel measurement More...

static uint32_t	last_imu_stamp = 0

static abi_event	pressure_diff_ev

static abi_event	airspeed_ev

static abi_event	incidence_ev

static abi_event	baro_ev

static abi_event	mag_ev

static abi_event	gyro_ev

static abi_event	accel_ev

static abi_event	aligner_ev

static abi_event	geo_mag_ev

static abi_event	gps_ev

static abi_event	reset_ev

Detailed Description

Paparazzi specific wrapper to run MEKF-Wind INS filter.

Definition in file ins_mekf_wind_wrapper.c.

Macro Definition Documentation

◆ INS_MEKF_WIND_AIRSPEED_ID

#define INS_MEKF_WIND_AIRSPEED_ID ABI_BROADCAST

airspeed (Pitot tube)

Definition at line 160 of file ins_mekf_wind_wrapper.c.

◆ INS_MEKF_WIND_BARO_ID

#define INS_MEKF_WIND_BARO_ID ABI_BROADCAST

baro

Definition at line 175 of file ins_mekf_wind_wrapper.c.

◆ INS_MEKF_WIND_FILTER_ID

#define INS_MEKF_WIND_FILTER_ID 3

Definition at line 47 of file ins_mekf_wind_wrapper.c.

◆ INS_MEKF_WIND_GPS_ID

#define INS_MEKF_WIND_GPS_ID GPS_MULTI_ID

ABI binding for gps data.

Used for GPS ABI messages.

Definition at line 196 of file ins_mekf_wind_wrapper.c.

◆ INS_MEKF_WIND_IMU_ID

#define INS_MEKF_WIND_IMU_ID ABI_BROADCAST

IMU (gyro, accel)

Definition at line 182 of file ins_mekf_wind_wrapper.c.

◆ INS_MEKF_WIND_INCIDENCE_ID

#define INS_MEKF_WIND_INCIDENCE_ID ABI_BROADCAST

incidence angles

Definition at line 166 of file ins_mekf_wind_wrapper.c.

◆ INS_MEKF_WIND_MAG_ID

#define INS_MEKF_WIND_MAG_ID ABI_BROADCAST

magnetometer

Definition at line 188 of file ins_mekf_wind_wrapper.c.

Function Documentation

◆ accel_cb()

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

static

Definition at line 379 of file ins_mekf_wind_wrapper.c.

References ACCELS_FLOAT_OF_BFP, and ins_mekf_wind_accel.

Referenced by ins_mekf_wind_wrapper_init().

Here is the caller graph for this function:

◆ airspeed_cb()

static void airspeed_cb	(	uint8_t	sender_id,
		float	airspeed
	)

static

Definition at line 270 of file ins_mekf_wind_wrapper.c.

References get_sys_time_float(), ins_mekf_wind, ins_mekf_wind_update_airspeed(), InsMekfWind::is_aligned, pprzLogFile, and tas_from_eas().

Referenced by ins_mekf_wind_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ aligner_cb()

static void aligner_cb	(	uint8_t	sender_id,
		uint32_t	stamp,
		struct Int32Rates *	lp_gyro,
		struct Int32Vect3 *	lp_accel,
		struct Int32Vect3 *	lp_mag
	)

static

Definition at line 408 of file ins_mekf_wind_wrapper.c.

References ACCELS_FLOAT_OF_BFP, ahrs_float_get_quat_from_accel_mag(), ins_mekf_wind, ins_mekf_wind_align(), InsMekfWind::is_aligned, MAGS_FLOAT_OF_BFP, RATES_FLOAT_OF_BFP, and set_state_from_ins().

Referenced by ins_mekf_wind_wrapper_init().

Here is the call graph for this function:

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 214 of file ins_mekf_wind_wrapper.c.

References alpha, baro_alt, InsMekfWind::baro_initialized, get_sys_time_float(), InsMekfWind::gps_initialized, ins_mekf_wind, ins_mekf_wind_update_baro(), InsMekfWind::is_aligned, pprz_isa_height_of_pressure(), PPRZ_ISA_SEA_LEVEL_PRESSURE, and pprzLogFile.

Referenced by ins_mekf_wind_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ geo_mag_cb()

static void geo_mag_cb	(	uint8_t	sender_id,
		struct FloatVect3 *	h
	)

static

Definition at line 434 of file ins_mekf_wind_wrapper.c.

References h(), and ins_mekf_wind_set_mag_h().

Referenced by ins_mekf_wind_wrapper_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 439 of file ins_mekf_wind_wrapper.c.

References UtmCoor_f::alt, UtmCoor_f::east, ecef_int_from_gps(), ecef_vel_float_from_gps(), GpsState::fix, get_sys_time_float(), GPS_FIX_3D, InsMekfWind::gps_initialized, ins_mekf_wind, ins_mekf_wind_set_pos_ned(), ins_mekf_wind_set_speed_ned(), ins_mekf_wind_update_pos_speed(), INT32_POS_OF_CM_DEN, INT32_POS_OF_CM_NUM, INT32_VECT3_SCALE_2, InsMekfWind::is_aligned, nav_utm_zone0, NED_FLOAT_OF_BFP, State::ned_initialized_f, ned_of_ecef_point_i(), ned_of_ecef_vect_f(), ned_vel_float_from_gps(), UtmCoor_f::north, pprzLogFile, state, stateGetHmslOrigin_f(), stateGetNedOrigin_f(), stateGetNedOrigin_i(), stateGetUtmOrigin_f(), utm_float_from_gps(), State::utm_initialized_f, NedCoor_f::x, NedCoor_f::y, and NedCoor_f::z.

Referenced by ins_mekf_wind_wrapper_init().

Here is the call graph for this function:

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

Call ins_mekf_wind_propagate on new gyro measurements.

Since acceleration measurement is also needed for propagation, use the last stored accel from #ins_mekfw_accel.

Definition at line 301 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ incidence_cb()

static void incidence_cb	(	uint8_t	sender_id,
		uint8_t	flag,
		float	aoa,
		float	sideslip
	)

static

Definition at line 283 of file ins_mekf_wind_wrapper.c.

References get_sys_time_float(), ins_mekf_wind, ins_mekf_wind_update_incidence(), InsMekfWind::is_aligned, and pprzLogFile.

Referenced by ins_mekf_wind_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ins_mekf_wind_wrapper_init()

void ins_mekf_wind_wrapper_init ( void )

Init function.

Definition at line 521 of file ins_mekf_wind_wrapper.c.

References ABI_BROADCAST, accel_cb(), accel_ev, ahrs_aligner_init(), airspeed_cb(), airspeed_ev, aligner_cb(), aligner_ev, UtmCoor_f::alt, LlaCoor_i::alt, baro_cb(), baro_ev, InsMekfWind::baro_initialized, counter, DefaultPeriodic, UtmCoor_f::east, ecef_of_lla_i(), geo_mag_cb(), geo_mag_ev, gps_cb(), gps_ev, InsMekfWind::gps_initialized, gyro_cb(), gyro_ev, LtpDef_i::hmsl, incidence_cb(), incidence_ev, ins_mekf_wind, INS_MEKF_WIND_AIRSPEED_ID, INS_MEKF_WIND_BARO_ID, INS_MEKF_WIND_GPS_ID, INS_MEKF_WIND_IMU_ID, INS_MEKF_WIND_INCIDENCE_ID, ins_mekf_wind_init(), INS_MEKF_WIND_MAG_ID, ins_mekf_wind_set_mag_h(), InsMekfWind::is_aligned, LlaCoor_i::lat, LlaCoor_i::lon, ltp_def, ltp_def_from_ecef_i(), mag_cb(), mag_ev, nav_utm_east0, nav_utm_north0, nav_utm_zone0, UtmCoor_f::north, pprzLogFile, pressure_diff_cb(), pressure_diff_ev, register_periodic_telemetry(), InsMekfWind::reset, reset_cb(), reset_ev, send_euler(), send_filter_status(), send_inv_filter(), send_wind(), stateSetLocalOrigin_i(), stateSetLocalUtmOrigin_f(), stateSetPositionUtm_f(), and UtmCoor_f::zone.

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 386 of file ins_mekf_wind_wrapper.c.

References get_sys_time_float(), InsMekfWind::gps_initialized, ins_mekf_wind, ins_mekf_wind_update_mag(), InsMekfWind::is_aligned, MAGS_FLOAT_OF_BFP, pprzLogFile, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ins_mekf_wind_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ pressure_diff_cb()

static void pressure_diff_cb	(	uint8_t	sender_id,
		float	pdyn
	)

static

Definition at line 256 of file ins_mekf_wind_wrapper.c.

References get_sys_time_float(), ins_mekf_wind, ins_mekf_wind_update_airspeed(), InsMekfWind::is_aligned, pprzLogFile, and tas_from_dynamic_pressure().

Referenced by ins_mekf_wind_wrapper_init().

Here is the call graph for this function:

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 642 of file ins_mekf_wind_wrapper.c.

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

Referenced by ins_mekf_wind_wrapper_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 604 of file ins_mekf_wind_wrapper.c.

References LlaCoor_i::alt, ecef_int_from_gps(), gps, LtpDef_i::hmsl, GpsState::hmsl, ins_mekf_wind_wrapper_Reset, LtpDef_i::lla, lla_int_from_gps(), ltp_def, ltp_def_from_ecef_i(), stateSetLocalOrigin_i(), stateSetLocalUtmOrigin_f(), and utm_float_from_gps().

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 623 of file ins_mekf_wind_wrapper.c.

References UtmCoor_f::alt, LlaCoor_i::alt, gps, LtpDef_i::hmsl, GpsState::hmsl, LlaCoor_i::lat, LtpDef_i::lla, GpsState::lla_pos, LlaCoor_i::lon, ltp_def, ltp_def_from_lla_i(), stateGetNedOrigin_i(), stateGetUtmOrigin_f(), stateSetLocalOrigin_i(), and stateSetLocalUtmOrigin_f().

Referenced by reset_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ send_euler()

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

static

logging functions

telemetry functions

Definition at line 78 of file ins_mekf_wind_wrapper.c.

References dev, float_eulers_of_quat(), INS_MEKF_WIND_FILTER_ID, ins_mekf_wind_get_quat(), FloatEulers::phi, FloatEulers::psi, and FloatEulers::theta.

Referenced by ins_mekf_wind_wrapper_init().

Here is the call graph for this function:

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 102 of file ins_mekf_wind_wrapper.c.

References InsMekfWind::baro_initialized, dev, get_sys_time_usec(), InsMekfWind::gps_initialized, ins_mekf_wind, INS_MEKF_WIND_FILTER_ID, InsMekfWind::is_aligned, last_imu_stamp, and val.

Referenced by ins_mekf_wind_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ send_inv_filter()

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

static

Definition at line 120 of file ins_mekf_wind_wrapper.c.

References dev, float_eulers_of_quat(), ins_mekf_wind_get_accel_bias(), ins_mekf_wind_get_airspeed_norm(), ins_mekf_wind_get_pos_ned(), ins_mekf_wind_get_quat(), ins_mekf_wind_get_rates_bias(), ins_mekf_wind_get_speed_ned(), FloatRates::p, FloatEulers::phi, FloatEulers::psi, FloatRates::q, FloatQuat::qi, FloatRates::r, FloatEulers::theta, FloatVect3::x, NedCoor_f::x, FloatVect3::y, NedCoor_f::y, FloatVect3::z, and NedCoor_f::z.

Referenced by ins_mekf_wind_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ send_wind()

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

static

Definition at line 91 of file ins_mekf_wind_wrapper.c.

References dev, ENU_OF_TO_NED, ins_mekf_wind_get_airspeed_norm(), ins_mekf_wind_get_wind_ned(), EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.

Referenced by ins_mekf_wind_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ set_state_from_ins()

static void set_state_from_ins ( void )

static

update state interface

Set current state (ltp to body orientation/rates and ned pos/speed/accel.

Definition at line 500 of file ins_mekf_wind_wrapper.c.

References ins_mekf_wind_get_accel_ned(), ins_mekf_wind_get_body_rates(), ins_mekf_wind_get_pos_ned(), ins_mekf_wind_get_quat(), ins_mekf_wind_get_speed_ned(), stateSetAccelNed_f(), stateSetBodyRates_f(), stateSetNedToBodyQuat_f(), stateSetPositionNed_f(), and stateSetSpeedNed_f().

Referenced by aligner_cb(), and gyro_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

◆ accel_ev

abi_event accel_ev

static

Definition at line 206 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

◆ airspeed_ev

abi_event airspeed_ev

static

Definition at line 201 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

◆ aligner_ev

abi_event aligner_ev

static

Definition at line 207 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

◆ baro_ev

abi_event baro_ev

static

Definition at line 203 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

◆ geo_mag_ev

abi_event geo_mag_ev

static

Definition at line 208 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

◆ gps_ev

abi_event gps_ev

static

Definition at line 209 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

◆ gyro_ev

abi_event gyro_ev

static

Definition at line 205 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

◆ incidence_ev

abi_event incidence_ev

static

Definition at line 202 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

◆ ins_mekf_wind

struct InsMekfWind ins_mekf_wind

Definition at line 1 of file ins_mekf_wind_wrapper.c.

Referenced by airspeed_cb(), aligner_cb(), baro_cb(), gps_cb(), gyro_cb(), incidence_cb(), ins_mekf_wind_wrapper_init(), mag_cb(), pressure_diff_cb(), and send_filter_status().

◆ ins_mekf_wind_accel

struct FloatVect3 ins_mekf_wind_accel

static

last accel measurement

Definition at line 1 of file ins_mekf_wind_wrapper.c.

Referenced by accel_cb(), and gyro_cb().

◆ last_imu_stamp

uint32_t last_imu_stamp = 0

static

Definition at line 54 of file ins_mekf_wind_wrapper.c.

Referenced by gyro_cb(), and send_filter_status().

◆ mag_ev

abi_event mag_ev

static

Definition at line 204 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

◆ pressure_diff_ev

abi_event pressure_diff_ev

static

Definition at line 200 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

◆ reset_ev

abi_event reset_ev

static

Definition at line 210 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

Macros

Functions

Variables

Detailed Description

Macro Definition Documentation

◆ INS_MEKF_WIND_AIRSPEED_ID

◆ INS_MEKF_WIND_BARO_ID

◆ INS_MEKF_WIND_FILTER_ID

◆ INS_MEKF_WIND_GPS_ID

◆ INS_MEKF_WIND_IMU_ID

◆ INS_MEKF_WIND_INCIDENCE_ID

◆ INS_MEKF_WIND_MAG_ID

Function Documentation

◆ accel_cb()

◆ airspeed_cb()

◆ aligner_cb()

◆ baro_cb()

◆ geo_mag_cb()

◆ gps_cb()

◆ gyro_cb()

◆ incidence_cb()

◆ ins_mekf_wind_wrapper_init()

◆ mag_cb()

◆ pressure_diff_cb()

◆ reset_cb()

◆ reset_ref()

◆ reset_vertical_ref()

◆ send_euler()

◆ send_filter_status()

◆ send_inv_filter()

◆ send_wind()

◆ set_state_from_ins()

Variable Documentation

◆ accel_ev

◆ airspeed_ev

◆ aligner_ev

◆ baro_ev

◆ geo_mag_ev

◆ gps_ev

◆ gyro_ev

◆ incidence_ev

◆ ins_mekf_wind

◆ ins_mekf_wind_accel

◆ last_imu_stamp

◆ mag_ev

◆ pressure_diff_ev

◆ reset_ev