wind_estimator.c File Reference

Original Simulink files available at https://github.com/enacuavlab/UKF_Wind_Estimation. More...

#include "modules/meteo/wind_estimator.h"
#include "modules/meteo/lib_ukf_wind_estimator/UKF_Wind_Estimator.h"
#include "mcu_periph/sys_time.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_geodetic_float.h"
#include "generated/modules.h"
#include "state.h"
#include <string.h>
#include <ch.h>
#include <hal.h>
#include "subsystems/datalink/downlink.h"
#include "subsystems/datalink/telemetry.h"
#include "subsystems/imu.h"

Include dependency graph for wind_estimator.c:

Go to the source code of this file.

Macros
#define	WE_UKF_KI   0.f
	Default parameters. More...
#define	WE_UKF_ALPHA   0.5f
#define	WE_UKF_BETA   2.f
#define	WE_UKF_P0   0.2f
#define	WE_UKF_R_GS   0.5f
#define	WE_UKF_R_VA   0.5f
#define	WE_UKF_R_AOA   0.002f
#define	WE_UKF_R_SSA   0.002f
#define	WE_UKF_Q_VA   0.1f
#define	WE_UKF_Q_VA_SCALE   0.0001f
#define	WE_UKF_Q_WIND   0.001f
#define	SEND_WIND_ESTIMATOR   TRUE
#define	LOG_WIND_ESTIMATOR   FALSE
#define	MAT_EL(_m, _l, _c, _n)   _m[_l + _c * _n]

Functions
static void	send_wind_estimator (struct transport_tx *trans, struct link_device *dev)
static	THD_WORKING_AREA (wa_thd_windestimation, 8 *1024)
static void	thd_windestimate (void *arg)
static	MUTEX_DECL (we_ukf_mtx)
static	SEMAPHORE_DECL (we_ukf_sem, 0)
void	init_calculator (void)
static void	wind_estimator_step (void)
void	wind_estimator_periodic (void)
void	wind_estimator_event (void)
void	wind_estimator_init (void)
void	wind_estimator_Set_R_GS (float _v)
void	wind_estimator_Set_R_VA (float _v)
void	wind_estimator_Set_R_AOA (float _v)
void	wind_estimator_Set_R_SSA (float _v)
void	wind_estimator_Set_Q_VA (float _v)
void	wind_estimator_Set_Q_WIND (float _v)
void	wind_estimator_Set_Q_VA_SCALE (float _v)

Variables
struct WindEstimator	wind_estimator
static uint32_t	time_step_before

Detailed Description

Original Simulink files available at https://github.com/enacuavlab/UKF_Wind_Estimation.

Definition in file wind_estimator.c.

Macro Definition Documentation

#define LOG_WIND_ESTIMATOR   FALSE

Definition at line 104 of file wind_estimator.c.

#define MAT_EL	(		_m,
			_l,
			_c,
			_n
	)		_m[_l + _c * _n]

Definition at line 122 of file wind_estimator.c.

Referenced by init_calculator(), wind_estimator_Set_Q_VA(), wind_estimator_Set_Q_VA_SCALE(), wind_estimator_Set_Q_WIND(), wind_estimator_Set_R_AOA(), wind_estimator_Set_R_GS(), wind_estimator_Set_R_SSA(), wind_estimator_Set_R_VA(), and wind_estimator_step().

#define SEND_WIND_ESTIMATOR   TRUE

Definition at line 81 of file wind_estimator.c.

#define WE_UKF_ALPHA   0.5f

Definition at line 50 of file wind_estimator.c.

Referenced by init_calculator().

#define WE_UKF_BETA   2.f

Definition at line 53 of file wind_estimator.c.

Referenced by init_calculator().

#define WE_UKF_KI   0.f

Default parameters.

Definition at line 47 of file wind_estimator.c.

Referenced by init_calculator().

#define WE_UKF_P0   0.2f

Definition at line 56 of file wind_estimator.c.

Referenced by init_calculator().

#define WE_UKF_Q_VA   0.1f

