Paparazzi UAS  v5.15_devel-81-gd13dafb
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 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

static MUTEX_DECL ( we_ukf_mtx  )
static
static SEMAPHORE_DECL ( we_ukf_sem  ,
 
)
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_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_Set_Q_VA ( float  _v)
void wind_estimator_Set_Q_VA_SCALE ( float  _v)
void wind_estimator_Set_Q_WIND ( float  _v)
void wind_estimator_Set_R_AOA ( float  _v)
void wind_estimator_Set_R_GS ( float  _v)
void wind_estimator_Set_R_SSA ( float  _v)
void wind_estimator_Set_R_VA ( float  _v)

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().