#include "modules/meteo/ekf_aw_wrapper.h"
#include "modules/meteo/ekf_aw.h"
#include <stdio.h>
#include "state.h"
#include "filters/low_pass_filter.h"
#include "math/pprz_algebra.h"
#include "modules/core/abi.h"
#include "autopilot.h"
#include "modules/actuators/actuators.h"
#include "mcu_periph/sys_time.h"
#include "modules/datalink/telemetry.h"

Include dependency graph for ekf_aw_wrapper.c:

Macros
#define	EKF_AW_WRAPPER_ROTWING false

#define	EKF_AW_WRAPPER_RANDOM_INPUTS false

#define	EKF_AW_QUICK_CONVERGENCE false

#define	EKF_AW_QUICK_CONVERGENCE_TIME 10.0f

#define	EKF_AW_DEBUG false

#define	PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH 100

#define	PERIODIC_FREQUENCY_AIRSPEED_EKF 25

Functions
static void	send_airspeed_wind_ekf (struct transport_tx trans, struct link_device dev)

static void	debug_vect (struct transport_tx trans, struct link_device dev, char name, float data, int datasize)

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

static void	rpm_cb (uint8_t sender_id, struct act_feedback_t *rpm_message, uint8_t num_act)

void	ekf_aw_wrapper_init (void)

void	ekf_aw_wrapper_periodic (void)

void	ekf_aw_wrapper_fetch (void)

void	set_in_air_status (bool in_air)

Variables
abi_event	RPM_ev

float	time_of_rpm = 0.0f

struct ekfAw	ekf_aw

static struct ekfAwParameters *	ekf_params
	The EKF parameters. More...

float	tau_filter_high = 25.0f

float	tau_filter_low = 0.2f

bool	reset_filter = false

struct NedCoor_f	zero_speed

Butterworth2LowPass	filt_groundspeed [3]

Butterworth2LowPass	filt_acc [3]

Butterworth2LowPass	filt_acc_low [3]

Butterworth2LowPass	filt_rate [3]

Butterworth2LowPass	filt_euler [3]

Butterworth2LowPass	filt_hover_prop_rpm [EKF_AW_RPM_HOVER_NUM]

Butterworth2LowPass	filt_pusher_prop_rpm

Butterworth2LowPass	filt_skew

Butterworth2LowPass	filt_elevator_pprz

Butterworth2LowPass	filt_airspeed_pitot

Macro Definition Documentation

◆ EKF_AW_DEBUG

#define EKF_AW_DEBUG false

Definition at line 31 of file ekf_aw_wrapper.c.

◆ EKF_AW_QUICK_CONVERGENCE

#define EKF_AW_QUICK_CONVERGENCE false

Definition at line 25 of file ekf_aw_wrapper.c.

◆ EKF_AW_QUICK_CONVERGENCE_TIME

#define EKF_AW_QUICK_CONVERGENCE_TIME 10.0f

Definition at line 28 of file ekf_aw_wrapper.c.

◆ EKF_AW_WRAPPER_RANDOM_INPUTS

#define EKF_AW_WRAPPER_RANDOM_INPUTS false

Definition at line 22 of file ekf_aw_wrapper.c.

◆ EKF_AW_WRAPPER_ROTWING

#define EKF_AW_WRAPPER_ROTWING false

Definition at line 19 of file ekf_aw_wrapper.c.

◆ PERIODIC_FREQUENCY_AIRSPEED_EKF

#define PERIODIC_FREQUENCY_AIRSPEED_EKF 25

Definition at line 133 of file ekf_aw_wrapper.c.

◆ PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH

#define PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH 100

Definition at line 129 of file ekf_aw_wrapper.c.

Function Documentation

◆ debug_vect()

static void debug_vect	(	struct transport_tx *	trans,
		struct link_device *	dev,
		char *	name,
		float *	data,
		int	datasize
	)

static

Definition at line 63 of file ekf_aw_wrapper.c.

References dev.

Referenced by send_airspeed_wind_ekf_debug().

Here is the caller graph for this function:

◆ ekf_aw_wrapper_fetch()

void ekf_aw_wrapper_fetch ( void )

Definition at line 338 of file ekf_aw_wrapper.c.