Definition at line 71 of file wind_estimator.c.

Referenced by init_calculator().

#define WE_UKF_Q_VA_SCALE   0.0001f

Definition at line 74 of file wind_estimator.c.

Referenced by init_calculator().

#define WE_UKF_Q_WIND   0.001f

Definition at line 77 of file wind_estimator.c.

Referenced by init_calculator().

#define WE_UKF_R_AOA   0.002f

Definition at line 65 of file wind_estimator.c.

Referenced by init_calculator().

#define WE_UKF_R_GS   0.5f

Definition at line 59 of file wind_estimator.c.

Referenced by init_calculator().

#define WE_UKF_R_SSA   0.002f

Definition at line 68 of file wind_estimator.c.

Referenced by init_calculator().

#define WE_UKF_R_VA   0.5f

Definition at line 62 of file wind_estimator.c.

Referenced by init_calculator().

Function Documentation

void init_calculator

(

void

)

Definition at line 145 of file wind_estimator.c.

References ukf_init_tag::alpha, ukf_init_tag::beta, WindEstimator::data_available, ukf_params_tag::dt, ukf_init_tag::ki, MAT_EL, ukf_init_tag::P0, ukf_params_tag::Q, WindEstimator::q_va, WindEstimator::q_va_scale, WindEstimator::q_wind, ukf_params_tag::R, WindEstimator::r_aoa, WindEstimator::r_gs, WindEstimator::r_ssa, WindEstimator::r_va, WindEstimator::reset, time_step_before, ukf_DW, ukf_init, ukf_params, ukf_U, UKF_Wind_Estimator_initialize(), ukf_Y, WE_UKF_ALPHA, WE_UKF_BETA, WE_UKF_KI, WE_UKF_P0, WE_UKF_Q_VA, WE_UKF_Q_VA_SCALE, WE_UKF_Q_WIND, WE_UKF_R_AOA, WE_UKF_R_GS, WE_UKF_R_SSA, WE_UKF_R_VA, wind_estimator, and ukf_init_tag::x0.

Referenced by wind_estimator_init(), and wind_estimator_periodic().

Here is the call graph for this function:

Here is the caller graph for this function:

static MUTEX_DECL ( we_ukf_mtx  )

static

static SEMAPHORE_DECL ( we_ukf_sem  , 0    )

static

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

static

Definition at line 86 of file wind_estimator.c.

References WindEstimator::airspeed, float_vect3_norm(), WindEstimator::wind, wind_estimator, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by wind_estimator_event(), and wind_estimator_init().

Here is the call graph for this function:

Here is the caller graph for this function:

static void thd_windestimate ( void *  arg )

static

Definition at line 444 of file wind_estimator.c.

References wind_estimator_step().

Referenced by wind_estimator_init().

Here is the call graph for this function:

Here is the caller graph for this function:

static THD_WORKING_AREA ( wa_thd_windestimation  , 8 *  1024  )

static

void wind_estimator_event

(

void

)

Definition at line 412 of file wind_estimator.c.

References WindEstimator::data_available, DefaultChannel, DefaultDevice, send_wind_estimator(), stateSetHorizontalWindspeed_f(), stateSetVerticalWindspeed_f(), WindEstimator::wind, wind_estimator, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Here is the call graph for this function:

void wind_estimator_init

(

void

)

Definition at line 465 of file wind_estimator.c.

References counter, DefaultPeriodic, init_calculator(), pprzLogFile, register_periodic_telemetry(), send_wind_estimator(), and thd_windestimate().

Here is the call graph for this function:

void wind_estimator_periodic

(

void

)

Definition at line 292 of file wind_estimator.c.