References ACCEL_BFP_OF_REAL, ACCELS_FLOAT_OF_BFP, body_accel_f, ekf_aw, EKF_AW_RPM_HOVER_NUM, filt_acc, filt_acc_low, filt_airspeed_pitot, filt_elevator_pprz, filt_euler, filt_groundspeed, filt_hover_prop_rpm, filt_pusher_prop_rpm, filt_rate, filt_skew, int32_rmat_vmult(), ekfAw::last_RPM_hover, ekfAw::last_RPM_pusher, p, stateGetAccelBody_i(), stateGetAccelNed_i(), stateGetAirspeed_f(), stateGetBodyRates_f(), stateGetNedToBodyEulers_f(), stateGetNedToBodyRMat_i(), stateGetSpeedNed_f(), update_butterworth_2_low_pass(), VECT3_COPY, FloatVect3::x, NedCoor_f::x, FloatVect3::y, NedCoor_f::y, FloatVect3::z, Int32Vect3::z, and NedCoor_f::z.

Here is the call graph for this function:

◆ ekf_aw_wrapper_init()

void ekf_aw_wrapper_init ( void )

Definition at line 136 of file ekf_aw_wrapper.c.

References ACT_FEEDBACK_RPM_SENSOR_ID, ekfHealth::crashes_n, DefaultPeriodic, ekf_aw, ekf_aw_get_param_handle(), ekf_aw_init(), EKF_AW_RPM_HOVER_NUM, ekf_params, ekfAw::elevator_angle, ekfAw::elevator_force, filt_acc, filt_acc_low, filt_airspeed_pitot, filt_elevator_pprz, filt_euler, filt_groundspeed, filt_hover_prop_rpm, filt_pusher_prop_rpm, filt_rate, filt_skew, ekfAw::fuselage_force, ekfAw::health, ekfHealth::healthy, ekfAw::hover_force, ekfAw::in_air, init_butterworth_2_low_pass(), ekfAw::innov_acc_filt, ekfAw::innov_V_gnd, ekfAw::innov_V_pitot, ekfAw::internal_clock, ekfAw::last_RPM_hover, ekfAw::last_RPM_pusher, ekfAw::offset, ekfAw::override_quick_convergence, ekfAw::override_start, PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH, ekfAw::pusher_force, register_periodic_telemetry(), rpm_cb(), RPM_ev, ekfAw::RPM_hover, ekfAw::RPM_pusher, send_airspeed_wind_ekf(), send_airspeed_wind_ekf_debug(), ekfAw::skew, tau_filter_high, tau_filter_low, ekfAw::time_last_on_gnd, ekfAw::V_body, ekfAw::wind, ekfAw::wing_force, FloatVect3::x, NedCoor_f::x, FloatVect3::y, NedCoor_f::y, FloatVect3::z, and NedCoor_f::z.

Here is the call graph for this function:

◆ ekf_aw_wrapper_periodic()

void ekf_aw_wrapper_periodic ( void )

Definition at line 215 of file ekf_aw_wrapper.c.

References ekfAw::acc, ekfAw::acc_filt, autopilot_in_flight(), ekf_aw, ekf_aw_get_elevator_force(), ekf_aw_get_fuselage_force(), ekf_aw_get_health(), ekf_aw_get_hover_force(), ekf_aw_get_innov_accel_filt(), ekf_aw_get_innov_V_gnd(), ekf_aw_get_innov_V_pitot(), ekf_aw_get_meas_cov(), ekf_aw_get_offset(), ekf_aw_get_process_cov(), ekf_aw_get_pusher_force(), ekf_aw_get_speed_body(), ekf_aw_get_state_cov(), ekf_aw_get_wind_ned(), ekf_aw_get_wing_force(), ekf_aw_propagate(), EKF_AW_QUICK_CONVERGENCE, EKF_AW_QUICK_CONVERGENCE_TIME, EKF_AW_RPM_HOVER_NUM, ekf_aw_set_speed_body(), EKF_AW_WRAPPER_RANDOM_INPUTS, ekf_params, ekfAw::elevator_angle, ekfAw::elevator_force, ekfAw::euler, filt_acc, filt_acc_low, filt_airspeed_pitot, filt_elevator_pprz, filt_euler, filt_groundspeed, filt_hover_prop_rpm, filt_pusher_prop_rpm, filt_rate, filt_skew, ekfAw::fuselage_force, ekfAw::gyro, ekfAw::health, ekfAw::hover_force, ekfAw::in_air, ekfAw::innov_acc_filt, ekfAw::innov_V_gnd, ekfAw::innov_V_pitot, ekfAw::internal_clock, ekfAw::meas_cov, SecondOrderLowPass::o, ekfAw::offset, ekfAw::override_quick_convergence, ekfAw::override_start, FloatRates::p, PERIODIC_FREQUENCY_AIRSPEED_EKF, PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH, FloatEulers::phi, ekfAw::process_cov, FloatEulers::psi, ekfAw::pusher_force, FloatRates::q, ekfAwParameters::quick_convergence, FloatRates::r, ekfAw::RPM_hover, ekfAw::RPM_pusher, set_in_air_status(), ekfAw::skew, ekfAw::state_cov, stateGetNedToBodyEulers_f(), stateGetPositionNed_f(), FloatEulers::theta, ekfAw::time_last_on_gnd, ekfAw::V_body, ekfAw::V_pitot, ekfAw::Vg_NED, ekfAw::wind, ekfAw::wing_force, FloatVect3::x, FloatVect3::y, FloatVect3::z, NedCoor_f::z, and zero_speed.