References ExtU::accel, Imu::accel, ACCEL_FLOAT_OF_BFP, ExtU::aoa, DefaultChannel, DefaultDevice, ukf_params_tag::dt, float_rmat_vmult(), get_sys_time_msec(), imu, init_calculator(), msg, FloatRates::p, FloatEulers::phi, ExtU::q, FloatRates::q, FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatRates::r, ExtU::rates, WindEstimator::reset, ExtU::sideslip, stateGetAccelNed_f(), stateGetAirspeed_f(), stateGetAngleOfAttack_f(), stateGetBodyRates_f(), stateGetNedToBodyEulers_f(), stateGetNedToBodyQuat_f(), stateGetNedToBodyRMat_f(), stateGetSideslip_f(), stateGetSpeedNed_f(), FloatEulers::theta, time_step_before, ukf_params, ukf_U, ExtU::va, VECT3_ADD, ExtU::vk, wind_estimator, wind_estimator_step(), FloatVect3::x, NedCoor_f::x, Int32Vect3::x, FloatVect3::y, NedCoor_f::y, Int32Vect3::y, FloatVect3::z, NedCoor_f::z, and Int32Vect3::z.

Here is the call graph for this function:

void wind_estimator_Set_Q_VA

(

float

_v

)

Definition at line 529 of file wind_estimator.c.

References MAT_EL, ukf_params_tag::Q, WindEstimator::q_va, ukf_params, and wind_estimator.

void wind_estimator_Set_Q_VA_SCALE

(

float

_v

)

Definition at line 545 of file wind_estimator.c.

References MAT_EL, ukf_params_tag::Q, WindEstimator::q_va_scale, ukf_params, and wind_estimator.

void wind_estimator_Set_Q_WIND

(

float

_v

)

Definition at line 537 of file wind_estimator.c.

References MAT_EL, ukf_params_tag::Q, WindEstimator::q_wind, ukf_params, and wind_estimator.

void wind_estimator_Set_R_AOA

(

float

_v

)

Definition at line 517 of file wind_estimator.c.

References MAT_EL, ukf_params_tag::R, WindEstimator::r_aoa, ukf_params, and wind_estimator.

void wind_estimator_Set_R_GS

(

float

_v

)

Definition at line 503 of file wind_estimator.c.

References MAT_EL, ukf_params_tag::R, WindEstimator::r_gs, ukf_params, and wind_estimator.

void wind_estimator_Set_R_SSA

(

float

_v

)

Definition at line 523 of file wind_estimator.c.

References MAT_EL, ukf_params_tag::R, WindEstimator::r_ssa, ukf_params, and wind_estimator.

void wind_estimator_Set_R_VA

(

float

_v

)

Definition at line 511 of file wind_estimator.c.

References MAT_EL, ukf_params_tag::R, WindEstimator::r_va, ukf_params, and wind_estimator.

static void wind_estimator_step ( void  )

inlinestatic

Definition at line 206 of file wind_estimator.c.

References ExtU::accel, WindEstimator::airspeed, ukf_init_tag::alpha, ExtU::aoa, ukf_init_tag::beta, WindEstimator::data_available, ukf_init_tag::ki, MAT_EL, pprzLogFile, ExtU::q, ukf_params_tag::Q, ukf_params_tag::R, ExtU::rates, ExtU::sideslip, time_step_before, ukf_init, ukf_params, ukf_U, UKF_Wind_Estimator_step(), ukf_Y, ExtU::va, ExtU::vk, WindEstimator::wind, wind_estimator, FloatVect3::x, ExtY::xout, FloatVect3::y, and FloatVect3::z.

Referenced by thd_windestimate(), and wind_estimator_periodic().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

uint32_t time_step_before

static

Definition at line 128 of file wind_estimator.c.

Referenced by init_calculator(), wind_estimator_periodic(), and wind_estimator_step().

struct WindEstimator wind_estimator

Definition at line 125 of file wind_estimator.c.

Referenced by init_calculator(), send_wind_estimator(), wind_estimator_event(), wind_estimator_periodic(), wind_estimator_Set_Q_VA(), wind_estimator_Set_Q_VA_SCALE(), wind_estimator_Set_Q_WIND(), wind_estimator_Set_R_AOA(), wind_estimator_Set_R_GS(), wind_estimator_Set_R_SSA(), wind_estimator_Set_R_VA(), and wind_estimator_step().