Here is the call graph for this function:

◆ rpm_cb()

static void rpm_cb	(	uint8_t	sender_id,
		struct act_feedback_t *	rpm_message,
		uint8_t	num_act
	)

static

Definition at line 413 of file ekf_aw_wrapper.c.

References ekf_aw, EKF_AW_RPM_HOVER_NUM, get_sys_time_float(), idx, ekfAw::last_RPM_hover, ekfAw::last_RPM_pusher, act_feedback_t::rpm, and time_of_rpm.

Referenced by ekf_aw_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ send_airspeed_wind_ekf()

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

static

Definition at line 45 of file ekf_aw_wrapper.c.

References ekfAw::acc, ekfHealth::crashes_n, dev, ekf_aw, ekfAw::health, ekfHealth::healthy, ekfAw::innov_acc_filt, ekfAw::innov_V_gnd, ekfAw::innov_V_pitot, ekfAw::offset, ekfAw::V_body, ekfAw::wind, FloatVect3::x, NedCoor_f::x, FloatVect3::y, NedCoor_f::y, FloatVect3::z, and NedCoor_f::z.

Referenced by ekf_aw_wrapper_init().

Here is the caller graph for this function:

◆ send_airspeed_wind_ekf_debug()

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

static

Definition at line 72 of file ekf_aw_wrapper.c.

References debug_vect(), dev, ekf_aw, ekfAw::elevator_angle, ekfAw::elevator_force, ekfAw::fuselage_force, ekfAw::hover_force, ekfAw::meas_cov, ekfAw::process_cov, ekfAw::pusher_force, ekfAw::RPM_hover, ekfAw::RPM_pusher, ekfAw::skew, ekfAw::state_cov, and ekfAw::wing_force.

Referenced by ekf_aw_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ set_in_air_status()

void set_in_air_status ( bool in_air )

Definition at line 445 of file ekf_aw_wrapper.c.

References ekf_aw, ekfAw::in_air, ekfAw::internal_clock, ekfAw::time_last_in_air, and ekfAw::time_last_on_gnd.

Referenced by ekf_aw_wrapper_periodic().

Here is the caller graph for this function:

Variable Documentation

◆ ekf_aw

struct ekfAw ekf_aw

Definition at line 97 of file ekf_aw_wrapper.c.

Referenced by ekf_aw_wrapper_fetch(), ekf_aw_wrapper_init(), ekf_aw_wrapper_periodic(), rpm_cb(), send_airspeed_wind_ekf(), send_airspeed_wind_ekf_debug(), and set_in_air_status().

◆ ekf_params

struct ekfAwParameters* ekf_params

static

The EKF parameters.

Definition at line 101 of file ekf_aw_wrapper.c.

Referenced by ekf_aw_wrapper_init(), and ekf_aw_wrapper_periodic().

◆ filt_acc

Butterworth2LowPass filt_acc[3]

Definition at line 118 of file ekf_aw_wrapper.c.

Referenced by ekf_aw_wrapper_fetch(), ekf_aw_wrapper_init(), and ekf_aw_wrapper_periodic().

◆ filt_acc_low

Butterworth2LowPass filt_acc_low[3]

Definition at line 119 of file ekf_aw_wrapper.c.

Referenced by ekf_aw_wrapper_fetch(), ekf_aw_wrapper_init(), and ekf_aw_wrapper_periodic().

◆ filt_airspeed_pitot

Butterworth2LowPass filt_airspeed_pitot

Definition at line 126 of file ekf_aw_wrapper.c.

Referenced by ekf_aw_wrapper_fetch(), ekf_aw_wrapper_init(), and ekf_aw_wrapper_periodic().

◆ filt_elevator_pprz

Butterworth2LowPass filt_elevator_pprz

Definition at line 125 of file ekf_aw_wrapper.c.

Referenced by ekf_aw_wrapper_fetch(), ekf_aw_wrapper_init(), and ekf_aw_wrapper_periodic().

◆ filt_euler

Butterworth2LowPass filt_euler[3]

Definition at line 121 of file ekf_aw_wrapper.c.

Referenced by ekf_aw_wrapper_fetch(), ekf_aw_wrapper_init(), and ekf_aw_wrapper_periodic().

◆ filt_groundspeed

Butterworth2LowPass filt_groundspeed[3]

Definition at line 117 of file ekf_aw_wrapper.c.

Referenced by ekf_aw_wrapper_fetch(), ekf_aw_wrapper_init(), and ekf_aw_wrapper_periodic().

◆ filt_hover_prop_rpm

Butterworth2LowPass filt_hover_prop_rpm[EKF_AW_RPM_HOVER_NUM]

Definition at line 122 of file ekf_aw_wrapper.c.

Referenced by ekf_aw_wrapper_fetch(), ekf_aw_wrapper_init(), and ekf_aw_wrapper_periodic().

◆ filt_pusher_prop_rpm

Butterworth2LowPass filt_pusher_prop_rpm

Definition at line 123 of file ekf_aw_wrapper.c.

Referenced by ekf_aw_wrapper_fetch(), ekf_aw_wrapper_init(), and ekf_aw_wrapper_periodic().

◆ filt_rate

Butterworth2LowPass filt_rate[3]

Definition at line 120 of file ekf_aw_wrapper.c.

Referenced by ekf_aw_wrapper_fetch(), ekf_aw_wrapper_init(), and ekf_aw_wrapper_periodic().

◆ filt_skew

Butterworth2LowPass filt_skew

Definition at line 124 of file ekf_aw_wrapper.c.

Referenced by ekf_aw_wrapper_fetch(), ekf_aw_wrapper_init(), and ekf_aw_wrapper_periodic().

◆ reset_filter

bool reset_filter = false

Definition at line 108 of file ekf_aw_wrapper.c.

◆ RPM_ev

abi_event RPM_ev

Definition at line 95 of file ekf_aw_wrapper.c.

Referenced by ekf_aw_wrapper_init().

◆ tau_filter_high

float tau_filter_high = 25.0f

Definition at line 104 of file ekf_aw_wrapper.c.

Referenced by ekf_aw_wrapper_init().

◆ tau_filter_low

float tau_filter_low = 0.2f

Definition at line 105 of file ekf_aw_wrapper.c.

Referenced by ekf_aw_wrapper_init().

◆ time_of_rpm

float time_of_rpm = 0.0f

Definition at line 96 of file ekf_aw_wrapper.c.

Referenced by rpm_cb().

◆ zero_speed

struct NedCoor_f zero_speed

Initial value:

= {
  .x = 0.0f,
  .y = 0.0f,
  .z = 0.0f
}

Definition at line 108 of file ekf_aw_wrapper.c.

Referenced by ekf_aw_wrapper_periodic().

Macros

Functions

Variables

Macro Definition Documentation

◆ EKF_AW_DEBUG

◆ EKF_AW_QUICK_CONVERGENCE

◆ EKF_AW_QUICK_CONVERGENCE_TIME

◆ EKF_AW_WRAPPER_RANDOM_INPUTS

◆ EKF_AW_WRAPPER_ROTWING

◆ PERIODIC_FREQUENCY_AIRSPEED_EKF

◆ PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH

Function Documentation

◆ debug_vect()

◆ ekf_aw_wrapper_fetch()

◆ ekf_aw_wrapper_init()

◆ ekf_aw_wrapper_periodic()

◆ rpm_cb()

◆ send_airspeed_wind_ekf()

◆ send_airspeed_wind_ekf_debug()

◆ set_in_air_status()

Variable Documentation

◆ ekf_aw

◆ ekf_params

◆ filt_acc

◆ filt_acc_low

◆ filt_airspeed_pitot

◆ filt_elevator_pprz

◆ filt_euler

◆ filt_groundspeed

◆ filt_hover_prop_rpm

◆ filt_pusher_prop_rpm

◆ filt_rate

◆ filt_skew

◆ reset_filter

◆ RPM_ev

◆ tau_filter_high

◆ tau_filter_low

◆ time_of_rpm

◆ zero_speed